/*
 * DbsScanHelper.cpp
 *
 */


#include "DbsScanHelper.h"

#include "gc_math.h"


#define DBS_MAP_PROC_LIMIT GC_DEG_TO_RAD(2)
#define GIMBAL_LIMIT GC_DEG_TO_RAD(60)
#define TEST_MAP_PROC_LIMIT GC_DEG_TO_RAD(5)

static const float DBS_AC_MIN=GC_K_KNOTS_TO_MS*70; //200;  70Knots required for flight trials
static const float DBS_AC_MIN_HIST=GC_K_KNOTS_TO_MS*2; //200;  80Knots required for flight trials

class DbsScanImplementation: public DbsScanHelper
{
public:

  static void cvr2d(dbs_scan_sector_t& v)
  {
    float* f=(float*)&v;
    //int n=sizeof(v)/sizeof(float);
    for(int i=2; i<10; ++i)
    {
      float x=bound_pi(f[i]);
      f[i]=x;
    }
  }

  bool dfeMaster;

  dbs_scan_volume_t vout;

  float ac_vel_previous;
  //float ac_vel_hyst;
  float scan_center_hyst;
  float scan_center_previous;

  float min_speed;
  float min_speed_hist;
  float map_proc_limit; //MAP_PROC_LIMIT
  dbs_scan_volume_t pvout;

  void initialize()
  {
	  scan_center_hyst=0;
	  scan_center_previous=0;
	  if (min_speed_hist==0)
		  min_speed_hist=DBS_AC_MIN_HIST;
	  if (min_speed==0)
		  min_speed=DBS_AC_MIN;
	  if (map_proc_limit==0)
		  map_proc_limit=TEST_MAP_PROC_LIMIT;
  }

  static float bound_pi(float x)
  {
    return gc_math::angle_bound_pi(x);
  }

  void set_sector(dbs_scan_sector_t& s, float center)
  {
    s.center=center+s.half_width+s.int_lim;
  }

  float CheckMapProcLimit()
  {
	  if ((map_proc_limit<DBS_MAP_PROC_LIMIT) || (map_proc_limit>DBS_MAP_PROC_LIMIT*10))
		  	  map_proc_limit=DBS_MAP_PROC_LIMIT;
	  return map_proc_limit;
  }

  void calc_sector(dbs_scan_sector_t& s, float left_right, float drift, float scan_center, float scan_width)
  {
	float bar_center=drift+scan_center;

	float MAP_PROC_LIMIT=CheckMapProcLimit(); //map_proc_limit;

	//s.int_lim=bar_center+left_right*MAP_PROC_LIMIT;
	s.int_lim=bar_center-(left_right)*(scan_width/2);//+left_right*MAP_PROC_LIMIT;
	s.ext_lim=s.int_lim+left_right*scan_width;
    s.hard_ext_lim=left_right*GIMBAL_LIMIT;
    s.hard_int_lim=drift+left_right*MAP_PROC_LIMIT;
    if (left_right>=0)
    {
    	if (s.int_lim<s.hard_int_lim)
    	{
    		s.int_lim=s.hard_int_lim;
    		s.ext_lim=s.hard_int_lim+left_right*scan_width;
    		if (s.ext_lim>s.hard_ext_lim)
        		s.ext_lim=s.hard_ext_lim;
    	}
    	if (s.ext_lim>s.hard_ext_lim)
    	{
    		s.ext_lim=s.hard_ext_lim;
    		s.int_lim=s.hard_ext_lim-left_right*scan_width;
    		if (s.int_lim<s.hard_int_lim)
        		s.int_lim=s.hard_int_lim;
    	}
    }
    else
    {
    	if (s.int_lim>s.hard_int_lim)
    	{
    		s.int_lim=s.hard_int_lim;
    		s.ext_lim=s.hard_int_lim+left_right*scan_width;
    		if (s.ext_lim<s.hard_ext_lim)
        		s.ext_lim=s.hard_ext_lim;
    	}
    	if (s.ext_lim<s.hard_ext_lim)
    	{
    		s.ext_lim=s.hard_ext_lim;
    		s.int_lim=s.hard_ext_lim-left_right*scan_width;
    		if (s.int_lim>s.hard_int_lim)
        		s.int_lim=s.hard_int_lim;
    	}
    }
    s.half_width=(s.ext_lim-s.int_lim)/2;
    s.abs_half_width=fabs(s.half_width);

	s.factor=1;

    if (s.abs_half_width<GC_DEG_TO_RAD(2))
    {
    	s.valid=false;
    	s.factor=1;
    }
    else
    {
    	//s.factor=logf(fabs(tanf(s.ext_lim/2)/tanf(s.int_lim/2)));
    	s.valid=true;
    }
  }

