/* "nav.c" global navigation program August 10, 1991 */

// This program is copyright (c) 2016, P. Lutus and is released
// under the GPL (http://www.gnu.org/licenses/gpl-3.0.en.html).

#include <stdio.h>
#include <math.h>

#define PI2 6.283185307179586
#define PD2 1.570796326794897
#define DTOR 1.74532925199433e-2
#define RTOD 5.729577951308232e1
#define isalpha(c) (((c >= 'A') && (c <= 'Z')) || ((c >= 'a') && (c <= 'z')))
#define isnum(c) (((c>='0') && (c<='9')) || (c=='-') || (c=='+') || (c=='.'))
#define toupper(c) (((c >= 'a') && (c <= 'z'))?c-32:c)

struct COMPLX {
	double rad;
	double lat;
	double lng;
	double x;
	double y;
	double z;
};

int matherr(struct exception *p) /* dummy matherr routine to suppress printed message */
//struct exception *p;
{
	return(1);
}

double myatan2(double a, double b)
// double a,b;
{
	if((a != 0.0) || (b != 0.0))
		a = atan2(a,b);
	return(a);
}

struct COMPLX rect_to_pol(struct COMPLX p)
//struct COMPLX p;
{
	p.lng = myatan2(p.x,p.z);
	p.rad = hypot(p.x,p.z);
	p.lat = myatan2(p.y,p.rad);
	return(p);
}

struct COMPLX comp_pr(struct COMPLX b, struct COMPLX a)
//struct COMPLX b,a;
{
	double dlo,cdl,sbl,cbl;
	dlo = a.lng - b.lng;
	cdl = cos(dlo);
	sbl = sin(a.lat);
	cbl = cos(a.lat);
	a.rad = acos(sbl * sin(b.lat) + cbl * cos(b.lat) * cdl);
	a.rad *= RTOD * 60;
	a.lat = myatan2(sin(dlo),(cbl * tan(b.lat) - sbl * cdl));
	if (a.lat < 0) a.lat = PI2 + a.lat;
	return(a);
}

struct COMPLX comp_rp(struct COMPLX b,struct COMPLX a)
//struct COMPLX b,a;
{
	struct COMPLX c;
	double r,sr,ang;
	r = b.rad / (RTOD * 60.0);
	sr = sin(r);
	c.x = -sin(b.lat) * sr;
	c.y = cos(b.lat) * sr;
	c.z = cos(r);
	r = hypot(c.y,c.z);
	ang = myatan2(c.y,c.z) + a.lat;
	c.z = r * cos(ang);
	c.y = r * sin(ang);
	c = rect_to_pol(c);
	c.lng += a.lng;
	return(c);
}

void disp_lat_lng2(struct COMPLX q)
//struct COMPLX q;
{
	int lat_s,lng_s;
	int lat_d,lng_d;
	int lat_m,lng_m;
	double lat_m2,lng_m2;
	q.lat *= RTOD;
	q.lng *= RTOD;
	if(q.lng > 180.0)
		q.lng -= 360.0;
	lat_s = (q.lat >= 0);
	lng_s = (q.lng >= 0);
	q.lat = fabs(q.lat) + .0000083333;
	q.lng = fabs(q.lng) + .0000083333;
	lat_d = (int) q.lat;
	lat_m = (int) ((q.lat - lat_d) * 6000.0);
	lat_m2 = (int) (lat_m * 1e-2);
	lng_d = (int) q.lng;
	lng_m = (int) ((q.lng - lng_d) * 6000.0);
	lng_m2 = (int) (lng_m * 1e-2);
	printf("lat %3d deg %5.02lf min %c lng %3d deg %5.02lf min %c"
	,lat_d,lat_m2,(lat_s)?'N':'S',lng_d,lng_m2,(lng_s)?'W':'E');
}

void disp_lat_lng(struct COMPLX q)
//struct COMPLX q;
{
	disp_lat_lng2(q);
	printf("\n");
}

void disp_dist_brg(struct COMPLX q)
//struct COMPLX q;
{
	q.rad += .0005;
	q.lat *= RTOD;
	q.lat += .0005;
	printf("%8.02lf nm, %6.02lf deg true\n",q.rad,q.lat);
}

int llscan(char *argv[],
double *q,
int i, int argc)
//char *argv[];
//double *q;
//int i,argc;
{
	double x;
	int c;
	if(i <= argc)
	{
		sscanf(*(argv+i),"%lf",q);
		i++;
	}
	if(i <= argc)
	{
		sscanf(*(argv+i),"%lf",&x);
		*q += (x/60.0);
		i++;
	}
	if(i <= argc)
	{
		c = toupper(*(argv+i)[0]);
		if(isalpha(c))
		{
			if ((c == 'S') || (c == 'E'))
				*q = -*q;
			i++;
		}
	}
	*q *= DTOR;
	return(i);
}

int dbscan(char *argv[],
double *q,
int i,int argc)
//char *argv[];
//double *q;
//int i,argc;
{
	if(i <= argc)
	{
		sscanf(*(argv+i),"%lf",q);
		i++;
	}
	return(i);
}

