#include "LibItsCommon_Functions.hh"
//#include "ITS_Container.hh"
#include <chrono>
#include <math.h>

#ifndef M_PI
#define M_PI	3.14159265358979323846
#endif

namespace LibItsCommon__Functions 
{

  /**
   * @desc    This external function gets the current time
   * @return  Timestamp - current time since 01/01/2014 in milliseconds
   * @see     fx_getCurrentTime() return TimestampIts
   */
  INTEGER fx__getCurrentTime(
) {
    unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L;  // TODO Add a global method such as its_tme() & its_time_mod() beacuse it is used also in geonetworking_layer
    /*unsigned long long its_ref_time = 1072911600000L;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    unsigned long long ms = tv.tv_sec * 1000 + tv.tv_usec / 1000;*/
    INTEGER i;
    i.set_long_long_val(ms);
    return i;
  }
  /**
   * @desc    Gets the current time since 01/01/2004
   * @return  TimeMark - tenths of a second in the current or next hour in units of 1/10th second from UTC time
   * @see     function f_getCurrentTimeMark() return TimeMark
   */
  INTEGER fx__getCurrentTimeMark(
) {
    //	TODO: this is just a sceleton. fill in the function	
    return 0;
  }

  /**
   * @desc    Gets the Minute of current UTC year
   * @return  MinuteOfTheYear - tenths of a second in the current or next hour in units of 1/10th second from UTC time
   * @see     function f_getMinuteOfTheYear() return MinuteOfTheYear
   */
  INTEGER fx__getMinuteOfTheYear(
) {
    //	TODO: this is just a sceleton. fill in the function	
    return 0;
  }

  /**
   * @desc    Gets the milliseconds point in the current UTC minute
   * @return  DSecond - The milliseconds point in the current UTC minute (0..60000)
   * @see     function f_getDSecond() return DSecond
   */
  INTEGER fx__getDSecond(
) {
    //	TODO: this is just a sceleton. fill in the function	
    return 0;
  }

  /*       * @desc    External function to compute distance between two points
   * @param   p_latitudeA   Latitude of first point
   * @param   p_longitudeA  Longitude of first point
   * @param   p_latitudeB   Latitude of second point
   * @param   p_longitudeB  Longitude of second point
   * @return  Computed distance in meters
   fx_computeDistance(in Int32 p_latitudeA, in Int32 p_longitudeA, in Int32 p_latitudeB, in Int32 p_longitudeB) return float;
  */
  FLOAT fx__computeDistance(
                            const INTEGER& p__latitudeA,
                            const INTEGER& p__longitudeA,
                            const INTEGER& p__latitudeB,
                            const INTEGER& p__longitudeB
                            ) {
    double d_latA = ((double)p__latitudeA)/10000000.0;
    double d_latB = ((double)p__latitudeB)/10000000.0;

    double d_lonA = ((double)p__longitudeA)/10000000.0;
    double d_lonB = ((double)p__longitudeB)/10000000.0;

    double earth_radius = 6371000.0; //meters

    double d_lat = (d_latB  - d_latA) * (M_PI/180.0);
    double d_lon = (d_lonB - d_lonA) * (M_PI/180.0);
  
    double a = sin(d_lat/2)*sin(d_lat/2) + cos(d_latA*M_PI/180.0)*cos(d_latB*M_PI/180.0)*sin(d_lon/2)*sin(d_lon/2);
    double c = 2*atan2(sqrt(a), sqrt(1-a));

    return FLOAT(earth_radius*c);
  }

  /*       * @desc    External function to compute a position using a reference position, a distance and an orientation 
   * @param   p_iutLongPosVector  Reference position
   * @param   p_distance          Distance to the reference position (in meter)
   * @param   p_orientation       Direction of the computed position (0 to 359; 0 means North)
   * @param   p_latitude          Computed position's latitude
   * @param   p_longitude         Computed position's longitude
   fx_computePositionUsingDistance(in Int32 p_refLatitude,in Int32 p_refLongitude,in float p_distance,in integer p_orientation,out Int32 p_latitude,out Int32 p_longitude);
  */
  void fx__computePositionUsingDistance(
                                        const INTEGER& p__refLatitude,
                                        const INTEGER& p__refLongitude,
                                        const FLOAT& p__distance,
                                        const INTEGER& p__orientation,
                                        INTEGER& p__latitude,
                                        INTEGER& p__longitude
                                        ) {
    double distance = ((double)p__distance) / 6371000.0;
    double angle = ((double)p__orientation) * (M_PI / 180.0);

    double ref_lat = ((double)p__refLatitude) * (M_PI / 180.0);
    //    double ref_lon = ((double)p__refLongitude) * (M_PI / 180.0);

    p__latitude = asin(sin(ref_lat)*cos(distance) + cos(ref_lat)*sin(distance)*cos(angle)) * 180.0 / M_PI;
    p__longitude = ((double)p__refLongitude) + atan2(sin(angle)*sin(distance)*cos(ref_lat), cos(distance) - sin(ref_lat)*sin((double)p__latitude))*(180.0/M_PI);
  }

  /*       * @desc    External function to compute radius of a given circular area
   * @param   p_squareMeters  Square meters of an circular area
   * @return  Computed radius in meters
   fx_computeRadiusFromCircularArea(in float p_squareMeters) return float;
  */
  FLOAT fx__computeRadiusFromCircularArea(
                                          const FLOAT& p__squareMeters
                                          ) {
    return FLOAT(sqrt(p__squareMeters/M_PI));
  }

} // end of Namespace