
#include "gc_algorithms.h"

//#include "gc_geometry.h"
#include "gc_config.h"

using namespace gc_cfg;
using namespace gc_math;


#define PIX_AZI_SCA_over_MAX_AZI_SCA  (float(PIX_AZI_SCA)/float(MAX_AZI_SCA))
#define PIX_ELE_SCA_over_MAX_ELE_SCA  (float(PIX_ELE_SCA)/float(MAX_ELE_SCA))
#define MAX_AZI_SCA_over_PIX_AZI_SCA  (1.0f/PIX_AZI_SCA_over_MAX_AZI_SCA)
#define MAX_ELE_SCA_over_PIX_ELE_SCA  (1.0f/PIX_ELE_SCA_over_MAX_ELE_SCA)

static inline int rad_pix_body_util(int center, float limit, float angle)
{
  return center+nint(limit*angle);
}

int gc_math::az_pix_body(float az)
{
  return rad_pix_body_util(X_AZI_SCA, PIX_AZI_SCA_over_MAX_AZI_SCA, az); //X_AZI_SCA+nint(PIX_AZI_SCA_over_MAX_AZI_SCA*az);
}

int gc_math::az_pix_body_half_scan_sector(float az, bool is_left_sector)
{
  if(is_left_sector)
    return rad_pix_body_util((X_AZI_SCA + PIX_AZI_SCA), (2.0f * PIX_AZI_SCA_over_MAX_AZI_SCA), az);
  else
    return rad_pix_body_util((X_AZI_SCA - PIX_AZI_SCA), (2.0f * PIX_AZI_SCA_over_MAX_AZI_SCA), az);
}

int gc_math::az_pix_nav(float az, float plat_az)
{
  return X_AZI_SCA-nint(PIX_AZI_SCA_over_MAX_AZI_SCA*angle_sum_pi(az, plat_az));
}

float gc_math::az_subpix_nav(float az, float plat_az)
{
  return float(X_AZI_SCA)-(PIX_AZI_SCA_over_MAX_AZI_SCA*angle_sum_pi(az, plat_az));
}

int gc_math::el_pix_body(float el)
{
  return rad_pix_body_util(Y_ELE_SCA, PIX_ELE_SCA_over_MAX_ELE_SCA, el);
}

int gc_math::el_pix_nav(float el)
{
  return Y_ELE_SCA-nint(PIX_ELE_SCA_over_MAX_ELE_SCA*el);
}

int gc_math::rng_pix(float range, float range_scale)
{
  return Y_ZERO_RANGE-gc_math::nint((float(Y_ZERO_RANGE-Y_FULL_RANGE)/range_scale)*range);
}

static inline float ppi_pix_rng_factor(float range_scale)
{
  return float(Y_ZERO_RNG_PPI-Y_FULL_RNG_PPI)/range_scale;
}

static inline float pix_ppi_rng_factor(float range_scale)
{
  return 1.0f/ppi_pix_rng_factor(range_scale);
}

static inline float nav_pix_rng_factor(float range_scale)
{
  return float(Y_ZERO_RANGE-Y_FULL_RANGE)/range_scale;
}

static inline float pix_nav_rng_factor(float range_scale)
{
  return 1.0f/nav_pix_rng_factor(range_scale);
}

static inline float ppi_pix_len_factor(float range_scale)
{
  return float((Y_ZERO_RNG_PPI-Y_FULL_RNG_PPI)+1)/range_scale;
}

static inline float ppi_pix_rng(float factor, float angle)
{
  return gc_math::nint(factor*angle);
}

static inline float ppi_subpix_rng(float factor, float angle)
{
  return (factor*angle);
}

static inline float ppi_pix_rng_a(float angle, float range, float range_scale)
{
  return gc_math::nint(ppi_pix_rng_factor(range_scale)*range*angle);
}

int gc_math::ppi_pix_x(float range, float range_scale, float az, float ptaz)
{
  float a=sinf(az+ptaz);
  return X_ZERO_RNG_PPI-ppi_pix_rng_a(a, range, range_scale);
}

