#include "objects.h"
#include <math.h>

extern int media;
static double v1, v2;     /* standard parallels for conical projection (s.b.) */

/* map information */

LOCATION map_origin;

long map_scale;
static long map_zeromeridian;
static double map_width, map_height;
long top, left, bottom, right;
static LOCATION topleft, topright, bottomleft, bottomright;
static double originx, originy;
static double scalefactor;
static double mercatorscale;    /* additional factor for merctor projection */
char map_projection = PROJECT_LAMBERT;   /* projection algorithm to be used */

extern double mapWidthcm, mapHeightcm;   /* declared in map.c */

/*
typedef struct {
  double x, y, z;
} COORD;
*/


/* prototypes for projection functions */

double proj_x_lambert (double latitude, double longitude);
double proj_y_lambert (double latitude, double longitude);
double proj_x_mercator (double latitude, double longitude);
double proj_y_mercator (double latitude, double longitude);
void plane2sphere_mercator (double x, double y, double *latitude, double *longitude);
void plane2sphere_lambert (double x, double y, double *latitude, double *longitude);


double distance2 (LOCATION a, LOCATION b)
{
  double lat1, lat2, long1, long2, dl;
  double alpha = 1 / 297.0;
  double s, s0, sdiva, stemp, cosquad;
  double ae = 6378388;

  /* L. Kiefer: Die Beruecksichtigung der Ellipsoidgestalt
     der Erde in der Flugnavigation */

  lat1  = internal2rad (a.latitude);  
  long1 = internal2rad (a.longitude);
  lat2  = internal2rad (b.latitude);  
  long2 = internal2rad (b.longitude);
  dl    = internal2rad (a.longitude - b.longitude);

  if ((lat1 == lat2) && (long1 == long2))
    return 0;

  if (lat1 == lat2)
    lat1 += 0.000000001;

  if (long1 = long2)
    long1 += 0.000000001;

  cosquad = cos((lat2-lat1)/2);
  cosquad *= cosquad;

  stemp = (2*cosquad-1)*cos(dl/2)*cos(dl/2) - (2*cosquad-1)*sin(dl/2)*sin(dl/2);


/* this made problems, I suspect the pow function of my library:
  stemp = (2*pow(cos((lat2-lat1)/2),2)-1)*pow(cos(dl/2),2) -
    (2*pow(cos((lat2+lat1)/2),2)-1)*pow(sin(dl/2),2);
*/

  s0 = acos(stemp);

/* again, we should avoid use of the power function 
  sdiva = s0 - alpha *
    (pow(sin((lat2+lat1)/2),2)*pow(cos((lat2-lat1)/2),2)*(s0-3*sin(s0))/(1+cos(s0)) +
     pow(cos((lat2+lat1)/2),2)*pow(sin((lat2-lat1)/2),2)*(s0+3*sin(s0))/(1-cos(s0)));
*/

  sdiva = s0 - alpha *
    (sin((lat2+lat1)/2)*sin((lat2+lat1)/2)*cos((lat2-lat1)/2)*
     cos((lat2-lat1)/2)*(s0-3*sin(s0))/(1+cos(s0)) +
     cos((lat2+lat1)/2)*cos((lat2+lat1)/2)*sin((lat2-lat1)/2)*
     sin((lat2-lat1)/2)*(s0+3*sin(s0))/(1-cos(s0)));

  s = sdiva * ae;

  return s/1852;
}


double distance (LOCATION a, LOCATION b)
{
  struct {double x,y,z;} start, dest;
  double chord;
  double angle;

  /* transfer to Cartesian system */

  start.x = EARTH_RADIUS * cos(internal2rad(a.longitude)) 
                         * cos(internal2rad(a.latitude));
  start.y = EARTH_RADIUS * sin(internal2rad(a.longitude)) 
                         * cos(internal2rad(a.latitude));
  start.z = EARTH_RADIUS * sin(internal2rad(a.latitude));

  dest.x = EARTH_RADIUS * cos(internal2rad(b.longitude)) 
                        * cos(internal2rad(b.latitude));
  dest.y = EARTH_RADIUS * sin(internal2rad(b.longitude)) 
                        * cos(internal2rad(b.latitude));
  dest.z = EARTH_RADIUS * sin(internal2rad(b.latitude));

  /* length of the chord between start and destination */

  chord = sqrt ((start.x-dest.x)*(start.x-dest.x) 
        + (start.y-dest.y)*(start.y-dest.y) 
	+ (start.z-dest.z)*(start.z-dest.z));

  angle = acos (1 - (chord*chord)/(2*EARTH_RADIUS*EARTH_RADIUS));
  
  return EARTH_RADIUS * angle / 1.852;   /* result in NM */
}



