#include "LibItsCommon_Functions.hh"
//#include "ITS_Container.hh"
#include <time.h>
#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 timestamp2014 = 1356998400000;
            struct timeval tv;
            gettimeofday(&tv, NULL);

            unsigned long long timestampNow = tv.tv_sec*1000 + tv.tv_usec/1000;

//    i.set_long_long_val(timestampNow - timestamp2014);
//    return i;
            return INTEGER(timestampNow - timestamp2014);
}
/**
  * @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(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