int gc_math::ppi_pix_y(float range, float range_scale, float az, float ptaz)
{
  float a=cosf(az+ptaz);
  return Y_ZERO_RNG_PPI-ppi_pix_rng_a(a, range, range_scale);
}

void gc_math::ppi_pix_xy(int* x, int* y, float range, float range_scale, float az, float ptaz)
{
  float a=az+ptaz;
  float sa=sinf(a);
  float ca=cosf(a);
  float factor=ppi_pix_rng_factor(range_scale)*range;
  *x=X_ZERO_RNG_PPI-ppi_pix_rng(factor, sa);
  *y=Y_ZERO_RNG_PPI-ppi_pix_rng(factor, ca);
}

float gc_math::ppi_rng_radius(float scale, float range)
{
  return ppi_pix_rng_factor(scale)*range;
}

void gc_math::rot_xy_ccw(int &x, int &y, float sa, float ca)
{
  float rx =   x * ca - y * sa;
  float ry =   x * sa + y * ca;
  x= nint(rx);
  y= nint(ry);
}

static float nav_rng_ppi_fact(float x, float y, float factor)
{
  float dx = float(X_ZERO_RNG_PPI)-x;
  float dy = float(Y_ZERO_RNG_PPI)-y;
  float rng = sqrtf( dx*dx + dy*dy );
  return factor * rng;
}

float nav_rng_b_fact(float y, float factor)
{
  return (float(Y_ZERO_RANGE)-y) * factor;
}

float gc_math::nav_rng_ppi(float x, float y, float range_scale)
{
  return nav_rng_ppi_fact(x, y, pix_ppi_rng_factor(range_scale));
}

float gc_math::nav_az_ppi(float x, float y, float az_ref)
{
  return angle_sum_pi(atan2f(float(X_ZERO_RNG_PPI)-x, float(Y_ZERO_RNG_PPI)-y), -az_ref);
}

float gc_math::nav_rng_b(float y, float range_scale)
{
  return nav_rng_b_fact(y, pix_nav_rng_factor(range_scale));
}

float gc_math::nav_az_b(float x, float az_ref)
{
  return angle_sum_pi((float(X_AZI_SCA)-x) * MAX_AZI_SCA_over_PIX_AZI_SCA, -az_ref);
}

float gc_math::nav_el_b(float y)
{
  return (float(Y_ELE_SCA)-y) * MAX_ELE_SCA_over_PIX_ELE_SCA;
}

void gc_math::sar_patch(float &rng0, float &rng1, float &daz0, float &daz1, float size, float range)
{
  daz0 = atan2f( size , (2*range) - size);
  daz1 = atan2f( size , (2*range) + size);
  float tmp = powf(size / 2.0f, 2);
  rng0 = sqrtf(powf(range - (size / 2.0f),2) + tmp);
  rng1 = sqrtf(powf(range + (size / 2.0f),2) + tmp);
}

void gc_math::calculator::set_reference(float reference_azimth_rad, float range_scale_mt, float velocity_scale_ms)
{
  set_reference_az(reference_azimth_rad);
  set_reference_rng(range_scale_mt);
  set_reference_vel(velocity_scale_ms);
}

void gc_math::calculator::set_reference_az(float reference_azimth_rad)
{
  ref_azimuth=gc_math::angle_bound_pi(reference_azimth_rad);
}

void gc_math::calculator::set_reference_rng(float range_scale_mt)
{
  ref_range_scale=range_scale_mt;
  c_rng_pix_factor=nav_pix_rng_factor(range_scale_mt);
  c_ppi_rng_factor=ppi_pix_rng_factor(range_scale_mt);
  c_ppi_len_factor=ppi_pix_len_factor(range_scale_mt);
  c_pix_rng_b_factor=pix_nav_rng_factor(range_scale_mt);
  c_pix_rng_ppi_factor=pix_ppi_rng_factor(range_scale_mt);
}

void gc_math::calculator::set_reference_vel(float velocity_scale_ms)
{
  ref_velocity_scale = velocity_scale_ms;
  c_vel_pix_factor=nav_pix_rng_factor(velocity_scale_ms);
  c_ppi_vel_factor=ppi_pix_rng_factor(velocity_scale_ms);
  c_pix_vel_b_factor=pix_nav_rng_factor(velocity_scale_ms);
}

