/*
 * DspRisTgt.cpp
 *
 */

#include "dsp_ris_internals_.h"

#include "idd_avionic_types.h"

#include "gc_math.h"

#include <math.h>
#include <float.h>
#include <string.h>

static dsp_ris_tgt_db_t autoMonitorable_ristgt;

dsp_ris_tgt_db_t& dsp_ris_tgt_db=autoMonitorable_ristgt;

static void polar2XY(float& x, float &y, float az, float rng)
{
	x=rng*cosf(az);
	y=rng*sinf(az);
}

static void XY2polar(float& az, float& rng, float x, float y)
{
	az=atan2(y, x);
	rng=sqrtf(x*x+y*y);
}


const iddtypes::inu_data_t& DspRisTgtInuData()
{
	return dsp_ris_tgt_db.inu;
}


void DspRisTargetSet(int id, unsigned int* flags, float* range, float* az, float* speed, float* heading, float* altitude, float* turn_rate, float* turn_duration)
{
	if (id==-1)
	{
		for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
		{
			dsp_ris_tgt_db.t[i].enabled=0;
		}
		return;
	}

	if ((id<0) || (id>=DSP_RIS_MAX_TGT))
		return;

	dsp_ris_tgt_t& t=dsp_ris_tgt_db.t[id];

	DspRigTgtMutexAcquire(true);

	if (range)
		t.ground_range=*range;
	if (az)
		t.az=*az;
	if (speed)
		t.speed=*speed;
	if (heading)
		t.heading=*heading;
	if (altitude)
		t.altitude=*altitude;

	if (turn_rate)
		t.turn=*turn_rate;
	if (turn_duration)
		t.turn_duration=*turn_duration;

	if (flags)
		t.enabled=*flags ? 1 : 0;

	++t.ext_updated;

	DspRigTgtMutexRelease();
}

void DspRisTgtUpdate(const iddtypes::inu_data_t& inu, float delta_time)
{
	//s.tracksUpdateExt(delta_time, cmd.nav.inertial_altitude.data);
	float ac_altitude=inu.inertial_altitude.data;

	if (delta_time<0.005)
		delta_time=0.005;

	DspRigTgtMutexAcquire(true);

	float rv=sqrtf(inu.own_ship_velocity.vector.x*inu.own_ship_velocity.vector.x+inu.own_ship_velocity.vector.y*inu.own_ship_velocity.vector.y);
	dsp_ris_tgt_db.vxy=rv;

	dsp_ris_tgt_db.rbm_range-=(rv*delta_time);

	for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
	{
		dsp_ris_tgt_t& t=dsp_ris_tgt_db.t[i];
		if (t.enabled==0)
			continue;
		if (t.enabled<0)
		{
			++t.enabled;
		}

		//t.range_delta=t.rng_meters;

		float az=t.az;
		float rng=t.ground_range; //t.rng_meters;

		float vx=0;
		float vy=0;
		float speed=t.speed; //*delta_time;

		if (t.turn_duration>0)
		{
			t.heading+=t.turn*delta_time;
			t.turn_duration-=delta_time;
		}

		t.heading=gc_math::angle_bound_pi(t.heading);

		polar2XY(vx, vy, t.heading, speed);

		float x=0;
		float y=0;
		polar2XY(x, y, az, rng);

		t.vx=vx; // /delta_time;
		t.vy=vy; // /delta_time;

		x+=(t.vx-inu.own_ship_velocity.vector.x)*delta_time; //x+=(vx-(cmd.nav.own_ship_velocity.vector.x*delta_time));
		y+=(t.vy-inu.own_ship_velocity.vector.y)*delta_time; //y+=(vy-(cmd.nav.own_ship_velocity.vector.y*delta_time));

		XY2polar(az, rng, x, y);

		t.rad_vel=0;

		if (rng>0)
		{
			float xn=x/rng;
			float yn=y/rng;

			float rv=(t.vx*xn)+(t.vy*yn);

			t.rad_vel=rv;
		}



		t.ground_range=rng;
		float delta_altitude=t.altitude-ac_altitude;
		rng=sqrtf(delta_altitude*delta_altitude+rng*rng);  //slant

		if (fabsf(rng)>FLT_EPSILON)
			t.el=asinf(delta_altitude/rng);
		else
			t.el=0;

		t.az=az;
		t.range_delta=(rng-t.slant_range);
		t.range_rate=t.range_delta/delta_time; //t.range_delta;
		t.slant_range=rng;
		//t.range=t.rng_meters;
		t.x=x;
		t.y=y;

		//if (t.range>risTrackOverrides.max_rng)
		//	t.enabled=0;
	}

	dsp_ris_tgt_db.inu=inu;
	DspRigTgtMutexRelease();
}