int timescan(char *argv[],
double *q,
int i,int argc)
//char *argv[];
//double *q;
//int i,argc;
{
	double h,m;
	if(i <= argc)
	{
		sscanf(*(argv+i),"%lf:%lf",&h,&m);
		i++;
		*q = h + (m / 60);
	}
	return(i);
}

int numflds(int argc,char *argv[])
//int argc;
//char *argv[];
{
	int count = 0,i;
	for(i = 1;i < argc;i++)
	{
		if(isnum(*(argv+i)[0]))
			count++;
	}
	return(count);
}

int istime(char *string)
//char *string;
{
	int i = 0;
	while((*string+i) && ((*string+i) != ':'))
		i++;
	return((*string+i) == ':');
}

char llstring[] = "(lat) ddd mm.mm [n/s] (lng) ddd mm.mm [w/e]";

void help()
{
	printf(
	"usage: [-f(ull)] (1) result distance & bearing for 2 positions:\n");
	printf(
	"(from) %s\n",llstring);
	printf(
	"(to)   %s\n\n",llstring);
	printf(
	"       (2) result speed, distance & bearing for 2 positions & times:\n");
	printf(
	"(from) (time) hh:mm %s\n",llstring);
	printf(
	"(to)   (time) hh:mm %s\n\n",llstring);
	printf(
	"       (3) result position for position, distance & bearing\n");
	printf(
	"(from) %s\n",llstring);
	printf(
	"(to)   (dist) nm (bearing true) ddd.dd\n\n");
	printf(
	"       (4) track intercept distance & bearing for 3 positions\n");
	printf(
	"(track begin position) %s\n",llstring);
	printf(
	"(track end position)   %s\n",llstring);
	printf(
	"(vessel position)      %s\n",llstring);
}

main(int argc,char *argv[])
//int argc;
//char *argv[];
{
	int i = 1,count,listmode,full = 0;
	struct COMPLX p1,p2,p3,p4,p5;
	double intpos,steps,atime,btime;
	if((argc > 1) && (argv[1][0] == '-') && (toupper(argv[1][1]) == 'F'))
	{
		argc--;
		argv++;
		full++;
	}
	count = numflds(argc,argv);
	if(count == 8)
	{
		i = llscan(argv,&p1.lat,i,argc);
		i = llscan(argv,&p1.lng,i,argc);
		i = llscan(argv,&p2.lat,i,argc);
		i = llscan(argv,&p2.lng,i,argc);
		p3 = comp_pr(p2,p1);
		if(full)
		{
			printf("from ");
			disp_lat_lng(p1);
			printf("to   ");
			disp_lat_lng(p2);
			printf("is   ");
		}
		disp_dist_brg(p3);
	}
	else if(count == 6)
	{
		i = llscan(argv,&p1.lat,i,argc);
		i = llscan(argv,&p1.lng,i,argc);
		i = dbscan(argv,&p2.rad,i,argc);
		i = dbscan(argv,&p2.lat,i,argc);
		p2.lat *= DTOR;
		p3 = comp_rp(p2,p1);
		if(full)
		{
			printf("from ");
			disp_lat_lng(p1);
			printf("to   ");
			disp_dist_brg(p2);
			printf("is   ");
		}
		disp_lat_lng(p3);
	}
	else if(count == 10)
	{
		i = timescan(argv,&atime,i,argc);
		i = llscan(argv,&p1.lat,i,argc);
		i = llscan(argv,&p1.lng,i,argc);
		i = timescan(argv,&btime,i,argc);
		i = llscan(argv,&p2.lat,i,argc);
		i = llscan(argv,&p2.lng,i,argc);
		p3 = comp_pr(p2,p1);
		if(full)
		{
			printf("from ");
			disp_lat_lng(p1);
			printf("to   ");
			disp_lat_lng(p2);
			printf("is   ");
		}
		disp_dist_brg(p3);
		btime -= atime;
		if(btime <= 0.0)
			btime += 24.0;
		atime = p3.rad / btime;
		if(atime > 40.0)
			btime += 24.0;
		atime = p3.rad / btime;
		printf("time %.2lf hours -- %.2lf knots\n",btime,atime);
	}
	else if(count == 12)
	{
		i = llscan(argv,&p1.lat,i,argc);
		i = llscan(argv,&p1.lng,i,argc);
		i = llscan(argv,&p2.lat,i,argc);
		i = llscan(argv,&p2.lng,i,argc);
		i = llscan(argv,&p3.lat,i,argc);
		i = llscan(argv,&p3.lng,i,argc);
		p4 = comp_pr(p2,p1); /* bearing to track end */
		p5 = comp_pr(p3,p1); /* distance to vessel from track start */
		p5.lat = p4.lat; /* now p5 points to track intercept */
		p4 = comp_rp(p5,p1); /* track intercept pos */
		p5 = comp_pr(p4,p3); /* dist, brg to intercept */
		if(full)
		{
			printf("track begin         ");
			disp_lat_lng(p1);
			printf("track end           ");
			disp_lat_lng(p2);
			printf("vessel position     ");
			disp_lat_lng(p3);
			printf("track intercept     ");
			disp_lat_lng(p4);
		}
		printf("vessel to intercept ");
		disp_dist_brg(p5);
	}
	else help();
}