  bool calculateNewScan(dbs_scan_volume_t& volume, float scan_center_cmd, float scan_width_cmd, float ptaz, float vx, float vy, float vz)
  {
	  float MAP_PROC_LIMIT=CheckMapProcLimit(); //map_proc_limit;

	  vout.invMsg=0;
	  vout.clear=false;

      //const float AC_MIN=GC_K_KNOTS_TO_MS*100; //200;
      //const float AC_VMIN_THR=10.0f; //m/s = 20Knots
      //const float SECT3_CEN_OFFSET=0.0261799387799149; //1.5deg
      const float SCAN_WIDTH_15=GC_DEG_TO_RAD(16.5);
      const float SCAN_WIDTH_MARGIN_15=GC_DEG_TO_RAD(28);   //30 deg but with a little selection margin
      const float SCAN_WIDTH_30=GC_DEG_TO_RAD(30);   //30 deg but with a little selection margin
      const float SCAN_WIDTH_MARGIN_30=GC_DEG_TO_RAD(58);   //30 deg but with a little selection margin
      const float MAX_DRIFT=GC_DEG_TO_RAD(40);

      float ground_track_angle=atan2(vy, vx);
      float drift_angle = bound_pi(ground_track_angle+ptaz);

      float ac_vel=sqrtf(vx*vx+vy*vy+vz*vz);
      //float ac_ground_vel=sqrtf(vx*vx+vy*vy);

      float width=GC_DEG_TO_RAD(60.0);
      float bar_valid=0;

      float scan_center = bound_pi(scan_center_cmd+ptaz);
      {
          if  (((scan_center>=0) && (scan_center_previous>=0)) || ((scan_center<0) && (scan_center_previous<0)))
          {
        	  scan_center_previous=scan_center;
        	  scan_center_hyst=scan_center>=0 ? -MAP_PROC_LIMIT : MAP_PROC_LIMIT;
          }
          else
          {
        	  if ((scan_center>=0) && (scan_center>=scan_center_hyst))
        		  scan_center_previous=scan_center;
        	  else if ((scan_center<0) && (scan_center<=scan_center_hyst))
        		  scan_center_previous=scan_center;
        	  scan_center=scan_center_previous;
          }

    	  const float H_EXT_LIMIT=GIMBAL_LIMIT; //-GC_DEG_TO_RAD(15);
    	  const float H_INT_LIMIT=GC_DEG_TO_RAD(20);

          if (scan_center>=0)
          {
        	  if (scan_center>H_EXT_LIMIT)
        	  {
        		  scan_center=H_EXT_LIMIT;
        	  }
        	  if (scan_center<H_INT_LIMIT)
        	  	  scan_center=H_INT_LIMIT;
          }
          else
          {
        	  if (scan_center<-H_EXT_LIMIT)
        	  {
        		  scan_center=-H_EXT_LIMIT;
        	  }
        	  if (scan_center>-H_INT_LIMIT)
        	  	  scan_center=-H_INT_LIMIT;
          }
      }

      if (scan_width_cmd<SCAN_WIDTH_MARGIN_30)
      {
    	  width=(scan_width_cmd>SCAN_WIDTH_MARGIN_15 ? SCAN_WIDTH_30 : SCAN_WIDTH_15);
    	  bar_valid=scan_center>=0 ? 1 : -1;

    	  calc_sector(vout.b[0], bar_valid, drift_angle, scan_center, width*2);
    	  vout.b[1]=vout.b[0];
      }
      else
      {
    	  calc_sector(vout.b[0], 1, drift_angle, 0, width);
    	  calc_sector(vout.b[1], -1, drift_angle, 0, width);
      }

      float heading=-ptaz;

      vout.b[0].bar_num=1;
      vout.b[1].bar_num=2;

      vout.fullVolume.bar_num=1;

      set_sector(vout.b[0], heading);
      set_sector(vout.b[1], heading);

      vout.valid=true;

#if 0
      if ((!vout.b[0].valid) || (!vout.b[1].valid))
      {
    	  vout.valid=false;
    	  vout.invMsg="Too Drifted";
      }
#endif

      if (bar_valid==0)
      {
    	  vout.side=0;
    	  if (drift_angle>MAX_DRIFT)
    	  {
    		  bar_valid=-1;	//force single bar right
    		  vout.b[0]=vout.b[1];
    	  }
    	  else if (drift_angle<-MAX_DRIFT)
    	  {
    		  bar_valid=1;	//force single bar left
    	  }
    	  else
    	  {
			  vout.fullVolume.center=(drift_angle/2)+heading;
			  vout.fullVolume.ext_lim=(vout.b[0].ext_lim+vout.b[0].center);
			  vout.fullVolume.int_lim=(vout.b[1].int_lim+vout.b[0].center);
			  vout.fullVolume.half_width=fabs(width)-fabs(drift_angle/2);
			  vout.fullVolume.abs_half_width=vout.fullVolume.half_width;
			  vout.fullVolume.bar_num=2;
    	  }
      }

      if (bar_valid!=0)
      {
          if (bar_valid<0)
          {
        	  vout.side=-1;

        	  if (drift_angle>=GIMBAL_LIMIT)
        	  {
            	  vout.valid=false;
            	  vout.invMsg="Too Drifted";
        	  }
          }
          else if (bar_valid>0)
          {
        	  vout.side=1;

        	  if (drift_angle<=-GIMBAL_LIMIT)
        	  {
            	  vout.valid=false;
            	  vout.invMsg="Too Drifted";
        	  }

          }
          vout.b[1]=vout.b[0];
          vout.fullVolume.center=vout.b[0].center;
          vout.fullVolume.ext_lim=(vout.b[0].ext_lim+heading);
          vout.fullVolume.int_lim=(vout.b[0].int_lim+heading);
		  vout.fullVolume.half_width=fabs(vout.b[0].abs_half_width);//-fabs(drift_angle);
		  vout.fullVolume.abs_half_width=vout.fullVolume.half_width;
          vout.fullVolume.bar_num=1;
      }

      vout.platform_azimuth=ptaz;
      vout.ground_track=ground_track_angle;
      vout.dirft=drift_angle;


      cvr2d(vout.b[0]);
      cvr2d(vout.b[1]);
      cvr2d(vout.fullVolume);

      vout.tooSlow=false;

      float ac_min=min_speed>0 ? min_speed : DBS_AC_MIN;
      float ac_hist=min_speed_hist>0 ? min_speed_hist : DBS_AC_MIN_HIST;

      if (ac_vel<ac_min)
      {
    	  //ac_vel_hyst=GC_K_NM_TO_METERS(10);
    	  vout.valid=false;
          vout.tooSlow=true;
          vout.invMsg="Too Slow";
          ac_vel_previous=ac_vel;
      }
      else if (ac_vel_previous<ac_min)
      {
    	  if (ac_vel<(ac_min+ac_hist))
    		  vout.tooSlow=true;
    	  else
    		  ac_vel_previous=ac_vel;
      }

      if (vout.tooSlow)
      {
    	  vout.valid=false;
          vout.tooSlow=true;
          vout.invMsg="Too Slow";
      }


      if ((vout.fullVolume.bar_num!=pvout.fullVolume.bar_num) || (vout.side!=pvout.side))
    	  vout.clear=true;
      volume=vout;

      pvout=vout;

      return vout.valid;
  }
};


