#ifndef GC_ALGORITHMS_H_
#define GC_ALGORITHMS_H_

#include "gc_math.h"
#include "gc_config.h"

namespace gc_math
{


unsigned long gray_to_packed_rgb(unsigned int gray);
unsigned int rgb_to_gray(unsigned int r, unsigned int g, unsigned b);

inline bool is_odd(int x) { return (x & 0x1)!=0;}
inline bool is_even(int x) { return !is_odd(x);}

inline bool is_odd(unsigned int x) { return (x & 0x1)!=0;}
inline bool is_even(unsigned int x) { return !is_odd(x);}

enum scan_dir_t { scan_dir_left_to_right=-1, scan_dir_right_to_left=1};

inline scan_dir_t scan_dir(int bar_number)
{
  return (gc_math::is_odd(bar_number)) ? scan_dir_left_to_right : scan_dir_right_to_left;
}

inline int scan_dir_int(int bar_number)
{
  return int(scan_dir(bar_number));
}

int az_pix_body(float az);
int az_pix_body_half_scan_sector(float az, bool is_left_sector);
int az_pix_nav(float az, float plat_az);
float az_subpix_nav(float az, float plat_az);
int el_pix_body(float el);
int el_pix_nav(float el);
int rng_pix(float range, float range_scale);
int ppi_pix_x(float range, float range_scale, float az, float ptaz);
int ppi_pix_y(float range, float range_scale, float az, float ptaz);

void ppi_pix_xy(int* x, int* y, float range, float range_scale, float az, float ptaz);

float ppi_rng_radius(float scale, float range);

void rot_xy_ccw(int &x, int &y, float sa, float ca);

//Pixel BScan/PPI to Navigation range/azimuth
float nav_rng_ppi(float x, float y, float range_scale);
float nav_az_ppi(float x, float y, float az_ref);
float nav_rng_b(float y, float range_scale);
float nav_az_b(float x, float az_ref);
float nav_el_b(float y);

//HR
void sar_patch(float &rng0, float &rng1, float &daz0, float &daz1, float size, float range);

class calculator
{
public:
  static int x_to_abs(int x) { return x+gc_cfg::X_MOVWIN_OFFSET;}
  static int y_to_abs(int y) { return y+gc_cfg::Y_MOVWIN_OFFSET;}
  static int x_to_mov(int x) { return x-gc_cfg::X_MOVWIN_OFFSET;}
  static int y_to_mov(int y) { return y-gc_cfg::Y_MOVWIN_OFFSET;}

  void recalculate_reference();

  void set_reference(float reference_azimth_rad, float range_scale_mt, float velocity_scale_ms);
  void set_reference_az(float reference_azimth_rad);
  void set_reference_rng(float range_scale_mt);
  void set_reference_vel(float velocity_scale_ms);
  void set_frozen_az(float frozen_azimth_rad);
  void set_reference_ownship(float ownship_azimth_rad, float ownship_range_m);

  float get_reference_az_rad() const { return ref_azimuth; };
  float get_reference_rng_mt() const { return ref_range_scale; };
  float get_reference_vel_ms() const { return ref_velocity_scale; };

  float platform_aizmuth() const { return ref_azimuth;}
  float range_scale() const { return ref_range_scale;}
  float vel_scale() const { return ref_velocity_scale;}
  float frozen_aizmuth() const { return frz_azimuth;}

  //B-SCAN Body Reference
  int az_pix_body(float az) const { return gc_math::az_pix_body(az);}
  int el_pix_body(float el) const { return gc_math::el_pix_body(el);}
  // This function is similar to az_pix_body but the scale is so that the whole x width
  // of the display cover half the nominal scan with. The center is also shifted left or
  // right so that the right/left half of the scan volume is shown.
  int az_pix_body_half_scan_sector(float az, bool is_left_sector) const { return gc_math::az_pix_body_half_scan_sector(az, is_left_sector);}

  //B-SCAN Navigation
  int az_pix_nav(float az) const { return gc_math::az_pix_nav(az, ref_azimuth);}
  float az_subpix_nav(float az) const { return gc_math::az_subpix_nav(az, ref_azimuth);}
  int el_pix_nav(float el) const { return gc_math::el_pix_nav(el);}

  //B-SCAN nav/body
  int rng_pix(float range) const { return gc_cfg::Y_ZERO_RANGE-gc_math::nint(c_rng_pix_factor*range);}
  float rng_subpix(float range) const { return float(gc_cfg::Y_ZERO_RANGE)-(c_rng_pix_factor*range);}
  int vel_pix(float velocity) const { return gc_cfg::Y_ZERO_RANGE-gc_math::nint(c_vel_pix_factor*velocity);}

