#include "gc_math.h"

const float gc_math::k_epsilon=FLT_EPSILON;

const float gc_math::k_180deg=180.f;
const float gc_math::k_360deg=360.0f;
const float gc_math::k_90deg=90.0f;

#define K_PIx(x_)  ((x_)*(K_PI))
#define K_PId(x_)  ((x_)/(K_PI))

const float gc_math::k_pi=GC_K_PI;
const float gc_math::k_2_pi=GC_K_2_PI;
const float gc_math::k_4_pi=GC_K_4_PI;
const float gc_math::k_half_pi=GC_K_PI_2;
const float gc_math::k_pi_over_3=GC_K_PI_3;
const float gc_math::k_2_pi_over_3=GC_K_2_PI_3;


const float gc_math::k_rad_to_deg=GC_K_RAD_TO_DEG;
const float gc_math::k_deg_to_rad=GC_K_DEG_TO_RAD;

const float gc_math::k_nm_to_meters=GC_K_NM_TO_METERS;
const float gc_math::k_meters_to_nm=GC_K_METERS_TO_NM;

const float gc_math::k_meters_to_feets=GC_K_METERS_TO_FEETS;
const float gc_math::k_feets_to_meters=GC_K_FEETS_TO_METERS;

const float gc_math::k_ms_to_knots=GC_K_MS_TO_KNOTS;
const float gc_math::k_knots_to_ms=GC_K_KNOTS_TO_MS;

float gc_math::angle_bound_pi(float x)
{
  while(x>k_pi)
    x-=k_2_pi;

  while(x<-k_pi)
    x+=k_2_pi;

  return x;
}

float gc_math::angle_bound_2pi(float x)
{
  while(x>k_2_pi)
    x-=k_2_pi;

  while(x<0)
    x+=k_2_pi;

  return x;
}

bool gc_math::angle_check_pi(float x)
{
  if(x>k_pi)
    return false;

  if(x<-k_pi)
      return false;

  return true;
}

bool gc_math::angle_check_pi_pos(float x)
{
    if(x>k_pi)
        return false;

    if(x<0.0f)
        return false;

    return true;
}

bool gc_math::angle_check_2pi_pos(float x)
{
    if(x>k_2_pi)
        return false;

    if(x<0.0f)
        return false;

    return true;
}

bool gc_math::angle_check_2pi3_pos(float x)
{
    if(x>k_2_pi_over_3)
        return false;

    if(x<0.0f)
        return false;

    return true;
}

bool gc_math::angle_check_pi2(float x)
{
    if(x>k_half_pi)
        return false;

    if(x<-k_half_pi)
        return false;

    return true;
}

float gc_math::angle_bound_180(float x)
{
  while(x>k_180deg)
    x-=k_360deg;

  while(x<-k_180deg)
    x+=k_360deg;

  return x;
}

float gc_math::angle_bound_360(float x)
{
  while(x>k_360deg)
    x-=k_360deg;

  while(x<0)
    x=k_360deg-x;

  return x;
}

int gc_math::angle_bound_int_359(int x)
{
  enum { k_i360 = 360};

  while(x>=k_i360)
    x-=k_i360;

  while(x<0)
    x+=k_i360;

  return x;
}

float gc_math::angle_sum_pi(float a, float b)
{
  float r=a+b;
  return angle_bound_pi(r);
}

float gc_math::angle_sum_2pi(float a, float b)
{
  float r=a+b;
  return angle_bound_2pi(r);
}

float gc_math::angle_dif_pi(float a, float b)
{
  float diff = b - a;

  while (fabs(diff) > k_pi)
  {
    if (diff < 0)
      diff += k_2_pi;
    else
      diff -= k_2_pi;
  }
  return( diff );
}

float gc_math::angle_sum_180(float a, float b)
{
  float r=a+b;
  return angle_bound_180(r);
}

float gc_math::angle_sum_360(float a, float b)
{
  float r=a+b;
  return angle_bound_360(r);
}

void gc_math::rad_to_dms(float angle_rad, int& sign, int& arc_deg, int& arc_min, float& arc_sec)
{
  angle_rad = gc_math::angle_bound_pi(angle_rad);
  sign = gc_math::sign(angle_rad);
  angle_rad *= sign;

  float angle_deg = gc_math::rad_to_deg(angle_rad);

  arc_deg = static_cast<int>(angle_deg);
  angle_deg -= arc_deg;
  angle_deg *= 60.0f;
  arc_min = static_cast<int>(angle_deg);
  angle_deg -= arc_min;
  arc_sec = angle_deg * 60.0f;
  if((arc_sec + 0.005f) >= 60.0f)
  {
    arc_sec -= 0.005f;
  }
}

void gc_math::pol_to_rect(float rng, float az_rad, float el_rad, float& x, float& y, float& z)
{
  float cose = cosf(el_rad);

  x = rng * cose * cosf(az_rad);
  y = rng * cose * sinf(az_rad);
  z = rng * sinf(el_rad);
}

void gc_math::rect_to_pol(float x, float y, float z, float& rng, float& az_rad, float& el_rad)
{
  rng = sqrtf(powf(x,2)+powf(y,2)+powf(z,2));

  az_rad=atan2f(y, x);

  if(gc_math::float_is_zero(rng))
  {
    el_rad = 0.0f;
  }
  else
  {
    el_rad = asinf(z / (rng));
  }
}

float gc_math::dist_from_pol(float rng1, float az1_rad, float el1_rad, float rng2, float az2_rad, float el2_rad)
{
  float x1 = 0;
  float y1 = 0;
  float z1 = 0;
  float x2 = 0;
  float y2 = 0;
  float z2 = 0;

  pol_to_rect(rng1, az1_rad, el1_rad, x1, y1, z1);
  pol_to_rect(rng2, az2_rad, el2_rad, x2, y2, z2);

  float dx2 = pow(x1 - x2,2);
  float dy2 = pow(y1 - y2,2);
  float dz2 = pow(z1 - z2,2);

  return sqrtf(dx2 + dy2 + dz2);
}
