

#include "gc_algorithms.h"

#include "gc_referencesystems.h"

#include "GcObject.h"

#if __cplusplus <= 199711L
#define override
#endif

IGcReferenceSystem::~IGcReferenceSystem()
{
}

void IGcReferenceSystem::rng_az_el_pix_xy(int& x, int& y, float range, float az, float /*el*/) const
{
	return rng_az_pix_xy(x, y, range, az);
}

typedef GcObjectList<GcReferenceSystem> RfObjList;

static RfObjList rfObjList("refsys", "rs");


GcReferenceSystem::GcReferenceSystem(const char* const name):
  GcObject(0, name, &rfObjList)
{
}

GcReferenceSystem::~GcReferenceSystem()
{
}

void GcReferenceSystem::pix_xy(int& x, int& y, float range, float az) const
{
  pix_xy(x, y, range, az, reference_azimuth(), reference_range());
}
void GcReferenceSystem::pix_xy(int& x, int& y, float range, float az, float refaz) const
{
  pix_xy(x, y, range, az, refaz, reference_range());
}
void GcReferenceSystem::pix_xy(int& x, int& y, float /*range*/, float /*az*/, float /*refaz*/, float /*scale*/) const
{
  // Dummy
  x = 0;
  y = 0;
}
void GcReferenceSystem::pix_xy_os_rel(int& x, int& y, float range, float az) const
{
  // Dummy
  //x = 0;
  //y = 0;
  rng_az_pix_xy(x, y, range, az);
}
int GcReferenceSystem::pix_x(float /*range*/, float /*az*/) const
{
  // Dummy
  return 0;
}
int GcReferenceSystem::pix_y(float /*range*/, float /*az*/) const
{
  // Dummy
  return 0;
}
float GcReferenceSystem::reference_azimuth() const
{
  return gc_math::Calc.platform_aizmuth();
}
float GcReferenceSystem::reference_range() const
{
  return gc_math::Calc.range_scale();
}
float GcReferenceSystem::reference_vel() const
{
    return gc_math::Calc.vel_scale();
}
int GcReferenceSystem::pix_to_pix_x(float x) const
{
  return gc_math::Calc.exp_raw_pixel_x(x);
}
int GcReferenceSystem::pix_to_pix_y(float y) const
{
  return gc_math::Calc.exp_raw_pixel_y(y);
}
int GcReferenceSystem::az_pix(float /*az*/) const
{
  //Dummy
  return 0;
}
int GcReferenceSystem::az_pix_half_scan_sector(float /*az*/, bool /*is_left_sector*/) const
{
  //Dummy
  return 0;
}
int GcReferenceSystem::el_pix(float /*el*/) const
{
  //Dummy
  return 0;
}
void GcReferenceSystem::rng_az_pix_xy(int& x, int& y, float /*range*/, float /*az*/) const
{
  // Dummy
  x = 0;
  y = 0;
}
void GcReferenceSystem::rng_az_subpix_xy(float& x, float& y, float /*range*/, float /*az*/) const
{
  // Dummy
  x = 0;
  y = 0;
}
void GcReferenceSystem::vel_az_pix_xy(int& x, int& y, float /*velocity*/, float /*az*/) const
{
  // Dummy
  x = 0;
  y = 0;
}
void GcReferenceSystem::rng_pix_y(int& y, float range) const
{
  y=rng_pix_y(range);
}
int GcReferenceSystem::rng_pix_y(float /*range*/) const
{
  // Dummy
  return 0;
}
int GcReferenceSystem::rng_pix_radius(float /*range*/, float /*scale*/) const
{
  // Dummy
  return 0;
}
int GcReferenceSystem::len_pix_qmy() const
{
  // Dummy
  return 0;
}
void GcReferenceSystem::pix_ac_rot_xy(int x, int y, int &rotX, int &rotY) const
{
  // Dummy
  rotX = x;
  rotY = y;
}
void GcReferenceSystem::pix_trkv_rot_xy(int x, int y, int &rotX, int &rotY, float trkv_ang) const
{
  // Dummy
  rotX = x;
  rotY = y;
  float ang_rot = trkv_ang + reference_azimuth() - gc_math::k_pi;
  float sin_ang = sinf(-ang_rot);
  float cos_ang = cosf(-ang_rot);
  gc_math::rot_xy_ccw(rotX, rotY, sin_ang, cos_ang);
}