double truetrack1 (LOCATION a, LOCATION b)
{
  double tt;
  double dlat;
  double d;

  d = distance (a, b);
  dlat = (b.latitude - a.latitude) / 360000;

  if (d)
    {
      tt = 180 * acos(dlat * ONE_DEGREE / d) / PI;
    }
  else
    tt = 0;    /* default value if a==b */

  return tt;
}


double truetrack (LOCATION a, LOCATION b)
{
  double tt;
  long dlat, dlong;
  int offset, factor;
  LOCATION p1, p2, p3;

  p1 = a;
  p2.latitude = b.latitude;
  p2.longitude = a.longitude;
  p3 = b;

  dlat  = b.latitude - a.latitude;
  dlong = b.longitude - a.longitude;


  if (dlat == 0)
    {
      if (dlong > 0) tt = 90;
      if (dlong < 0) tt = 270;
    }
  else
    {
      offset = 0;
      factor = 1;

      if ((dlat < 0) && (dlong > 0))
	{
	  offset =  90;
	  factor = -1;
	}
      if ((dlat < 0) && (dlong < 0)) offset = 180;
      if ((dlat > 0) && (dlong < 0)) 
	{
	  offset = 270;
	  factor = -1;
	}

      dlat  *= (dlat  >= 0) ? 1 : -1;
      dlong *= (dlong >= 0) ? 1 : -1;
      
/*      tt = factor * (180 * atan((double) dlong / (double) dlat) / PI);  */
      tt = factor * (180 * atan(distance(p2, p3) / distance(p1, p2)) / PI);

      if (tt < 0)
	tt += 90;
      tt += offset;
    }

  return tt;      /* not implemented either... */
}



/* convert a string 99.99'99"d into the internal degrees representation */

long deg2num (char *string)
{
  int deg, min, sec;
  long result;

  deg = atoi (string);
  min = atoi (string+3);
  sec = atoi (string+6);

  result = (((deg * 60) + min) * 60 + sec) * 100;

  if ((string[9]=='W') || (string[9]=='S'))
    result *= -1;

  return result;
}



/* convert internal representation to human readable string */

char *internal2string (LOCATION pos)
{
  static char build[25];

  sprintf (build, "%2d.%2d\047%2d\042%c %2d.%2d\047%2d\042%c",
	   (int) (abs(pos.latitude) / 360000),
	   (int) ((abs(pos.latitude) / 6000) % 60),
	   (int) ((abs(pos.latitude) / 100) % 60),
	   (pos.latitude < 0) ? 'S' : 'N',
	   (int) (abs(pos.longitude) / 360000),
	   (int) ((abs(pos.longitude) / 6000) % 60),
	   (int) ((abs(pos.longitude) / 100) % 60),
	   (pos.longitude < 0) ? 'W' : 'E');

  if (build[0] == ' ') build[0] = '0';
  if (build[3] == ' ') build[3] = '0';
  if (build[6] == ' ') build[6] = '0';
  if (build[11] == ' ') build[11] = '0';
  if (build[14] == ' ') build[14] = '0';
  if (build[15] == ' ') build[15] = '0';
  if (build[17] == ' ') build[17] = '0';
  if (build[18] == ' ') build[18] = '0';

  return build;
}


/* convert degrees into radians */

double rad(double deg)

{
  return (deg * PI / 180);
}


/* convert radians into degrees */

double deg(double rad)
{
  return (rad * 180 / PI);
}


/* convert internal repr. into radians */

double internal2rad(long internal)
{
  return (((double) internal) * PI / 64800000);
}


