/*
 * DspRisProxy.cpp
 *
 */

#include "DspRisProxy.h"

#include "dsp_ris_internals_.h"

#include "rtlog.h"

#include <stdint.h>

#define LOGID	"RIS"

#ifdef __MINGW32__

#define RIS_PROXY_BRIDGE_SECTION

#else

#define RIS_PROXY_BRIDGE_SECTION __attribute__((section(".dsp_ris_proxy_bridge")))

#endif

struct dsp_ris_proxy_t
{
	int grant;
	int enable;
	int map_enable;
};

static dsp_ris_proxy_t dsp_ris_proxy;

struct dsp_ris_proxy_bridge_t
{
	uint32_t marker_begin;

	uint32_t marker_end;
};

dsp_ris_proxy_bridge_t dsp_ris_proxy_bridge__ RIS_PROXY_BRIDGE_SECTION;

void DspRisProxy::risMasterGrant(bool grant)
{
	if (dsp_ris_proxy.grant)
	{
		if (!grant)
		{
			dsp_ris_proxy.enable=0;
		}
		else
			dsp_ris_proxy.enable=1;
	}
	else
	{
		if (grant)
		{
			DspRisInstall();
			rtlog_const_message(LOGID, rtlog_breaking_news|rtlog_report, "RIS GRANT: ENABLE");
			dsp_ris_proxy.grant=1;
			dsp_ris_proxy.enable=1;
		}
	}
}


void DspRisProxy::risProcessEnvironment(unsigned int timetag, const iddtypes::inu_data_t* inu)
{
	if (!dsp_ris_proxy.enable)
		return;
	if (!inu)
		return;

	DspRisProcessEnvironment(timetag, inu);
}

#include "dspExt.h"
#include <math.h>