#include "DspLrpHeader.h"
#include "idd_dsp_results_types.h"


static float rs2m(iddtypes::range_scale_t rs)
{
	float rmax=10*1852;
	int ri=int(rs);
	if (ri<0)
		return rmax;
	if (ri>(int(iddtypes::_80NM)))
		return 80*1852;

	static const float cvt[]={1852*10, 1852*10, 1852*20, 1852*40, 1852*80};
	return cvt[ri];
}

float DspRisRanceScale2Meters(iddtypes::range_scale_t rs)
{
	return rs2m(rs);
}

#define SPEED_OF_LIGHT_MS (2.997925e+08F*10e-6f)
static const float STT_MR_DX=0.1639f;;
static const float STT_MR_DY=-0.1212f;

static void pos2mp(float& monopulse_ratio_az, float& monopulse_ratio_el, double tgt_body_az, double tgt_body_el, float au, float av, float processed_tx_chan_mhz)
{
    const float lambda=SPEED_OF_LIGHT_MS/processed_tx_chan_mhz;

    float t_v=sinf(-tgt_body_el); //float tgt_ant_el = -asinf(tgt_v);
    float t_u=sinf(-(tgt_body_az)*cosf(tgt_body_el)); //float tgt_ant_az = -asinf(tgt_u)/cosf(tgt_ant_el);

    float delta_v=t_v-av;
    float delta_u=t_u-au;

    float mp_angle=gc_math::angle_bound_pi((delta_u/lambda)*(GC_K_PI*STT_MR_DX));
    float mp_daz=tanf(mp_angle); //(delta_u/lambda)*(PI*STT_MR_DX));

    mp_angle=gc_math::angle_bound_pi((delta_v/lambda)*(GC_K_PI*STT_MR_DY));
    float mp_del=tanf(mp_angle);

    monopulse_ratio_az=mp_daz;
    monopulse_ratio_el=mp_del;
}