/* convert radians into internal rep. */

long rad2internal (double rad)
{
  return (long) (rad * 64800000 / PI);
}





/*
   conformal conical projection with two standard parallels (Lambert)

   The following two routines require a geographical loaction,
   given in geographical coordinates (radian).
   They return a coordinate pair (x,y) for the position on a two-dimensional
   map on which the given location should be displayed.
   -1 < x < 1;  -1 < y < 1.
   The zero-meridian (the one going through London) will be the line from
   (0,0) to (0, -1).
   If anything goes wrong with these two functions: the mathematical background
   can be found in Josef Hoschek: "Mathematische Grundlagen der Kartographie".

   note: the latitudes of the two standard parallels are stored in global vars
   v1 and v2. It doesn't matter which one is which.
*/


double proj_x (double latitude, double longitude)
{
  switch (map_projection) {
  case PROJECT_LAMBERT:
    return proj_x_lambert (latitude, longitude);
    break;
  case PROJECT_MERCATOR:
    return proj_x_mercator (latitude, longitude);
    break;
  } 
}

double proj_y (double latitude, double longitude)
{
  switch (map_projection) {
  case PROJECT_LAMBERT:
    return proj_y_lambert (latitude, longitude);
    break;
  case PROJECT_MERCATOR:
    return proj_y_mercator (latitude, longitude);
    break;
  } 
}



double proj_x_lambert (double latitude, double longitude)
{
  double x;

  x = 2*(sqrt(cos(v1)*cos(v1)+(sin(v1)-sin(latitude))
       *(sin(v1)+sin(v2)))/(sin(v1)+sin(v2)) )
       *sin((sin(v1)+sin(v2))*longitude/2);

  return x;
}


double proj_y_lambert (double latitude, double longitude)
{
  double y;

  y = -2*(sqrt(cos(v1)*cos(v1)+(sin(v1)-sin(latitude))
	*(sin(v1)+sin(v2)))/(sin(v1)+sin(v2)))
        *cos((sin(v1)+sin(v2))*longitude/2);

  return y;
}



double proj_x_mercator (double latitude, double longitude)
{
  return longitude * mercatorscale;
}



double proj_y_mercator (double latitude, double longitude)
{
  return log(tan(PI/4+latitude/2)) * mercatorscale;    /* log is natural log */
}




/*
   Now, we also need the other way around, i.e. from a given map coordinate
   we'd like to find out what the "real world" coordinate is. The code for
   the reverse lambert projection looks a bit ugly - it was created 
   automatically by Maple...
*/

void plane2sphere (double x, double y, double *latitude, double *longitude)
{
  switch (map_projection) {
  case PROJECT_LAMBERT:
    plane2sphere_lambert (x, y, latitude, longitude);
    break;
  case PROJECT_MERCATOR:
    plane2sphere_mercator (x, y, latitude, longitude);
    break;
  } 
}

void plane2sphere_mercator (double x, double y, double *latitude, double *longitude)
{
  *latitude = 2*atan(exp(y/mercatorscale))-PI/2;
  *longitude = x / mercatorscale;
}


void plane2sphere_lambert (double x, double y, double *latitude, double *longitude)
{
  *latitude = -asin((-4.0*pow(cos(v1),2.0)-4.0*pow(sin(v1),2.0)-4.0*sin(v1)*sin(v2)
  +y*y*pow(sin(v1),2.0)+pow(sin(v1),2.0)*x*x+2.0*y*y*sin(v1)*sin(v2)+2.0*sin(v1)
  *sin(v2)*x*x+y*y*pow(sin(v2),2.0)+pow(sin(v2),2.0)*x*x)/(sin(v1)+sin(v2))/4);

  *longitude = -2.0*atan(x/y)/(sin(v1)+sin(v2));
}



/* map-draw related funtions (callback funtions for parser) */

void map_setscale (double newscale)
{
  extern long scales[];   /* from map.c */
  extern int numscales;
  extern int scale;
  int i;

  map_scale = (long) newscale;

  /* try to find that scale in the scale-list */

  scale = numscales - 1;
  for (i=0; i<numscales; i++)
    if (map_scale >= scales[i])
      scale = i;
}

