// calcs.c
//

#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <math.h>
#include <conio.h>
#include <time.h>
#include <string.h>

#define EARTH_RADIUS 6371             // kilometers
#define DEGREES_TO_RADS (3.1415926536 / 180)
#define RADS_TO_DEGREES (180 / 3.1415926536)

extern char* file_name;
extern double missionSpeed;
extern double diveAngle;

// newLatLon function
// calculates new waypoint from current waypoint, heading, distance
//  heading and distance are from the previous waypoint
//
int newLatLon(double currentLat, double currentLon, double headingDeg, double distance, double *newLat, double *newLon)
  {
  double R  = 6371;  //earth radius in kilometers
  double PI = 3.1415926536;
  double headingRad;

  //convert decimal degrees to radians
  double currentLatRad = currentLat * (PI / 180);
  double currentLonRad = currentLon * (PI / 180);
  headingRad = headingDeg * (PI / 180);

  // convert meters distance to kilometers
  distance = distance / 1000;

  *newLat = asin( sin(currentLatRad) * cos(distance/R) + cos(currentLatRad) * sin(distance/R) * cos(headingRad) );
  *newLon = currentLonRad + atan2( sin(headingRad) * sin(distance/R) * cos(currentLatRad), cos(distance/R) - sin(currentLatRad) * sin(*newLat) );

  //convert radians to decimal degrees
  *newLat = *newLat * RADS_TO_DEGREES;
  *newLon = *newLon * RADS_TO_DEGREES;

  return (0);
  }


// dist function returns a double ( distance in meters )
// which is stored in struct WayPoint member distance
//
double dist(double lat1, double lon1, double lat2, double lon2)
{
  double dx, dy, dz;
  lon1 -= lon2;

  // convert decimal degrees to radians
  lon1 *= DEGREES_TO_RADS, lat1 *= DEGREES_TO_RADS, lat2 *= DEGREES_TO_RADS;

  dz = sin(lat1) - sin(lat2);
  dx = cos(lon1) * cos(lat1) - cos(lat2);
  dy = sin(lon1) * cos(lat1);

  // return distance in meters
  return (asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * EARTH_RADIUS)*1000;
}

// heading function returns a double ( heading in decimal degrees )
// which is stored in struct WayPoint member heading
//
double heading(double a, double b, double c, double d)
{
  // a, b, c, d are in decimal degrees

  double head, lon1, lon2, lat1, lat2; // in radians
  double londif;
  double headingDeg, headingDeg2; //decimal degrees

  // convert decimal degrees into radians
  lat1=(a*3.14159)/180;
  lon1=(b*3.14159)/180;
  lat2=(c*3.14159)/180;
  lon2=(d*3.14159)/180;
  londif=lon2-lon1;

  head=atan2((sin(londif)*cos(lat2)),((cos(lat1)*sin(lat2))-(sin(lat1)*cos(lat2)*cos(londif)))) ;

  //convert radians into decimal degrees
  headingDeg=(head*180)/3.14159;
  headingDeg2=0;
  if(headingDeg<=0)
    {
      headingDeg2=headingDeg+360;
      return headingDeg2;
    }
    else
      return headingDeg;  // return decimal degrees
}


// timeToNextWayPoint() function returns time in seconds ( double numSeconds )
// which is stored in struct WayPoint member numSeconds
//
//  USING 1.3 TIMES THE LEG DISTANCE IN METERS
//
double timeToNextWayPoint(double distance)
  {
    return ( distance * 1.00 );  // changed from 1.30 to 1.00 on 26_Sept_14
  }

// construct output filename
//  year_day_time.cfg
//
char* fileName()
{
  time_t now;
//  struct tm *tm_now;

  struct tm *gtm_now;

  int year = 0;
  char year_buf[5];
  char yday_buf[4];
  char hour_buf[3];
  char min_buf[3];

//  char fileName[25];
file_name = malloc(25);

  now = time ( NULL );
//  tm_now = localtime ( &now );

//  printf ( "%s", ctime ( &now ) );
//  printf ( "%s", asctime ( tm_now ) );

//  printf ( "\n\n Year: %d ", ( tm_now->tm_year ) + 1900 );
//  printf ( "\n Day:  %d ", tm_now->tm_yday );
//  printf ( "\n Hour: %d ", tm_now->tm_hour );
//  printf ( "\n Min:  %d \n", tm_now->tm_min );


  gtm_now = gmtime ( &now );


//  printf ( "\n\n Year: %d ", ( gtm_now->tm_year ) + 1900 );
//  printf ( "\n Day:  %d ", gtm_now->tm_yday );
//  printf ( "\n Hour: %d ", gtm_now->tm_hour );
//  printf ( "\n Min:  %d \n", gtm_now->tm_min );

//  printf ( "\n\n---Time Strings---\n\n");

  year = ( gtm_now->tm_year ) + 1900;
  itoa ( year, year_buf, 10);
  // printf ( " Year: %s \n", year_buf );

  itoa ( gtm_now->tm_yday, yday_buf, 10);
  // printf ( " Day: %s \n", yday_buf );

  itoa ( gtm_now->tm_hour, hour_buf, 10);
  // printf ( " Hour: %s \n", hour_buf );

  itoa ( gtm_now->tm_min, min_buf, 10);
  // printf ( " Minute: %s \n\n", min_buf );

//  printf ( "\n\n---filename---\n\n" );
//  getch();

  strcpy ( file_name, "\0" );

//printf("\n %s \n", file_name);
//getch();

  strcat ( file_name, year_buf );
  strcat ( file_name, "_" );
  strcat ( file_name, yday_buf );
  strcat ( file_name, "_" );
  strcat ( file_name, hour_buf );
  strcat ( file_name, min_buf );
  strcat ( file_name, "." );

//printf ( "#  %s", file_name );
//getch();

  return file_name;
}