void DspRisTargetsLookup(const DspLrpHeaderOutput::Header* hdr, idddsp::dsp_detections_t& dspdet, idddsp::dsp_stt_data_t* stt, bool scan_mode)
{
	if (dsp_ris_tgt_db.no_override==0)
	{
		dspdet.detections.num_of_detected_tgts=0;
	}

	if ((!hdr->ge.mode.radiate) && (dsp_ris_tgt_db.ignore_silence==0))
		return;

	bool as_mode=false;

	bool close_plot=
			(!hdr->ge.general_settings.antenna.event.bar_in_progress) |
			(!hdr->ge.general_settings.antenna.event.frame_in_progress) |
			(hdr->ge.function_settings.pe_settings.close_plot_flag);

	bool stt_mode=false;

	switch(hdr->ge.mode.operation_mode)
	{
	case iddtypes::rws_master_mode:
	case iddtypes::tws_master_mode:
	case iddtypes::sam_master_mode:
		break;

	case iddtypes::acm_master_mode:
		//WARNING: submode required
		break;

	case iddtypes::gmti_master_mode:
	case iddtypes::sea_high_master_mode:
		as_mode=true;
		break;

	case iddtypes::gm_master_mode:
	case iddtypes::dbs_master_mode:
		as_mode=true;
		break;

	case iddtypes::stt_master_mode:
	case iddtypes::sstt_master_mode:
		close_plot=true;
		stt_mode=true;
		break;

	case iddtypes::vs_master_mode:
		break;

	default:
		return;
	}

	if (stt_mode)
	{
		if (!stt)
			return;

		stt->eccm_data_list.num_meas=0;
		stt->measures_list.num_meas=0;
	}

	float max_range=rs2m(hdr->ge.mode.range_scale);

	float nav_az=hdr->ge.general_settings.antenna.position.navaz_rad;
	float nav_el=hdr->ge.general_settings.antenna.position.navel_rad;

	float az_bw=GC_DEG_TO_RAD(2.5);
	float el_bw=GC_DEG_TO_RAD(2.5);

	unsigned int nfft_total=dspdet.detections.processed_nfft_total;
	unsigned int tt=dspdet.detections.processed_pck_timetag;
	float chmhz=hdr->ge.function_settings.pe_settings.tx_chan_mhz;

	unsigned int n_det=dspdet.detections.num_of_detected_tgts;

	float ac_altitude=hdr->ge.general_settings.navigation.geo_pos.altitude_m;

	auto bar_n=hdr->ge.general_settings.antenna.event.current_bar_number;

	if (dsp_ris_tgt_db.space_invaders)
	{
		static unsigned int sp_decimator;
		static unsigned int sp_id;
		if (sp_decimator==0)
		{
			float range=max_range/(MAX_NO_OF_TGT_DETECTIONS+2);

			for(int i=0; i<MAX_NO_OF_TGT_DETECTIONS; ++i)
			{
		        auto& d=dspdet.detections.target_detections[i];

				d.associated_altitude=ac_altitude;
				d.associated_azimuth=nav_az;
				d.associated_elevation=nav_el;
				d.detection_valid=i+1;
				d.id=++sp_id;
				d.associated_bar_number=bar_n;
				d.detected_tgt.detected_tgt_range=range*(i+1);

                d.detected_tgt.monop_u=0;
                d.detected_tgt.monop_v=0;
                d.detected_tgt.monop_valid=3;
                d.detected_tgt.detection_timetag=tt;
                d.detected_tgt.amplitude_det=50;
                d.detected_tgt.detected_tgt_range_validity=iddtypes::valid;
                d.detected_tgt.detected_tgt_rrate=0;
                d.detected_tgt.detected_tgt_rrate_validity=iddtypes::valid;
                d.detected_tgt.sdr_det=5;
                d.detected_tgt.nfft_total=nfft_total;
                d.detected_tgt.tx_chan_mhz=chmhz;
			}
			dspdet.detections.num_of_detected_tgts=MAX_NO_OF_TGT_DETECTIONS;
		}
		++sp_decimator;
		if (sp_decimator>=6)
			sp_decimator=0;
		return;
	}

	if (!DspRigTgtMutexAcquire(false))
	{
		++dsp_ris_tgt_db.contentions;
		return;
	}

	for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
	{
		if (n_det>=MAX_NO_OF_TGT_DETECTIONS)
			break;

		dsp_ris_tgt_t& t=dsp_ris_tgt_db.t[i];

		if (!t.enabled)
		{
			t.tot=0;
			t.already_seen=0;
			continue;
		}

		if ((!scan_mode) || close_plot)
			t.already_seen=0;

		if (t.already_seen)
		{
			t.tot=0;
			continue;
		}

		if (t.slant_range<0)
			continue;
		if (t.slant_range>max_range)
			continue;
		if (t.altitude<0)
			continue;


		bool visible=false;

		float daz=fabsf(t.az-nav_az);
		if (daz<az_bw)
		{

			float del=fabsf(t.el-nav_el);

			if ((del<el_bw) || ((as_mode && (t.altitude<ac_altitude))))
			{
				visible=true;
			}
		}

		if (visible)
		{
	        if (stt_mode)
	        {
	        	const float rng_win=400;
	        	float r_start=hdr->ge.function_settings.pe_settings.unamb_tgt_range-rng_win;
	        	float r_stop=r_start+rng_win*2;

	        	if ((t.slant_range<r_start) || (t.slant_range>r_stop))
	        		continue;

                //float ac_vel=hdr->ge.general_settings.navigation.speed.vx;//*dspEmu.ac_vel_factor;
                //ac_vel=ac_vel*cosf(t.az+hdr->ge.general_settings.navigation.attitude.platfrom_azimuth_rad);
                //float vel_tgt=t.speed+ac_vel;

	        	idddsp::dsp_stt_data_t& tgt=*stt;
	            tgt.measures_list.num_meas=1;
	            tgt.measures_list.main_measure[0].rng_tgt=t.slant_range;
	            tgt.measures_list.main_measure[0].mr_daz=0;
	            tgt.measures_list.main_measure[0].mr_del=0;
	            tgt.measures_list.main_measure[0].vel_tgt=t.rad_vel; //vel_tgt; //t.speed; //range_rate;
	            if (t.sdr)
	            	tgt.measures_list.main_measure[0].snr_tgt=t.sdr;
	            else
	            	tgt.measures_list.main_measure[0].snr_tgt=100;

	            tgt.eccm_data_list.num_meas=1;
	            if (t.amplitude)
	            	tgt.eccm_data_list.eccm_data[0].amp_tgt=t.amplitude;
	            else
	            	tgt.eccm_data_list.eccm_data[0].amp_tgt=6;

#if 0
	            {//Monopulse simulation
	                //const float SPEED_OF_LIGHT=(2.997925e+08F*10e-6f);
	                const float PI=GC_K_PI;

	                const float lambda=SPEED_OF_LIGHT_MS/chmhz;
	                const float tgt_az=t.az;
	                const float tgt_el=t.el;
	                const float cmd_az=nav_az;
	                const float cmd_el=nav_el;

	                float ref_u=sinf(tgt_az)*cosf(tgt_el);
	                float tgt_u=sinf(cmd_az)*cosf(cmd_el);
	                float delta_u=ref_u-tgt_u;
	                float mr_az=tanf(delta_u)*2*PI*STT_MR_DX/lambda;
	                float ref_v=sinf(tgt_el);
	                float tgt_v=sinf(cmd_el);
	                float delta_v=ref_v-tgt_v;
	                float mr_el=tanf(delta_v)*2*PI*STT_MR_DY/lambda;
	                tgt.measures_list.main_measure[0].mr_daz=mr_az;
	                tgt.measures_list.main_measure[0].mr_del=mr_el;
	            }
#else
	            {
					float mr_az=0;
					float mr_el=0;
					float au=hdr->ge.general_settings.antenna.position.urx_rad;
					float av=hdr->ge.general_settings.antenna.position.vrx_rad;
					float t_az=-(t.az+hdr->ge.general_settings.navigation.attitude.platfrom_azimuth_rad);
					float t_el=-t.el;
					pos2mp(mr_az, mr_el, t_az, t_el, au, av, chmhz);
					tgt.measures_list.main_measure[0].mr_daz=mr_az;
					tgt.measures_list.main_measure[0].mr_del=mr_el;
	            }
#endif

	        	break;
	        }

			++t.tot;
			if ((t.tot<8) & (!as_mode))
				continue;
		}

		if (t.tot<1)
		{
			continue;
		}


        float sinel=sinf(t.el);

        auto& d=dspdet.detections.target_detections[n_det];

		d.associated_azimuth=t.az;
		d.associated_elevation=t.el;

		d.associated_altitude=//t.altitude;
                hdr->ge.general_settings.navigation.geo_pos.altitude_m+sinel*t.slant_range;

		d.associated_bar_number=bar_n; //hdr->ge.general_settings.antenna.event.current_bar_number;

		d.detected_tgt.amplitude_det=t.amplitude>0 ? t.amplitude : 10;
		d.detected_tgt.sdr_det=t.sdr>0 ? t.sdr : 5;

		d.detected_tgt.detected_tgt_range=t.slant_range;
		d.detected_tgt.detected_tgt_range_validity=iddtypes::valid;

		d.detected_tgt.detected_tgt_rrate=t.range_rate;
		d.detected_tgt.detected_tgt_rrate_validity=iddtypes::valid;

		d.detected_tgt.detected_tgt_radial_vel=t.rad_vel;
		d.detected_tgt.detected_tgt_radial_vel_validity=iddtypes::not_valid;

		d.detected_tgt.monop_valid=3;
		d.detected_tgt.monop_u=0;
		d.detected_tgt.monop_v=0;

        d.detected_tgt.nfft_total=nfft_total;
        d.detected_tgt.tx_chan_mhz=chmhz;

        d.detected_tgt.detection_timetag=tt;

        d.id=i;
        d.track_association=i;
        d.track_association_value=0;

		d.detection_valid=3;

		++t.hits;

		if (t.max_tot<t.tot)
		{
			t.max_tot=t.tot;
		}
		t.tot=0;
		t.already_seen=1;

		++n_det;
	}
	DspRigTgtMutexRelease();

	dspdet.detections.num_of_detected_tgts=n_det;
}