void gc_math::calculator::set_frozen_az(float frozen_azimth_rad)
{
  frz_azimuth = frozen_azimth_rad;
  float alpha = (ref_azimuth - frz_azimuth);
  frz_sa_ac=sinf(alpha);
  frz_ca_ac=cosf(alpha);
}

void gc_math::calculator::set_reference_ownship(float ownship_azimth_rad, float ownship_range_m)
{
  float alpha = (ownship_azimth_rad + frz_azimuth);
  c_ownship_x_ofs = ownship_range_m * sinf(alpha);
  c_ownship_y_ofs = ownship_range_m * cosf(alpha);
}

void gc_math::calculator::set_bar_number(int bar_number)
{
  ref_bar_number=bar_number;
  c_bar_odd=(bar_number & 1) ? true : false;
  c_bar_even=!c_bar_odd;
  c_scan_dir=is_bar_odd() ? scan_dir_left_to_right: scan_dir_right_to_left;
}

int gc_math::calculator::ppi_pix_x(float range, float az)
{
  float a=sinf(az+ref_azimuth);
  float factor=c_ppi_rng_factor*range;
  return X_ZERO_RNG_PPI-ppi_pix_rng(factor, a);
}

int gc_math::calculator::ppi_pix_y(float range, float az)
{
  float a=cosf(az+ref_azimuth);
  float factor=c_ppi_rng_factor*range;
  return Y_ZERO_RNG_PPI-ppi_pix_rng(factor, a);
}

void gc_math::calculator::ppi_pix_xy(int &x, int &y, float range, float az)
{
  float a=az+ref_azimuth;
  float sa=sinf(a);
  float ca=cosf(a);
  float factor=c_ppi_rng_factor*range;
  x= X_ZERO_RNG_PPI-ppi_pix_rng(factor, sa);
  y= Y_ZERO_RNG_PPI-ppi_pix_rng(factor, ca);
}

void gc_math::calculator::rng_az_ppi_pix_xy(int &x, int &y, float range, float az)
{
  float a=az+ref_azimuth;
  float sa=sinf(a);
  float ca=cosf(a);
  float factor=c_ppi_rng_factor*range;
  x= X_ZERO_RNG_PPI-ppi_pix_rng(factor, sa);
  y= Y_ZERO_RNG_PPI-ppi_pix_rng(factor, ca);
}

void gc_math::calculator::rng_az_ppi_subpix_xy(float& x, float& y, float range, float az)
{
  float a=az+ref_azimuth;
  float sa=sinf(a);
  float ca=cosf(a);
  float factor=c_ppi_rng_factor*range;
  x= static_cast<float>(X_ZERO_RNG_PPI)-ppi_subpix_rng(factor, sa);
  y= static_cast<float>(Y_ZERO_RNG_PPI)-ppi_subpix_rng(factor, ca);
}

void gc_math::calculator::vel_az_ppi_pix_xy(int &x, int &y, float velocity, float az)
{
  float a=az+ref_azimuth;
  float sa=sinf(a);
  float ca=cosf(a);
  float factor=c_ppi_vel_factor*velocity;
  x= X_ZERO_RNG_PPI-ppi_pix_rng(factor, sa);
  y= Y_ZERO_RNG_PPI-ppi_pix_rng(factor, ca);
}

gc_math::calculator gc_math::Calc;

//Some stuff about colors
unsigned long gc_math::gray_to_packed_rgb(unsigned int gray)
{
  if (gray>255)
	  gray=255;
  return gray|(gray<<8)|(gray<<16)|(gray<<24);
}

unsigned int gc_math::rgb_to_gray(unsigned int r, unsigned int g, unsigned b)
{
  return gc_math::nint(float(r*11+g*16+b+5)/32.0f);
}

static float exp_cent_calculation(float oc_out, float curs_out, float cent_disp, float ratio)
{
  return oc_out-(curs_out-cent_disp)/ratio;
}