void GcReferenceSystem::nav_rng_az(float &range, float &az, float /*x*/, float /*y*/) const
{
  // Dummy
  range = 0;
  az = 0;
}

void GcReferenceSystem::nav_rng_az(float &range, float &az, float /*x*/, float /*y*/, float /*az_ref*/) const
{
  // Dummy
  range = 0;
  az = 0;
}

void GcReferenceSystem::nav_rng_vel_az(float &range, float &velocity, float &az, float /*x*/, float /*y*/) const
{
  // Dummy
  range = 0;
  velocity = 0;
  az = 0;
}

void GcReferenceSystem::nav_az_el(float &az, float &el, float /*x*/, float /*y*/) const
{
  // Dummy
  az = 0;
  el = 0;
}

bool GcReferenceSystem::geo_pix_xy(int& x, int& y, float latitude, float longitude) const
{
	return gc_math::Calc.geo_bscan_pix_xy(x, y, latitude, longitude);
}

using namespace gc_math;

class RsBodyBScan: public GcReferenceSystem
{
public:
  RsBodyBScan(const char* name="BodyBS"):
    GcReferenceSystem(name)
  {
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
    x=Calc.az_pix_body(az);
    y=Calc.rng_pix(range);
  }
  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/) const override
  {
    pix_xy(x, y, range, az);
  }
  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/, float /*scale*/) const override
  {
    pix_xy(x, y, range, az);
  }

  virtual int pix_x(float /*range*/, float az) const override
  {
    return Calc.az_pix_body(az);
  }

  virtual int pix_y(float range, float /*az*/) const override
  {
    return Calc.rng_pix(range);
  }

  virtual float reference_azimuth() const override
  {
    return 0;
  }

  virtual void rng_az_pix_xy(int& x, int& y, float range, float az) const override
  {
    x = Calc.az_pix_body(az);
    y = Calc.rng_pix(range);
  }

  virtual int az_pix(float az) const override
  {
    return gc_math::Calc.az_pix_body(az);
  }

  virtual int az_pix_half_scan_sector(float az, bool is_left_sector) const override
  {
    return gc_math::Calc.az_pix_body_half_scan_sector(az, is_left_sector);
  }

  virtual int el_pix(float el) const override
  {
    return gc_math::Calc.el_pix_body(el);
  }
};

class RsBodyPPI: public GcReferenceSystem
{
public:
  RsBodyPPI(const char* name="BodyPPI"):
    GcReferenceSystem(name)
  {
  }
};

class RsNavigationPPI: public GcReferenceSystem
{
public:
  RsNavigationPPI(const char* name="NavPPI"):
    GcReferenceSystem(name)
  {
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
    Calc.ppi_pix_xy(x, y, range, az);
  }
  virtual void pix_xy(int& x, int& y, float range, float az, float refaz) const override
  {
    gc_math::ppi_pix_xy(&x, &y, range, Calc.range_scale(), az, refaz);
  }
  virtual void pix_xy(int& x, int& y, float range, float az, float refaz, float scale) const override
  {
    gc_math::ppi_pix_xy(&x, &y, range, scale, az, refaz);
  }

  virtual int pix_x(float range, float az) const override
  {
    return Calc.ppi_pix_x(range, az);
  }

  virtual int pix_y(float range, float az) const override
  {
    return Calc.ppi_pix_y(range, az);
  }

  virtual int az_pix(float az) const override
  {
    return gc_math::Calc.az_pix_nav(az);
  }

  virtual int el_pix(float el) const override
  {
    return gc_math::Calc.el_pix_nav(el);
  }

  virtual void rng_az_pix_xy(int& x, int& y, float range, float az) const override
  {
	Calc.rng_az_ppi_pix_xy(x, y, range, az);
  }