#define SAR_IMAGE_DX	2048
struct ris_sar_image_t
{
	enum { DIM=SAR_IMAGE_DX}; //was 484
	uint16_t data[DIM][DIM];
};

bool DspRisPostprocessSignal(const DspLrpHeaderOutput::Header* hdrptr, DspRawSweepData* sc)
{
	int map_type=0;

	if ((!hdrptr->ge.mode.radiate) && (dsp_ris_tgt_db.ignore_silence==0))
		return false;

	switch(hdrptr->ge.mode.operation_mode)
	{
	default:
		break;
	case iddtypes::gm_master_mode:
	case iddtypes::dbs_master_mode:
	case iddtypes::sea_low_master_mode:
		map_type=1;
		break;
	case iddtypes::wa_master_mode:
		map_type=2;
		break;
	case iddtypes::sar_master_mode:
		map_type=3;
		break;
	}

	if (map_type==0)
		return false;

	if (map_type==3)
	{
        ris_sar_image_t& img=*(ris_sar_image_t*)&(sc->radials);

    	int sq_min=(SAR_IMAGE_DX/2)-512;
    	int sq_max=(SAR_IMAGE_DX/2)+512;

        for(int r=0; r<SAR_IMAGE_DX; ++r)
        {
        	uint16_t* p=img.data[r];

        	if ((r<sq_min) || (r>sq_max))
        	{
            	for(int c=0; c<SAR_IMAGE_DX; ++c)
            	{
            		p[c]=256;
            	}
        	}
        	else
        	{
        		int c=0;
            	for(c=0; c<sq_min; ++c)
            	{
            		p[c]=512;
            	}
            	for(; c<sq_max; ++c)
            	{
            		p[c]=16000;
            	}
            	for(; c<SAR_IMAGE_DX; ++c)
            	{
            		p[c]=512;
            	}
        	}
        }
		return true;
	}
	unsigned int n_radials=sc->radials.valid_radials;

	if (n_radials==0)
		return false;

	int samples=sc->radials.sweep[0].samples;

	float range_scale_meters=DspRisRanceScale2Meters(hdrptr->ge.mode.range_scale);
	float scale_m=samples/range_scale_meters;

	float ac_altitude=hdrptr->ge.general_settings.navigation.geo_pos.altitude_m;

	float x_range=dsp_ris_tgt_db.rbm_range;

	if (x_range<ac_altitude)
	{
		x_range=range_scale_meters-1000;
		dsp_ris_tgt_db.rbm_range=x_range;
	}
	if (x_range>range_scale_meters)
	{
		x_range=range_scale_meters-1000;
		dsp_ris_tgt_db.rbm_range=x_range;
	}

	int alt_line=ac_altitude*scale_m;
	if (alt_line>samples)
		alt_line=samples;

	int gain=hdrptr->ge.function_settings.pe_settings.mgc;
	if (gain>127)
		gain=127;

	float ptaz=hdrptr->ge.general_settings.navigation.attitude.platfrom_azimuth_rad;


	if (map_type==2)
	{
		float nav_az=hdrptr->ge.general_settings.antenna.position.navaz_rad;
		int x_rbin=fabsf((x_range*scale_m)/cosf(nav_az+ptaz));
		for(unsigned int n=0; n<n_radials; ++n)
		{
			uint8_t* d=sc->radials.sweep[n].data;
			memset(d, 0, samples);
			for(int i=-16; i<16; ++i)
			{
				int pos=x_rbin+i;
				if ((pos<samples) && (pos>10))
				{
					int lvl=i<0 ? -i : i;
					lvl=16-lvl;
					lvl/=5;
					lvl*=2;
					d[x_rbin-i]=lvl;
				}
			}
			sc->radials.sweep[n].azimuth=nav_az;
		}
	}
	else
	{
		static idddsp::dsp_detections_t dspdet;
		dspdet.detections.num_of_detected_tgts=0;

		DspRisTargetsLookup(hdrptr, dspdet, 0, false);
		float nav_az=sc->radials.sweep[0].azimuth;
		int x_rbin=fabsf((x_range*scale_m)/cosf(nav_az+ptaz));
		for(unsigned int n=0; n<n_radials; ++n)
		{
			int i=0;
			uint8_t* d=sc->radials.sweep[n].data;
			for(;i<alt_line; ++i)
			{
				d[i]=0;
			}
			unsigned char v=64+gain;
			for(;i<samples; ++i)
			{
				d[i]=v;
			}

			if (dsp_ris_tgt_db.rbm_sim)
			{
				unsigned char x_intesity=v+40;

				for(int i=-5; i<5; ++i)
				{
					int pos=x_rbin+i;
					if ((pos<samples) && (pos>10))
						d[x_rbin-i]=x_intesity; //+i;
				}
			}
		}


		int tgt_size=2;

		switch(hdrptr->ge.mode.range_scale)
		{
		default:
		case iddtypes::_10NM:
			//samples=123*2;
			break;
		case iddtypes::_20NM:
			//samples=247*2; //samples*=2;
			tgt_size*=2;
			break;
		case iddtypes::_40NM:
			//samples=494*2; //samples*=4;
			tgt_size*=4;
			break;
		case iddtypes::_80NM:
			//samples=988*2; //samples*=8;
			tgt_size*=8;
			break;
		}

		for(unsigned int n=0; n<dspdet.detections.num_of_detected_tgts; ++n)
		{
			if (!dspdet.detections.target_detections[n].detection_valid)
				continue;

			float rng=dspdet.detections.target_detections[n].detected_tgt.detected_tgt_range;
			int rbin=rng*cosf(dspdet.detections.target_detections[n].associated_elevation)*scale_m;

			int b_min=rbin-tgt_size;
			int b_max=rbin+tgt_size;
			if (b_min<0)
				b_min=0;
			if (b_max>samples)
				b_max=samples;

			float az_offset=fabsf(dspdet.detections.target_detections[n].associated_azimuth-nav_az);
			float w=1;
			if (az_offset>FLT_EPSILON)
					w=1-(az_offset/0.04);
			float intensity=200;
			if (w<=0)
				intensity=0;
			else
				intensity*=w;

			//intensity=200;

			if ((b_min<alt_line) || (intensity<64))
			{
				dspdet.detections.target_detections[n].detection_valid=false;
				continue;
			}
			//unsigned char intensity_color=intensity;
			uint8_t* d=sc->radials.sweep[n].data;

			for(int k=b_min; k<b_max; ++k)
			{
				unsigned char intensity_color=intensity-fabsf(10*((k-rbin)/(tgt_size*2)));
				d[k]=intensity_color;
			}
		}
	}

	return true;
}