void map_setorigin (LOCATION origin)
{
  map_origin = origin;
}

void map_setparallels (long par1, long par2)
{
  v1 = internal2rad (par1);
  v2 = internal2rad (par2);
}



void map_display()
{
  char *progname = "ICAO";

  /* sort objects in memory to guarantee 'niceness' */

  sort_objects();

  map (1, &progname);
}


/* convert position x,y (in cm) to human readable string */

char *cm2string (double mapx, double mapy)
{
  double x, y;
  double latitude, longitude;
  LOCATION temp;

  x = mapx / scalefactor + originx;
  y = mapy / scalefactor + originy;

  plane2sphere (x, y, &latitude, &longitude);

  temp.latitude  = rad2internal(latitude);
  temp.longitude = rad2internal(longitude) + map_zeromeridian;
  return internal2string(temp);
}


/* convert position x,y (in cm) to internal format */

LOCATION cm2internal (double mapx, double mapy)
{
  double x, y;
  double latitude, longitude;
  LOCATION temp;

  x = mapx / scalefactor + originx;
  y = mapy / scalefactor + originy;

  plane2sphere (x, y, &latitude, &longitude);

  temp.latitude  = rad2internal(latitude);
  temp.longitude = rad2internal(longitude) + map_zeromeridian;
  return temp;
}


/* convert internal format to map position in cm */

void internal2cm (LOCATION position, double *mapx, double *mapy)
{
  double x, y;


  x = proj_x (internal2rad(position.latitude), 
	      internal2rad(position.longitude - map_zeromeridian));
  y = proj_y (internal2rad(position.latitude), 
	      internal2rad(position.longitude - map_zeromeridian));

  *mapx = (x-originx)*scalefactor;
  *mapy = (y-originy)*scalefactor;
}



/* set zeromeridian */

void map_setzero (long zero)
{
  map_zeromeridian = zero;
}

/* initialize some static vars for later projection */

void map_initcoord()
{
  double latitude, longitude;

  /* for mercator projection, adjust scale to gegion being displayed */

  mercatorscale = cos(internal2rad(map_origin.latitude));

  /* find origin on map */

  originx = proj_x (internal2rad(map_origin.latitude),
		    internal2rad(map_origin.longitude - map_zeromeridian));

  originy = proj_y (internal2rad(map_origin.latitude),
		    internal2rad(map_origin.longitude - map_zeromeridian));

  /* get window extents in cm from map.c */

  map_width = mapWidthcm; map_height = mapHeightcm;

  /* scalefactor between map (cm) and doubles from proj_x/proj_y*/

  scalefactor = 100000*EARTH_RADIUS / map_scale;

 

  /* obere linke Ecke */

  plane2sphere (originx-(map_width/scalefactor/2), originy+(map_height/scalefactor/2),
		&latitude, &longitude);
  
  top = bottom = rad2internal (latitude);
  right = left = rad2internal (longitude);

  topleft.latitude = rad2internal (latitude);
  topleft.longitude = rad2internal (longitude) + map_zeromeridian;
 

  /* obere rechte Ecke */

  plane2sphere (originx+(map_width/scalefactor/2), originy+(map_height/scalefactor/2),
		&latitude, &longitude);

  if (top < rad2internal(latitude)) top = rad2internal(latitude);
  if (bottom > rad2internal(latitude)) bottom = rad2internal(latitude);
  if (left > rad2internal(longitude)) left = rad2internal(longitude);
  if (right < rad2internal(longitude)) right = rad2internal(longitude);

  topright.latitude = rad2internal (latitude);
  topright.longitude = rad2internal (longitude) + map_zeromeridian;
 
  /* untere linke Ecke */

  plane2sphere (originx-(map_width/scalefactor/2), originy-(map_height/scalefactor/2),
		&latitude, &longitude);

  if (top < rad2internal(latitude)) top = rad2internal(latitude);
  if (bottom > rad2internal(latitude)) bottom = rad2internal(latitude);
  if (left > rad2internal(longitude)) left = rad2internal(longitude);
  if (right < rad2internal(longitude)) right = rad2internal(longitude);

  bottomleft.latitude = rad2internal (latitude);
  bottomleft.longitude = rad2internal (longitude) + map_zeromeridian;
 
  /* untere rechte Ecke */

  plane2sphere (originx+(map_width/scalefactor/2), originy-(map_height/scalefactor/2),
		&latitude, &longitude);

  if (top < rad2internal(latitude)) top = rad2internal(latitude);
  if (bottom > rad2internal(latitude)) bottom = rad2internal(latitude);
  if (left > rad2internal(longitude)) left = rad2internal(longitude);
  if (right < rad2internal(longitude)) right = rad2internal(longitude);

  bottomright.latitude = rad2internal (latitude);
  bottomright.longitude = rad2internal (longitude) + map_zeromeridian;

  /* Mitte oben */

  plane2sphere (originx, originy+(map_height/scalefactor/2),
		&latitude, &longitude);

  if (top < rad2internal(latitude)) top = rad2internal(latitude);
  if (bottom > rad2internal(latitude)) bottom = rad2internal(latitude);

  /* Mitte unten */

  plane2sphere (originx, originy-(map_height/scalefactor/2),
		&latitude, &longitude);

  if (top < rad2internal(latitude)) top = rad2internal(latitude);
  if (bottom > rad2internal(latitude)) bottom = rad2internal(latitude);

  left += map_zeromeridian;
  right += map_zeromeridian;

  /* check what objects are visible in the current map selection */

  findvisibleobjects();     /* in mapdisplay.c */
}