void gc_math::calculator::set_field_of_view(int expand_factor, int virtual_x, int virtual_y, float ref_x, float ref_y, float field_platform_azimuth)
{
  exp_factor=expand_factor==0 ? 1 : expand_factor;

  exp_ratio=float(exp_factor);

  exp_virtual_x=virtual_x;
  exp_virtual_y=virtual_y;

  if (exp_factor==1)
  {
    exp_ref_x=virtual_x;
    exp_ref_y=virtual_y;
    exp_x_cent=X_CENTRE_DISPL;
    exp_y_cent=Y_CENTRE_DISPL;
  }
  else
  {
    exp_ref_x=ref_x;
    exp_ref_y=ref_y;
    exp_x_cent=exp_cent_calculation(exp_ref_x, exp_virtual_x, X_CENTRE_DISPL, exp_ratio);
    exp_y_cent=exp_cent_calculation(exp_ref_y, exp_virtual_y, Y_CENTRE_DISPL, exp_ratio);
  }
  field_ptaz=gc_math::angle_bound_pi(field_platform_azimuth);
}


int gc_math::calculator::exp_raw_pixel_x(float x_un_dis) const
{
  return X_CENTRE_DISPL+exp_ratio*(x_un_dis-exp_x_cent);
}

int gc_math::calculator::exp_raw_pixel_y(float y_un_dis) const
{
  return Y_CENTRE_DISPL+exp_ratio*(y_un_dis-exp_y_cent);
}

int gc_math::calculator::frz_ppi_pix_x(float range, float az)
{
  float a=sinf(az+frz_azimuth);
  float factor=c_ppi_rng_factor*range;
  return X_ZERO_RNG_PPI-ppi_pix_rng(factor, a);
}

int gc_math::calculator::frz_ppi_pix_y(float range, float az)
{
  float a=cosf(az+frz_azimuth);
  float factor=c_ppi_rng_factor*range;
  return Y_ZERO_RNG_PPI-ppi_pix_rng(factor, a);
}

void gc_math::calculator::frz_ppi_pix_xy(int& x, int& y, float range, float az)
{
  x=frz_ppi_pix_x(range, az);
  y=frz_ppi_pix_y(range, az);
}

int gc_math::calculator::frz_ppi_pix_x_os_rel(float range, float az)
{
  float a=sinf(az+frz_azimuth);
  float obj_x_cmp = range * a;
  return X_ZERO_RNG_PPI-gc_math::nint(c_ppi_rng_factor * ( obj_x_cmp + c_ownship_x_ofs ) );
}

int gc_math::calculator::frz_ppi_pix_y_os_rel(float range, float az)
{
  float a=cosf(az+frz_azimuth);
  float obj_y_cmp = range * a;
  return Y_ZERO_RNG_PPI-gc_math::nint(c_ppi_rng_factor * ( obj_y_cmp + c_ownship_y_ofs ) );
}

void gc_math::calculator::frz_ppi_pix_xy_os_rel(int& x, int& y, float range, float az)
{
  x=frz_ppi_pix_x_os_rel(range, az);
  y=frz_ppi_pix_y_os_rel(range, az);
}

int gc_math::calculator::exp_ppi_pix_x(float range, float az)
{
  int x_un_dis=ppi_pix_x(range, az);
  return X_CENTRE_DISPL+exp_ratio*(x_un_dis-exp_x_cent);
}

int gc_math::calculator::exp_ppi_pix_y(float range, float az)
{
  int y_un_dis=ppi_pix_y(range, az);
  return Y_CENTRE_DISPL+exp_ratio*(y_un_dis-exp_y_cent);
}

void gc_math::calculator::exp_ppi_pix_xy(int& x, int& y, float range, float az)
{
  x=exp_ppi_pix_x(range, az);
  y=exp_ppi_pix_y(range, az);
}

int gc_math::calculator::frz_exp_ppi_pix_x(float range, float az)
{
  int x_un_dis=frz_ppi_pix_x(range, az);
  return X_CENTRE_DISPL+exp_ratio*(x_un_dis-exp_x_cent);
}

int gc_math::calculator::frz_exp_ppi_pix_y(float range, float az)
{
  int y_un_dis=frz_ppi_pix_y(range, az);
  return Y_CENTRE_DISPL+exp_ratio*(y_un_dis-exp_y_cent);
}