void DspRisProxy::risPostprocessNci(DspLrpHeaderOutput::Header* hdrptr,
				float* sum,
				float* guard,
				float* daz,
				float* del)
{
	//if (!dsp_ris_proxy.enable)
	//	return;


    if (hdrptr->ge.mode.operation_mode==iddtypes::agr_master_mode)
    {
    	if (dsp_ris_tgt_db.agr.enable)
    	{
  		   const float RB_LEN=74.948125f;
  		   int N_RBIN=(hdrptr->results.nci_results.n_rbin);
		   int r_len=(dsp_ris_tgt_db.agr.rb_len>0 ? dsp_ris_tgt_db.agr.rb_len : 50)/2;
		   float max_rng=N_RBIN*RB_LEN;

  		   N_RBIN=N_RBIN  & (~1U); // make even

  		   float el=-hdrptr->ge.general_settings.antenna.position.navel_rad;
  		   bool lock=false;
  		   float rng=0;

  		   if ((hdrptr->ge.function_settings.pe_settings.stt_rb_start>0) && (hdrptr->ge.function_settings.pe_settings.stt_rb_stop>hdrptr->ge.function_settings.pe_settings.stt_rb_start))
  		   {
  			   r_len=(hdrptr->ge.function_settings.pe_settings.stt_rb_stop-hdrptr->ge.function_settings.pe_settings.stt_rb_start)/2;
  			   rng=(hdrptr->ge.function_settings.pe_settings.stt_rb_start+r_len)*RB_LEN;

  			   if ((rng<max_rng) && (rng>RB_LEN))
  			   {
  				   lock=true;
  			   }
  		   }
  		   else if (el>deg_to_rad(0.2))
  		   {
  			   float sel=sinf(el);
  			   float altitude=hdrptr->ge.general_settings.navigation.geo_pos.altitude_m;
  			   rng=altitude/sel;
  			   float min_rng=dsp_ris_tgt_db.agr.enable>1 ? 10 : altitude;

  			   if ((rng<max_rng) && (rng>min_rng))
  	  			   lock=true;
  			   //if ((rng<max_rng) && (rng>altitude))
  			   //	   lock=true;
  		   }

  		   if (lock)
  		   {
  			   int r_start=dsp_ris_tgt_db.agr.rb_start ? dsp_ris_tgt_db.agr.rb_start : rng/RB_LEN;
  			   //int r_len=(dsp_ris_tgt_db.agr.rb_len>0 ? dsp_ris_tgt_db.agr.rb_len : 50)/2;
  			   r_start-=r_len;
  			   int r_stop=r_start+r_len;

  			   r_start=r_start & (~1U); // make even
  			   r_stop=r_stop & (~1U); // make even

  			   int i=0;

  			   float* __restrict S_NCI=sum;
  			   float* __restrict G_NCI=guard;
  			   float* __restrict DAZ_NCI=daz;
  			   float* __restrict DEL_NCI=del;

  			   {
  				   int t_begin=r_start<N_RBIN ? r_start : N_RBIN;

  				   std::_nassert(t_begin % 2 ==0);
  				   std::_nassert(((unsigned int)S_NCI % 4)==0);
  				   std::_nassert(((unsigned int)G_NCI % 4)==0);
  				   std::_nassert(((unsigned int)DAZ_NCI % 4)==0);
  				   std::_nassert(((unsigned int)DEL_NCI % 4)==0);
  				   for(i=0; i<t_begin; ++i)
  				   {
  					   S_NCI[i]=(1+(i & 7));  //Noise
  					   G_NCI[i]=1;
  					   DAZ_NCI[i]=-1;
  					   DEL_NCI[i]=-1;
  				   }
  			   }

  			   if (r_stop>=N_RBIN)
  				   r_stop=N_RBIN;

  			   float d_lvl=(r_stop-r_start);

  			   float level=dsp_ris_tgt_db.agr.level ? dsp_ris_tgt_db.agr.level : 100;

  			   if (level>32000)
  				   level=32000;
  			   if (level<1)
  				   level=1;

  			   float level_start=level;

  			   float l_inc=(level*2)/d_lvl;

  			   float l_max=1/level;
  			   level=-level;

			   std::_nassert(r_stop % 2 ==0);
			   std::_nassert(((unsigned int)S_NCI % 4)==0);
			   std::_nassert(((unsigned int)G_NCI % 4)==0);
			   std::_nassert(((unsigned int)DAZ_NCI % 4)==0);
			   std::_nassert(((unsigned int)DEL_NCI % 4)==0);
  			   for(;i<r_stop; ++i)
  			   {
  				   float L=(level<0 ? -level : level);
  				   S_NCI[i]=100+level_start-L;
  				   DAZ_NCI[i]=level*l_max; //L*l_max;
  				   DEL_NCI[i]=level*l_max; //L*l_max;
  				   G_NCI[i]=0.3*S_NCI[i];
  				   level+=l_inc;
  			   }

			   std::_nassert(N_RBIN % 2 ==0);
			   std::_nassert(((unsigned int)S_NCI % 4)==0);
			   std::_nassert(((unsigned int)G_NCI % 4)==0);
			   std::_nassert(((unsigned int)DAZ_NCI % 4)==0);
			   std::_nassert(((unsigned int)DEL_NCI % 4)==0);
  			   for(; i<N_RBIN; ++i)
  			   {
  				   S_NCI[i]=(1+(i & 0x7));  //Noise
  				   G_NCI[i]=(1+(i & 0x7));  //Noise
  				   DAZ_NCI[i]=1;
  				   DEL_NCI[i]=1;
  			   }
  		   }
  	   }
    }

}


bool DspRisProxy::risPreprocessSignal(DspInputSignalDescriptor* in)
{
	if (!dsp_ris_proxy.enable)
		return false;
	DspRisSfpUpdateHeader(&in->header.header);
	return true;
}

bool DspRisProxy::risPostprocessSignal(const DspLrpHeaderOutput::Header* hdrptr, DspRawSweepData* sc)
{
	if (!dsp_ris_proxy.enable)
		return false;

	return DspRisPostprocessSignal(hdrptr, sc);
}

bool DspRisProxy::risPostprocessDetection(const DspLrpHeaderOutput::Header* hdrptr, idddsp::dsp_detections_t* det, idddsp::dsp_stt_data_t* stt)
{
	if (!dsp_ris_proxy.enable)
		return false;

	DspRisTargetsLookup(hdrptr, *det, stt, true);

	return true;
}

static void ris_go()
{
	DspRisProxy::risMasterGrant(true);
}

static void ris_stop()
{
	dsp_ris_proxy.enable=0;
}

#include "mex.h"

int dsp_ris_proxy_autostart_;

void DspRisProxy::risInitialize()
{
	static unsigned int ris_proxy_initialized;
	if (!ris_proxy_initialized)	//WARNING: not thread safe
	{
		ris_proxy_initialized=1;
		mex_add_simple_command("ris_go", ris_go);
		mex_add_simple_command("ris_stop", ris_stop);
	}
#ifdef __MINGW32__
	if (dsp_ris_proxy_autostart_)
		ris_go();
#endif
}