  virtual void rng_az_subpix_xy(float& x, float& y, float range, float az) const override
  {
    Calc.rng_az_ppi_subpix_xy(x, y, range, az);
  }

  virtual void vel_az_pix_xy(int& x, int& y, float velocity, float az) const override
  {
	Calc.vel_az_ppi_pix_xy(x, y, velocity, az);
  }

  virtual int rng_pix_y(float range) const override
  {
	  return Calc.ppi_pix_y(range, Calc.platform_aizmuth());
  }

  void rng_pix_y(int& y, float range) const override
  {
    y=rng_pix_y(range);
  }


  virtual int rng_pix_radius(float range, float scale) const override
  {
    return gc_math::nint(gc_math::ppi_rng_radius(scale,range));
  }

  virtual void nav_rng_az(float &range, float &az, float x, float y) const override
  {
    range = Calc.nav_rng_ppi(x, y);
    az = Calc.nav_az_ppi(x, y);
  }

  virtual void nav_rng_az(float &range, float &az, float x, float y, float az_ref) const override
  {
    range = Calc.nav_rng_ppi(x, y);
    az = gc_math::nav_az_ppi(x, y, az_ref);
  }

  virtual bool geo_pix_xy(int& x, int& y, float latitude, float longitude) const override
  {
  	return gc_math::Calc.geo_ppi_pix_xy(x, y, latitude, longitude);
  }

};

class RsNavigationAAPPI: public RsNavigationPPI
{
public:
	RsNavigationAAPPI(const char* name="NavAAPPI"):
		RsNavigationPPI(name)
	{

	}

	static inline float adjustSlantRange(float rng, float el)
	{
		return rng*cosf(el);
	}

	virtual void rng_az_el_pix_xy(int& x, int& y, float range, float az, float el) const override
	{
		rng_az_pix_xy(x, y, adjustSlantRange(range, el), az);
	}
};

class RsExpNavigationPPI: public RsNavigationPPI
{
public:
  RsExpNavigationPPI(const char* name="ExpNavPPI"):
    RsNavigationPPI(name)
  {
  }

  virtual int len_pix_qmy() const override
  {
    return gc_math::Calc.exp_ppi_pix_qmy();
  }
};

class RsFrzNavigationPPI: public RsNavigationPPI
{
public:
  RsFrzNavigationPPI(const char* name="FrzNavPPI"):
    RsNavigationPPI(name)
  {
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
    gc_math::Calc.frz_ppi_pix_xy(x, y, range, az);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/) const override
  {
      pix_xy(x, y, range, az); //reference is fixed in freeze
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/, float /*scale*/) const override
  {
      pix_xy(x, y, range, az); //reference is fixed in freeze //gc_math::ppi_pix_xy(&x, &y, range, scale, az, refaz);
  }


  virtual void pix_ac_rot_xy(int x, int y, int &rotX, int &rotY) const override
  {
    rotX = x;
    rotY = y;
    Calc.frz_ac_loc_pix_rot_xy(rotX, rotY);
  }

  virtual void pix_trkv_rot_xy(int x, int y, int &rotX, int &rotY, float trkv_ang) const override
  {
    rotX = x;
    rotY = y;
    Calc.frz_trkv_loc_pix_rot_xy(rotX, rotY, trkv_ang);
  }

  virtual void pix_xy_os_rel(int& x, int& y, float range, float az) const override
  {
    gc_math::Calc.frz_ppi_pix_xy_os_rel(x, y, range, az);
  }
};

class RsFrzExpNavigationPPI: public RsFrzNavigationPPI
{
public:
  RsFrzExpNavigationPPI(const char* name="FrzExpNavPPI"):
    RsFrzNavigationPPI(name)
  {
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
    gc_math::Calc.frz_exp_ppi_pix_xy(x, y, range, az);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/) const override
  {
      gc_math::Calc.frz_exp_ppi_pix_xy(x, y, range, az);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/, float /*scale*/) const override
  {
      gc_math::Calc.frz_exp_ppi_pix_xy(x, y, range, az);
  }

};