void gc_math::calculator::frz_exp_ppi_pix_xy(int& x, int& y, float range, float az)
{
  x=frz_exp_ppi_pix_x(range, az);
  y=frz_exp_ppi_pix_y(range, az);
}

int gc_math::calculator::exp_ppi_pix_qmy()
{
  return gc_math::nint(c_ppi_len_factor*exp_ratio*gc_math::nm_to_meters(0.25f));
}

void gc_math::calculator::frz_ac_loc_pix_rot_xy(int &x, int &y)
{
  gc_math::rot_xy_ccw(x, y, frz_sa_ac, frz_ca_ac);
}

void gc_math::calculator::frz_trkv_loc_pix_rot_xy(int &x, int &y, float val_ang)
{
  float ang_rot = val_ang + frz_azimuth - gc_math::k_pi;
  float sin_ang = sinf(-ang_rot);
  float cos_ang = cosf(-ang_rot);
  gc_math::rot_xy_ccw(x, y, sin_ang, cos_ang);
}

float gc_math::calculator::nav_rng_ppi(float x, float y)
{
  return nav_rng_ppi_fact(x, y, c_pix_rng_ppi_factor);
}

float gc_math::calculator::nav_az_ppi(float x, float y)
{
  return gc_math::nav_az_ppi(x, y, ref_azimuth);
}

float gc_math::calculator::nav_rng_b(float y)
{
  return nav_rng_b_fact(y, c_pix_rng_b_factor);
}

float gc_math::calculator::nav_vel_b(float y)
{
  return nav_rng_b_fact(y, c_pix_vel_b_factor);
}

float gc_math::calculator::nav_az_b(float x)
{
  return gc_math::nav_az_b(x, ref_azimuth);
}

//Geographic
void gc_math::calculator::geo_to_rngaz(float& range, float& azimth,  float latitude, float longitude)
{
	constexpr float R_WGS84 = 3.44391556916807361962e+03; //*GC_K_NM_TO_METERS; //{-- NM -> meters--}
	constexpr float F_WGS84 = 3.35281068231769422166749745962564e-03; //*GC_K_NM_TO_METERS;

	const float th=ref_true_heading;
	const float psi=ref_azimuth;

	const float phi_av=ref_latitude;
	const float lambda_av=ref_longitude;

	const float p_phi=latitude;
	const float p_lambda=longitude;

	float d_phi=p_phi-phi_av;
	float d_lambda=p_lambda-lambda_av;

	float sin_phi_av=sinf(phi_av);
	float cos_phi_av=cosf(phi_av);
	//float sin_lambda_av=sinf(lambda_av);

	float r_east=R_WGS84*( 1 + F_WGS84*sin_phi_av*sin_phi_av );
	float r_north=R_WGS84*( 1 - F_WGS84*( 2 - 3*sin_phi_av*sin_phi_av ) );
	float alpha_w=th- psi;
	float atan_az=atan2f(d_lambda*r_east*cos_phi_av, d_phi*r_north);

	float az=alpha_w-atan_az;

	azimth=gc_math::angle_bound_pi(az);
	float rng=sqrtf(d_phi*d_phi*r_north*r_north + d_lambda*d_lambda*r_east*r_east*cos_phi_av*cos_phi_av );
	range=rng*GC_K_NM_TO_METERS;
#if 0
  var r_east, r_north, sin_phi_av, cos_phi_av, sin_lambda_av;
  var d_phi, d_lambda, phi_av, lambda_av, alpha_w, atan_az;

begin
{  phi_av    := ( p.phi + n.phi )/2;}
  phi_av    := n.phi;
{  lambda_av := ( p.lambda + n.lambda )/2;}
  lambda_av := n.lambda;
  d_phi     := p.phi - n.phi;
  d_lambda  := p.lambda - n.lambda;
  rC        := xsin( sin_phi_av, phi_av );
  rC        := xcos( cos_phi_av, phi_av );
  rC        := xsin( sin_lambda_av, lambda_av );
  r_east    := R_WGS84*( 1 + F_WGS84*sin_phi_av*sin_phi_av );
  r_north   := R_WGS84*( 1 - F_WGS84*( 2 - 3*sin_phi_av*sin_phi_av ) );

  alpha_w   := th {in_app.true_heading} - psi;
  rC        := xatan2( atan_az, d_lambda*r_east*cos_phi_av, d_phi*r_north );
  p.rng     := sqrt( d_phi*d_phi*r_north*r_north +
                 d_lambda*d_lambda*r_east*r_east*cos_phi_av*cos_phi_av );
  p.az      := alpha_w - atan_az;
  while ( p.az < -PI ) do p.az := p.az + 2*PI;
  while ( p.az >= PI ) do p.az := p.az - 2*PI;
end;
#endif
}