/* draw coordinate grid */

void map_drawline (double xcm1, double ycm1, double xcm2, double ycm2);
void grid_printunder (double xcm, char *string);
void grid_printleft (double ycm, char *string);

void coord_drawgrid (int printlabels)
{
  long latitude, longitude;
  int i, j, numi, numj;
  double gridx[100][100];
  double gridy[100][100];
  LOCATION temp;
  char str[30];

#define i60000 (map_scale < 1000000 ? 60000 : 180000)

  /* distance between lines: 10' = 60000 */

  for (i=0, latitude = (bottom-(i60000-1))/i60000*i60000; latitude<top+i60000; latitude += i60000, i++)
    for (j=0, longitude = (left-(i60000-1))/i60000*i60000; longitude<right+i60000; longitude += i60000, j++)
      {
	temp.latitude = latitude;
	temp.longitude = longitude;
	internal2cm (temp, &(gridx[i][j]), &(gridy[i][j]));
      }

  /* save number of calculated points */

  numi = i; numj=j;

  for (i=0; i<numi; i++)
    for (j=0; j<numj; j++)
      {
        if (i<numi-1) 
	  map_drawline (gridx[i][j], gridy[i][j], gridx[i+1][j], gridy[i+1][j]);
        if ((j<numj-1) && (i>0)) 
	  map_drawline (gridx[i][j], gridy[i][j], gridx[i][j+1], gridy[i][j+1]);
      }

  /* labels for the coordinate grid */

  for (i=0; i<numi; i++)
    for (j=0; j<numj; j++)
      {
	temp.latitude = (bottom-(i60000-1))/i60000*i60000 + i*i60000; 
	temp.longitude = (left-(i60000-1))/i60000*i60000 + j * i60000;
	strcpy (str, internal2string (temp));
	str[6] = str[9]; str[7] = 0;
	str[17] = str[20]; str[18] = 0;

	if ((i==0) && (j>0) && (temp.longitude % (3*i60000) == 0) && printlabels)
	  grid_printunder (gridx[i][j], str+11);   
	if ((j==1) && printlabels)
	  grid_printleft (gridy[i][j], str);    
      }
}



/*
  drawing to the map, it may be necessary to find out what offset has to added
  to a true track at a given position to properly draw that angle on the map
  (shifts by zeromeridian etc.):
*/

int truenorthoffset (LOCATION pos)
{
  if (map_projection == PROJECT_LAMBERT)
    return (int) ((map_zeromeridian - pos.longitude) / 360000);
  else
    return 0;     /* naturally, true north is always up in Mercator style maps */
}