class RsNavigationBScan: public GcReferenceSystem
{
public:
  RsNavigationBScan(const char* name="NavBS"):
    GcReferenceSystem(name)
  {
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
    Calc.ppi_pix_xy(x, y, range, az);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float refaz) const override
  {
    gc_math::ppi_pix_xy(&x, &y, range, Calc.range_scale(), az, refaz);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float refaz, float scale) const override
  {
    gc_math::ppi_pix_xy(&x, &y, range, scale, az, refaz);
  }

  virtual int pix_x(float range, float az) const override
  {
    return Calc.ppi_pix_x(range, az);
  }

  virtual int pix_y(float range, float az) const override
  {
    return Calc.ppi_pix_y(range, az);
  }

  virtual int az_pix(float az) const override
  {
    return gc_math::Calc.az_pix_nav(az);
  }

  virtual int el_pix(float el) const override
  {
    return gc_math::Calc.el_pix_nav(el);
  }

  virtual void rng_az_pix_xy(int& x, int& y, float range, float az) const override
  {
	x = Calc.az_pix_nav(az);
	y = Calc.rng_pix(range);
  }

  virtual void vel_az_pix_xy(int& x, int& y, float velocity, float az) const override
  {
	x = Calc.az_pix_nav(az);
	y = Calc.vel_pix(velocity);
  }

  virtual void rng_az_subpix_xy(float& x, float& y, float range, float az) const override
  {
    x = Calc.az_subpix_nav(az);
    y = Calc.rng_subpix(range);
  }

  virtual int rng_pix_y(float range) const override
  {
	return Calc.rng_pix(range);
  }

  virtual void rng_pix_y(int& y, float range) const
  {
    y=rng_pix_y(range);
  }

  virtual void nav_rng_az(float &range, float &az, float x, float y) const override
  {
    range = Calc.nav_rng_b(y);
    az = Calc.nav_az_b(x);
  }

  virtual void nav_rng_az(float &range, float &az, float x, float y, float az_ref) const override
  {
    range = Calc.nav_rng_b(y);
    az =gc_math::nav_az_b(x, az_ref); //Calc.nav_az_b(x, az_ref);
  }


  virtual void nav_rng_vel_az(float &range, float &velocity, float &az, float x, float y) const override
  {
    range = Calc.nav_rng_b(y);
    velocity = Calc.nav_vel_b(y);
    az = Calc.nav_az_b(x);
  }

  virtual void nav_az_el(float &az, float &el, float x, float y) const override
  {
    az = Calc.nav_az_b(x);
    el = gc_math::nav_el_b(y);
  }
};

class RsFrzNavigationBScan: public RsNavigationBScan
{
public:
  RsFrzNavigationBScan(const char* name="FrzNavBS"):
    RsNavigationBScan(name)
  {
  }

  virtual int az_pix(float az) const override
  {
    return gc_math::Calc.frz_az_pix_nav(az);
  }
};

class RsExpNavigationBScan: public RsNavigationBScan
{
public:
  RsExpNavigationBScan(const char* name="ExpNavBS"):
    RsNavigationBScan(name)
  {
  }
};

class RsFrzExpNavigationBScan: public RsFrzNavigationBScan
{
public:
  RsFrzExpNavigationBScan(const char* name="FrzExpNavBS"):
    RsFrzNavigationBScan(name)
  {
  }
};

class RsPixels: public GcReferenceSystem
{
public:
  RsPixels():
    GcReferenceSystem("Pixels")
  {
  }

  virtual int pix_x(float /*range*/, float az) const override
  {
    return Calc.exp_raw_pixel_x(az);
  }

  virtual int pix_y(float range, float /*az*/) const override
  {
    return Calc.exp_raw_pixel_y(range);
  }
  virtual void pix_xy(int& x, int& y, float range, float az, float /*refaz*/, float /*scale*/) const override
  {
    x=pix_x(range, az);
    y=pix_x(range, az);
  }