void gc_math::calculator::set_reference_true_heading(float true_heading_rad)
{
	ref_true_heading=true_heading_rad;
}

void gc_math::calculator::set_geographic_reference(float latitude, float longitude)
{
	ref_latitude=latitude;
	ref_longitude=longitude;
}

bool gc_math::calculator::geo_ppi_pix_xy(int& x, int& y, float latitude, float longitude)
{
	float rng;
	float az;
	geo_to_rngaz(rng, az, latitude, longitude);
	ppi_pix_xy(x, y, rng, az);
	return true;
}

bool gc_math::calculator::geo_bscan_pix_xy(int& x, int& y, float latitude, float longitude)
{
	float rng;
	float az;
	geo_to_rngaz(rng, az, latitude, longitude);
	x=az_pix_nav(az);
	y=rng_pix(rng);
	return true;
}


//Geo Image referenced

void gc_math::calculator::set_image_reference(float latitude, float longitude, float patch_len, float ground_range, float azimtuh)
{
	img_latitude=latitude;
	img_longitude=longitude;
	img_range=ground_range;
	img_azimuth=azimtuh;
	img_len=patch_len;
}

void gc_math::calculator::geoimg_to_xy(
		int& x_pixel, int& y_pixel,
		float latitude, float longitude,
		float patch_l, float image_g_range, float image_az, float image_latitude, float image_longitude,
		bool nose_oriented)
{
	constexpr float R_WGS84_M = 6378131.6341;
	constexpr float F_WGS84_M = 0.0033528;

	const int NUM_X=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;
	const int NUM_Y=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;
	const int HNUM_X=NUM_X/2;
	const int HNUM_Y=NUM_Y/2;

	const float AlphaW=gc_math::Calc.ref_azimuth-gc_math::Calc.ref_true_heading;

	float LATavg=(image_latitude+latitude)/2;
	//float LONavg=(image_longitude+longitude)/2; -- TODO: warning not used to be verified!!!

	float DLat=latitude-image_latitude;
	float DLon=longitude-image_longitude;

	float sin_LATavg=sinf(LATavg);
	float sin2_LATavg=sin_LATavg*sin_LATavg;

	float cos_LATavg=cosf(LATavg);
	float cos2_LATavg=cos_LATavg*cos_LATavg;

	float REast=R_WGS84_M*(1+F_WGS84_M*sin2_LATavg);
	float RNorth=R_WGS84_M*( 1 - F_WGS84_M*( 2 - 3*sin2_LATavg) );

	float RNG=sqrtf((DLat*DLat)+((DLon*DLon)*(REast*REast)*cos2_LATavg));
	float PHI=AlphaW-atan2f(DLon*REast*cos_LATavg, DLon*RNorth);

	float sin_az=sinf(image_az);
	float cos_az=cosf(image_az);

	float x=RNG*cosf(PHI)-image_g_range*cos_az;
	float y=RNG*sinf(PHI)-image_g_range*sin_az;

	if (nose_oriented)
	{
		sin_az=sinf(gc_math::Calc.ref_azimuth);
		cos_az=cosf(gc_math::Calc.ref_azimuth);
	}

	x=-x*sin_az-y*cos_az;
	y= x*cos_az-y*sin_az;

	x=HNUM_X+NUM_X*(x/patch_l);
	y=HNUM_Y+NUM_Y*(y/patch_l);

	x_pixel=x-0.5;
	y_pixel=y-0.5;
}