  //Navigation PPI
  int ppi_pix_x(float range, float az);
  int ppi_pix_y(float range, float az);
  void ppi_pix_xy(int& x, int& y, float range, float az);
  void rng_az_ppi_pix_xy(int& x, int& y, float range, float az);
  void rng_az_ppi_subpix_xy(float& x, float& y, float range, float az);
  void vel_az_ppi_pix_xy(int& x, int& y, float velocity, float az);

  //Freeze
  int frz_ppi_pix_x(float range, float az);
  int frz_ppi_pix_y(float range, float az);
  void frz_ppi_pix_xy(int& x, int& y, float range, float az);
  int frz_ppi_pix_x_os_rel(float range, float az);
  int frz_ppi_pix_y_os_rel(float range, float az);
  void frz_ppi_pix_xy_os_rel(int& x, int& y, float range, float az);
  int frz_az_pix_nav(float az) const { return gc_math::az_pix_nav(az, frz_azimuth);}

  //Expand
  void set_field_of_view(int expand_factor, int virtual_x, int virtual_y, float ref_x, float ref_y, float field_platform_azimuth);
  int exp_ppi_pix_x(float range, float az);
  int exp_ppi_pix_y(float range, float az);
  void exp_ppi_pix_xy(int& x, int& y, float range, float az);
  int exp_ppi_pix_qmy();
  int exp_raw_pixel_x(float x) const;
  int exp_raw_pixel_y(float y) const;

  //FreezeExpanded
  int frz_exp_ppi_pix_x(float range, float az);
  int frz_exp_ppi_pix_y(float range, float az);
  void frz_exp_ppi_pix_xy(int& x, int& y, float range, float az);

  //Pixel rotations related to ALPHA_AC angle
  void frz_ac_loc_pix_rot_xy(int &x, int &y);
  //Pixel rotations related to ALPHA_STT angle
  void frz_trkv_loc_pix_rot_xy(int &x, int &y, float vel_ang);

  //Pixel BScan/PPI to Navigation range/azimuth
  float nav_rng_ppi(float x, float y);
  float nav_az_ppi(float x, float y);
  float nav_rng_b(float y);
  float nav_vel_b(float y);
  float nav_az_b(float x);

  //Scan Direction
  void set_bar_number(int bar_number);

  scan_dir_t scan_dir() const { return c_scan_dir;}
  bool is_bar_odd() const { return c_bar_odd;}
  bool is_bar_even() const { return c_bar_even;}

  //Geographic
  void set_reference_true_heading(float true_heading_rad);

  void set_geographic_reference(float latitude, float longitude);
  bool geo_ppi_pix_xy(int& x, int& y, float latitude, float longitude);
  bool geo_bscan_pix_xy(int& x, int& y, float latitude, float longitude);
  void geo_to_rngaz(float& range, float& azimth,  float latitude, float longitude);

  void set_image_reference(float latitude, float longitude, float patch_len, float ground_range, float azimtuh);

  void 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);

public: //protected:
  float ref_azimuth;
  float ref_range_scale;
  float ref_velocity_scale;

  float c_rng_pix_factor;
  float c_vel_pix_factor;
  float c_ppi_rng_factor; // Used to evaluate delta pixels to go x to x1
  float c_ppi_len_factor; // Used to evaluate length of pixels segments x to x1
  float c_ppi_vel_factor;

  float c_pix_rng_ppi_factor;
  float c_pix_rng_b_factor;
  float c_pix_vel_b_factor;

  int exp_factor;
  float exp_ratio;
  float exp_virtual_x;
  float exp_virtual_y;
  int exp_ref_x;
  int exp_ref_y;
  float field_ptaz;
  float exp_x_cent;
  float exp_y_cent;

  float frz_azimuth;
  float frz_sa_ac;
  float frz_ca_ac;

  float c_ownship_x_ofs;
  float c_ownship_y_ofs;

  int ref_bar_number;
  scan_dir_t c_scan_dir;
  bool c_bar_odd;
  bool c_bar_even;

  //Geo
  float ref_true_heading;
  float ref_longitude;
  float ref_latitude;

  //Image
  float img_azimuth;
  float img_range;
  float img_latitude;
  float img_longitude;
  float img_len;
};

extern calculator Calc;


} /*namespace*/

#endif /*GC_ALGORITHMS_H_*/