DbsScanImplementation autoMonitorable_dbsScan;

void DbsScanHelper::initialize()
{
	autoMonitorable_dbsScan.initialize();
}


bool DbsScanHelper::calculateNewScan(dbs_scan_volume_t& volume, float scan_center_cmd, float scan_width, float ptaz, float vx, float vy, float vz)
{
	return autoMonitorable_dbsScan.calculateNewScan(volume, scan_center_cmd, scan_width, ptaz, vx, vy, vz);
}

DbsScanHelper& DbsScanHelper::instance()
{
	return autoMonitorable_dbsScan;
}

DbsScanHelper::dbs_scan_volume_t& DbsScanHelper::scanVolume()
{
	return autoMonitorable_dbsScan.vout;
}

bool DbsScanHelper::dfeMaster()
{
	return autoMonitorable_dbsScan.dfeMaster;
}

#if 0
bool calculateNewScan(dbs_scan_volume_t& volume, float scan_center_cmd, float scan_width, float ptaz, float vx, float vy, float vz)
{
	  vout.invMsg=0;

    const float AC_MIN=GC_K_KNOTS_TO_MS*20; //200;
    //const float AC_VMIN_THR=10.0f; //m/s = 20Knots
    //const float SECT3_CEN_OFFSET=0.0261799387799149; //1.5deg
    const float SCAN_WIDTH_15=GC_DEG_TO_RAD(16.5);
    const float SCAN_WIDTH_MARGIN_15=GC_DEG_TO_RAD(28);   //30 deg but with a little selection margin
    //const float SCAN_WIDTH_30=GC_DEG_TO_RAD(30);   //30 deg but with a little selection margin
    const float SCAN_WIDTH_MARGIN_30=GC_DEG_TO_RAD(58);   //30 deg but with a little selection margin
    const float MAX_DRIFT=GC_DEG_TO_RAD(40);

    float ground_track_angle=atan2(vy, vx);
    float drift_angle = bound_pi(ground_track_angle+ptaz);

    float ac_vel=sqrtf(vx*vx+vy*vy+vz*vz);
    //float ac_ground_vel=sqrtf(vx*vx+vy*vy);

    //float ac_vel_stby=ac_vel*0.2+ac_vel_previous*0.8;
    //ac_vel_previous=ac_vel_stby;

    //float abs_drift=DRIFT_ANGLE<0 ? -DRIFT_ANGLE : DRIFT_ANGLE;

    //Calculate valid scan limits based on maximum width and drift angle
    float left_ext_limit=drift_angle+GIMBAL_LIMIT;
    float left_int_limit=drift_angle+MAP_PROC_LIMIT;

    float right_ext_limit=drift_angle-GIMBAL_LIMIT;
    float right_int_limit=drift_angle-MAP_PROC_LIMIT;

    //Gimbal limits
    if (drift_angle<0) //limit right
    {
        if (right_ext_limit<-GIMBAL_LIMIT)
            right_ext_limit=-GIMBAL_LIMIT;
    }
    else if (left_ext_limit>GIMBAL_LIMIT)
        left_ext_limit=GIMBAL_LIMIT;

    //Validate commanded scan width and center on azimuth command where required
    int bar_valid=0;

    float scan_center = bound_pi(scan_center_cmd+ptaz);

    if (scan_width<=SCAN_WIDTH_MARGIN_30)
    {
        if  (((scan_center>=0) && (scan_center_previous>=0)) || ((scan_center<00) && (scan_center_previous<0)))
        {
      	  scan_center_previous=scan_center;
      	  scan_center_hyst=scan_center>=0 ? -MAP_PROC_LIMIT : MAP_PROC_LIMIT;
        }
        else
        {
      	  if ((scan_center>=0) && (scan_center>=scan_center_hyst))
      		  scan_center_previous=scan_center;
      	  else if ((scan_center<0) && (scan_center<=scan_center_hyst))
      		  scan_center_previous=scan_center;
      	  scan_center=scan_center_previous;
        }

        right_ext_limit=-GIMBAL_LIMIT;
        left_ext_limit=+GIMBAL_LIMIT;

        float bar_id=scan_center<0: 1 : -1;

        if (scan_width<SCAN_WIDTH_MARGIN_15)
      	  wdith=SCAN_WIDTH_15;
        else
      	  wdith=SCAN_WIDTH_30;

        float bar_center=drift_angle+scan_center;
        float ext_lim=bar_center-bar_id*width;
        float int_lim=
    }
    else
    {
  	  scan_center_previous=scan_center;
  	  scan_center_hyst=0;
    }

    if (scan_width<SCAN_WIDTH_MARGIN_15)
    {
        float width=(SCAN_WIDTH_15/2)-MAP_PROC_LIMIT;

        if (scan_center>=0)
        {//center left
            float actual_center=(left_ext_limit-left_int_limit)/2;
            float delta=actual_center-scan_center;
            float in_limit=in_limit-delta;
            if (in_limit>left_int_limit)
            {
                float ext_limit=in_limit+width;
                if (ext_limit<left_ext_limit)
                {
                    left_ext_limit=ext_limit;
                    left_int_limit=in_limit;
                }
                else
                    left_int_limit=left_ext_limit-SCAN_WIDTH_15;
            }
            else
                left_ext_limit=left_int_limit+SCAN_WIDTH_15;
            bar_valid=1;
        }
        else //center right
        {
            bar_valid=-1;
            float in_limit=scan_center+width;
            if (in_limit<right_int_limit)
            {
                float ext_limit=in_limit-width;
                if (ext_limit>right_ext_limit)
                {
                    right_ext_limit=ext_limit;
                    right_int_limit=in_limit;
                }
            }
            else
                right_ext_limit=in_limit-width;
        }
    }
    else if (scan_width<SCAN_WIDTH_MARGIN_30)
    {
        bar_valid=(scan_center>=0) ? 1 : -1;
    }



#if 0
    const float SCAN_CONST=211.471E-6;

    const float C_LIGHT_M_S=2.99792E8f;
    const float tx_chan_mhz=9280000000;
    float lambda=C_LIGHT_M_S/tx_chan_mhz;

    float factor=logf(fabs((tanf(left_limit/2))/tan(left_int_limit/2)));

    if (bnum==2)
        factor+=logf(fabs((tanf(right_limit/2))/tan(right_int_limit/2)));


    float map_ref_time=lambda*(SCAN_CONST*factor/ac_ground_vel);


    float half_scan_width=width;

    float bar_center=half_scan_width+MAP_PROC_LIMIT; //+DRIFT_ANGLE;


    float external_lim=bar_center+half_scan_width;
    float internal_lim=bar_center+half_scan_width;
    float hard_ext_lim=MAP_PROC_LIMIT;
#endif


    float heading=-ptaz;

    vout.b[0].bar_num=1;
    vout.b[1].bar_num=2;

    set_sector(vout.b[0], heading, left_ext_limit, left_int_limit);
    set_sector(vout.b[1], heading, right_ext_limit, right_int_limit);

    vout.valid=true;

    if (bar_valid==0)
    {
  	  if (drift_angle>MAX_DRIFT)
  	  {
  		  bar_valid=-1;	//force single bar right
  	  }
  	  else if (drift_angle<-MAX_DRIFT)
  	  {
  		  bar_valid=1;	//force single bar left
  	  }
  	  else
  	  {
			  vout.fullVolume.center=drift_angle+heading;
			  vout.fullVolume.ext_lim=(vout.b[0].ext_lim+vout.b[0].center);
			  vout.fullVolume.int_lim=(vout.b[0].int_lim+vout.b[0].center);
			  vout.fullVolume.width=scan_width-fabs(drift_angle);
			  vout.fullVolume.bar_num=2;
  	  }
    }

    if (bar_valid!=0)
    {
        if (bar_valid<0)
        {
      	  if (drift_angle>=GIMBAL_LIMIT)
      	  {
          	  vout.valid=false;
          	  vout.invMsg="Too Drifted";
      	  }
            vout.b[1].bar_num=1;
            vout.b[0]=vout.b[1];
        }
        else if (bar_valid>0)
        {
      	  if (drift_angle<=-GIMBAL_LIMIT)
      	  {
          	  vout.valid=false;
          	  vout.invMsg="Too Drifted";
      	  }

            vout.b[1]=vout.b[0];
        }
        vout.fullVolume.center=vout.b[0].center;
        vout.fullVolume.ext_lim=(vout.b[0].ext_lim+heading);
        vout.fullVolume.int_lim=(vout.b[0].int_lim+heading);
        vout.fullVolume.width=vout.b[0].width;

        vout.fullVolume.bar_num=1;
    }

    vout.platform_azimuth=ptaz;
    vout.ground_track=ground_track_angle;
    vout.dirft=drift_angle;


    cvr2d(vout.b[0]);
    cvr2d(vout.b[1]);
    cvr2d(vout.fullVolume);

    if (ac_vel<AC_MIN)
    {
  	  //ac_vel_hyst=GC_K_NM_TO_METERS(10);
  	  vout.valid=false;
        vout.tooSlow=true;
        vout.invMsg="Too Slow";
        ac_vel_previous=ac_vel;
    }
    else if (ac_vel_previous<AC_MIN)
    {
  	  if (ac_vel<(AC_MIN+(GC_K_KNOTS_TO_MS*10)))
  		  vout.tooSlow=true;
  	  else
  		  ac_vel_previous=ac_vel;
    }

    if (vout.tooSlow)
    {
  	  vout.valid=false;
        vout.tooSlow=true;
        vout.invMsg="Too Slow";
    }


    volume=vout;

    return vout.valid;
}
#endif