  virtual void pix_xy(int& x, int& y, float range, float az, float refaz) const override
  {
      pix_xy(x, y, range, az, refaz, 0);
  }

  virtual void pix_xy(int& x, int& y, float range, float az) const override
  {
      pix_xy(x, y, range, az, 0, 0);
  }


  virtual int az_pix(float az) const override
  {
    return gc_math::Calc.az_pix_body(az);
  }

  virtual int el_pix(float el) const override
  {
    return gc_math::Calc.el_pix_body(el);
  }
};

class RsGeoImage: public RsNavigationPPI
{
public:
	RsGeoImage(): RsNavigationPPI("GeoImage") {}
	virtual bool geo_pix_xy(int& x, int& y, float latitude, float longitude) const override
	{
		//float patch_l, float image_g_range, float image_az, float image_latitude, float image_longitude,

		gc_math::Calc.geoimg_to_xy(x, y, latitude, longitude,
				3*1852, //gc_math::Calc.img_len,
				gc_math::Calc.img_range,
				gc_math::Calc.img_azimuth,
				gc_math::Calc.img_latitude,
				gc_math::Calc.img_longitude,
				false);
		return true;
	}
};

static RsPixels rsPixels;

static RsBodyBScan rsBodyBScan("BodyBS");
static RsBodyPPI rsBodyPPI("BodyPPI");

static RsNavigationBScan rsNavigationBScan("NavBS");
static RsFrzNavigationBScan rsFrzNavigationBScan("FrzNavBS");
static RsExpNavigationBScan rsExpNavigationBScan("ExpNavBS");
static RsFrzExpNavigationBScan rsFrzExpNavigationBScan("FrzExpNavBS");

static RsNavigationBScan rsRotatedNavigationBScan("RtNavBS");
static RsNavigationBScan rsRotatedExpandNavigationBScan("RtExNavBS");

static RsNavigationPPI rsNavigationPPI("NavPPI");
static RsNavigationAAPPI rsNavigationAAPPI("NavAAPPI");
static RsNavigationPPI rsRoatedNavigationPPI("RtNavPPI");
static RsNavigationPPI rsRoatedExpandNavigationPPI("RtExNavPPI");

static RsExpNavigationPPI rsExpNavigationPPI("ExpNavPPI");
static RsFrzNavigationPPI rsFrzNavigationPPI("FrzNavPPI");
static RsFrzExpNavigationPPI rsFrzExpNavigationPPI("FrzExpNavPPI");

static RsGeoImage rsGeoImage;

IGcReferenceSystem& GcReferenceSystems::pixels(rsPixels);

IGcReferenceSystem& GcReferenceSystems::bodyBScan=rsBodyBScan;
IGcReferenceSystem& GcReferenceSystems::bodyPPI=rsBodyPPI;

IGcReferenceSystem& GcReferenceSystems::navBScan=rsNavigationBScan;
IGcReferenceSystem& GcReferenceSystems::navAAPPI=rsNavigationAAPPI;

IGcReferenceSystem& GcReferenceSystems::frzNavBScan=rsFrzNavigationBScan;
IGcReferenceSystem& GcReferenceSystems::expNavBScan=rsExpNavigationBScan;
IGcReferenceSystem& GcReferenceSystems::frzExpNavBScan=rsFrzExpNavigationBScan;

IGcReferenceSystem& GcReferenceSystems::navPPI=rsNavigationPPI;
IGcReferenceSystem& GcReferenceSystems::frzNavPPI=rsFrzNavigationPPI;
IGcReferenceSystem& GcReferenceSystems::expNavPPI=rsExpNavigationPPI;
IGcReferenceSystem& GcReferenceSystems::frzExpNavPPI=rsFrzExpNavigationPPI;

IGcReferenceSystem& GcReferenceSystems::rotatedNavPPI=rsRoatedNavigationPPI;

IGcReferenceSystem& GcReferenceSystems::geoImage=rsGeoImage;

GC_OBJ_MON_DEFINE(GcReferenceSystem)
