/*
	Command generator for J2000.0 positions

	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2011, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2011, 2013

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h>
#include <signal.h>
#include <ctype.h>
#include <inttypes.h>
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <stdbool.h>

#include <libnova/transform.h>
#include <libnova/julian_day.h>
#include <libnova/utility.h>
#include <libnova/aberration.h>
#include <libnova/precession.h>
#include <libnova/refraction.h>

#include "setpoint.h"
#include "tcp_connect.h"
#include "tcp_listen.h"
#include "weather.h"
#include "dt_model.h"
#include "trace.h"

#include "dt_port_numbers.h"

#define TIME_OFFSET 2

char *command_host = "localhost";
int command_port = CONSOLE_COMMAND_PORT;
char *trace_host = "localhost";
int trace_port = CONSOLE_TRACE_PORT;
int stat_port = CONSOLE_J2000_POS_PORT;
int cmd_port = CONSOLE_J2000_CMD_PORT;
char *az_command_spg = "Azimuth_Setpoint";
char *el_command_spg = "Elevation_Setpoint";
char *az_trace_name = "Azimuth_Position";
char *el_trace_name = "Elevation_Position";

static bool tracking_command_send_p180 = false;
static bool tracking_command_last_az_valid = false;
static float tracking_command_last_az = 0;


bool tracking_enabled = false;
bool refraction_enable = true;
bool dt_model_enable = false;

struct compensation_switch {
	char *name;
	bool *value;
};

struct compensation_switch compensation_switches[] = {
	{ "enabled", &tracking_enabled },
	{ "refraction", &refraction_enable },
	{ "dt_model", &dt_model_enable },
	{ NULL, NULL }
};

struct weather *weather;

struct traceval *traceval_az;
struct traceval *traceval_el;
float traceval_az_deg;

typedef union {
	uint32_t u;
	float f;
} float32_t;

struct lnh_lnlat_posn dwingeloo;
struct ln_lnlat_posn dwingeloo_pos;
struct ln_equ_posn object;
struct ln_equ_posn object_prec;
struct ln_equ_posn object_aber;
struct ln_hms track_hms;
struct ln_dms track_dms;

struct stat_client {
	struct stat_client *next;
	int fd;
};

struct stat_client *stat_clients = NULL;

struct cmd_client {
	struct cmd_client *next;
	int fd;
	char buffer[100];
	int buflen;
};

struct cmd_client *cmd_clients = NULL;



void a2coord(char *ara, char *adec, struct ln_hms *ra, struct ln_dms *dec)
{
	int i, neg;

	if (strlen(adec) && adec[0] == '-') {
		dec->neg = 1;
		i = 1;
	} else {
		dec->neg = 0;
		i = 0;
	}
	dec->degrees = 0;
	dec->minutes = 0;
	dec->seconds = 0.0;
	
	for (; isdigit(adec[i]) && i < strlen(adec); i++) {
		dec->degrees *= 10;
		dec->degrees += adec[i] - '0';
	}
	for (i++; isdigit(adec[i]) && i < strlen(adec); i++) {
		dec->minutes *= 10;
		dec->minutes += adec[i] - '0';
	}
	i++;
	if (i < strlen(adec)) {
		dec->seconds = atof(adec + i);
	}
	
	ra->hours = 0;
	ra->minutes = 0;
	ra->seconds = 0.0;
	
	if (strlen(ara) && ara[0] == '-') {
		neg = -1;
		i = 1;
	} else {
		neg = 1;
		i = 0;
	}
	for (; isdigit(ara[i]) && i < strlen(ara); i++) {
		ra->hours *= 10;
		ra->hours += (ara[i] - '0') * neg;
	}
	for (i++; isdigit(ara[i]) && i < strlen(ara); i++) {
		ra->minutes *= 10;
		ra->minutes += (ara[i] - '0') * neg;
	}
	i++;
	if (i < strlen(ara)) {
		ra->seconds = atof(ara + i) * neg;
	}
	if (neg < 0) {
		ra->hours += 24;
		ra->minutes += 60;
		ra->seconds += 60.0;
	}
}

void new_stat_client(int fd_stat)
{
	struct stat_client *new_stat;
	int fd;
	
	fd = tcp_accept(fd_stat);
	if (fd < 0)
		return;
	ioctl(fd, FIONBIO, &(int){ 1 });
	
	new_stat = malloc(sizeof(struct stat_client));
	if (!new_stat) {
		close(fd);
		return;
	}
	
	new_stat->fd = fd;
	new_stat->next = stat_clients;
	stat_clients = new_stat;
}

void new_cmd_client(int fd_cmd)
{
	struct cmd_client *new_cmd;
	int fd;
	
	fd = tcp_accept(fd_cmd);
	if (fd < 0)
		return;
	ioctl(fd, FIONBIO, &(int){ 1 });
	
	new_cmd = malloc(sizeof(struct cmd_client));
	if (!new_cmd) {
		close(fd);
		return;
	}
	
	new_cmd->fd = fd;
	new_cmd->buflen = 0;
	new_cmd->next = cmd_clients;
	cmd_clients = new_cmd;
}

void fdset_cmd_clients(fd_set *fdset_rx, int *high)
{
	struct cmd_client *cmd;
	
	for (cmd = cmd_clients; cmd; cmd = cmd->next) {
		FD_SET(cmd->fd, fdset_rx);
		if (cmd->fd >= *high)
			*high = cmd->fd + 1;
	}
}

void handle_cmd(struct cmd_client *cmd)
{
	char *ara, *adec, *switches, *ptr;
	double oldra, olddec;
	
	ara = strtok_r(cmd->buffer, " \t", &ptr);
	adec = strtok_r(NULL, " \t", &ptr);
	switches = strtok_r(NULL, " \t", &ptr);
	
	if (!ara || !adec)
		return;
	
	while (adec[strlen(adec)-1] == '\n' ||
	    adec[strlen(adec)-1] == '\r')
		adec[strlen(adec)-1] = 0;
	
	a2coord(ara, adec, &track_hms, &track_dms);
	oldra = object.ra;
	olddec = object.dec;
	object.ra = ln_hms_to_deg(&track_hms);
	object.dec = ln_dms_to_deg(&track_dms);
	
	if (oldra != object.ra || olddec != object.dec)
		tracking_command_last_az_valid = false;
	
	if (switches) {
		char *name, *val;
		int i;
		name = strtok_r(switches, ",=", &ptr);
		val = strtok_r(NULL, ",=", &ptr);
		for (i = 0; compensation_switches[i].name; i++) {
			if (!strcmp(name, compensation_switches[i].name)) {
				if (val[0] == '1') {
					*compensation_switches[i].value = true;
				} else {
					*compensation_switches[i].value = false;
				}
			}
		}
	}
}

void check_cmd_clients(fd_set *fdset_rx)
{
	struct cmd_client **cmdp, **nextp = NULL;
	
	for (cmdp = &cmd_clients; *cmdp; cmdp = nextp) {
		int ret;
		
		nextp = &(*cmdp)->next;
		if (!FD_ISSET((*cmdp)->fd, fdset_rx))
			continue;
		
		do {
			ret = read((*cmdp)->fd, (*cmdp)->buffer+(*cmdp)->buflen, 1);
			if ((ret < 0 && errno != EAGAIN) || ret == 0) {
				struct cmd_client *tmp;
				
				tmp = *cmdp;
				close((*cmdp)->fd);
				*cmdp = (*cmdp)->next;
				free(tmp);
				nextp = cmdp;
			} else if (ret == 1) {
				(*cmdp)->buflen += 1;
				if ((*cmdp)->buffer[(*cmdp)->buflen-1] == '\n' ||
				    (*cmdp)->buffer[(*cmdp)->buflen-1] == '\r') {
					if ((*cmdp)->buflen > 1) {
						(*cmdp)->buffer[(*cmdp)->buflen] = 0;
						handle_cmd(*cmdp);
						(*cmdp)->buffer[0] = '9';
					}
					(*cmdp)->buflen = 0;
				}
				if ((*cmdp)->buflen >= 100)
					(*cmdp)->buflen = 0;
			}
		} while (ret > 0);
	}
}

void output(void)
{
	static time_t last = 0;
	time_t now;
	struct ln_hrz_posn hrz;
	struct ln_date date;
	double JD;
	struct ln_equ_posn equ_pos, equ_pos_prec, equ_pos_aber;
	struct ln_hms hms;
	struct ln_dms dms;
	struct stat_client **stat_clientp, **nextp;
	char statline[200];
	int ret, i;
	char *statlinepos;
	int do_output = 1;
	double pressure, temperature, alt_adj;
	
	if (traceval_az->new == 0 || traceval_el->new == 0)
		do_output = 0;
	
	if (traceval_az->seconds < traceval_el->seconds)
		now = traceval_az->seconds;
	else
		now = traceval_el->seconds;	
	
	if (now <= last)
		do_output = 0;
	
	if (do_output == 0) {
		time_t localtime;
		
		localtime = time(NULL);
		if (trace_fd(traceval_az) >= 0 && localtime > (traceval_az->seconds + 5)) {
			trace_close(traceval_az);
		}
		if (trace_fd(traceval_el) >= 0 && localtime > (traceval_el->seconds + 5)) {
			trace_close(traceval_el);
		}
		
		if (localtime > now + 10 &&
		    localtime > last) {
			last = localtime;
		} else {
			return;
		}
	} else {
		last = now;
	}
	
	hrz.az = traceval_az->value * 360.0 / (2*M_PI);
	traceval_az_deg = hrz.az;
	hrz.alt = traceval_el->value * 360.0 / (2*M_PI);
	
	ln_get_date_from_timet(&last, &date);
	JD = ln_get_julian_day(&date);

	if (dt_model_enable) {
		double daz, del;
				
		daz = dt_model_azimuth_delta_rev(hrz.az, hrz.alt);
		del = dt_model_elevation_delta_rev(hrz.az, hrz.alt);
		printf("rev(%f, %f): %g %g\n", hrz.az, hrz.alt, daz, del);
		hrz.az += daz;
		hrz.alt += del;
	}

	if (refraction_enable) {
		/* Get current weather */
		temperature = weather_get_temperature(weather);
		pressure = weather_get_pressure(weather);
		/* Refraction adjustment */
		alt_adj = ln_get_refraction_adj(hrz.alt, pressure, temperature);
		printf("Refraction: %f @ %fC,%fmbar\n", -alt_adj, temperature, pressure);
		hrz.alt -= alt_adj;
	}
	
	/* libnova can't handle anything above 90.0 */
	if (hrz.alt > 90.0)
		hrz.alt = 90.0;

	ln_get_equ_from_hrz (&hrz, &dwingeloo_pos, JD, &equ_pos_prec);
	/* Calculate inverse precession */
	ln_get_equ_prec2(&equ_pos_prec, JD, JD2000, &equ_pos_aber);
	ln_get_equ_mean_from_aber(&equ_pos_aber, JD, &equ_pos);

	ln_deg_to_hms(equ_pos.ra, &hms);
	ln_deg_to_dms(equ_pos.dec, &dms);

	printf("Trace  %f %f : %02dh%02dm%04.1f %s%03dd%02dm%04.1f\n",
	    hrz.az, hrz.alt,
	    hms.hours, hms.minutes, hms.seconds,
	    dms.neg ? "-" : " ", dms.degrees, dms.minutes, dms.seconds);

	ret = sprintf(statline, 
	    "%02dh%02dm%04.1f %s%03dd%02dm%04.1f %02dh%02dm%04.1f %s%03dd%02dm%04.1f %ld ",
	    track_hms.hours, track_hms.minutes, track_hms.seconds,
	    track_dms.neg ? "-" : "", 
	    track_dms.degrees, track_dms.minutes, track_dms.seconds,
	    hms.hours, hms.minutes, hms.seconds,
	    dms.neg ? "-" : "",
	    dms.degrees, dms.minutes, dms.seconds,
	    (unsigned long)last);
	statlinepos = statline + ret;
	for (i = 0; compensation_switches[i].name; i++) {
		if (i) {
			ret = sprintf(statlinepos, ",");
			statlinepos += ret;
		}
		ret = sprintf(statlinepos,
		    "%s=%d",
		    compensation_switches[i].name,
		    *compensation_switches[i].value);
		statlinepos += ret;
	}
	sprintf(statlinepos, "\n");

	for (stat_clientp = &stat_clients; *stat_clientp; stat_clientp = nextp){
		int ret;
		
		ret = write((*stat_clientp)->fd, statline, strlen(statline));
		if (ret <= 0) {
			struct stat_client *tmp;
			
			tmp = *stat_clientp;
			close((*stat_clientp)->fd);
			*stat_clientp = (*stat_clientp)->next;
			free(tmp);
			nextp = stat_clientp;
		} else {
			nextp = &(*stat_clientp)->next;
		}
	}
}

int main(int argc, char **argv)
{
	struct setpoint_command *sp_command_az = NULL;
	struct setpoint_command *sp_command_el = NULL;

	time_t lastt = 0;
	int fd_stat, fd_cmd;

	printf("libnova version: %s\n", ln_get_version());

	dwingeloo.lng.neg = 0;
	dwingeloo.lng.degrees = 6;
	dwingeloo.lng.minutes = 23;
	dwingeloo.lng.seconds = 46.21;
	dwingeloo.lat.neg = 0;
	dwingeloo.lat.degrees = 52;
	dwingeloo.lat.minutes = 48;
	dwingeloo.lat.seconds = 43.27;
	ln_hlnlat_to_lnlat(&dwingeloo, &dwingeloo_pos);

	track_hms.hours = 23;
	track_hms.minutes = 23;
	track_hms.seconds = 26;
	track_dms.neg = 0;
	track_dms.degrees = 58;
	track_dms.minutes = 48;
	track_dms.seconds = 0.0;

	signal(SIGPIPE, SIG_IGN);

	if (argc == 2 || argc == 4) {
		command_host = argv[argc - 1];
		trace_host = argv[argc - 1];
	}
	if (argc == 3 || argc == 4) {
		printf("Coords from command line\n");
		a2coord(argv[1], argv[2], &track_hms, &track_dms);
	}
	object.ra = ln_hms_to_deg(&track_hms);
	object.dec = ln_dms_to_deg(&track_dms);

	ln_deg_to_hms(object.ra, &track_hms);
	ln_deg_to_dms(object.dec, &track_dms);

	do {
		sp_command_az = setpoint_command_init(command_host, command_port,
		    az_command_spg, "console/j2000tracker");
		if (!sp_command_az) {
			printf("Could not open connection for az commands\n");
			sleep(1);
		}
	} while (!sp_command_az);
	do {
		sp_command_el = setpoint_command_init(command_host, command_port,
		    el_command_spg, "console/j2000tracker");
		if (!sp_command_el) {
			printf("Could not open connection for el commands\n");
			sleep(1);
		}
	} while (!sp_command_el);
	
	traceval_az = trace_create();
	traceval_el = trace_create();
	trace_init(trace_host, trace_port, az_trace_name, traceval_az);
	trace_init(trace_host, trace_port, el_trace_name, traceval_el);

	fd_cmd = tcp_listen(cmd_port, 0, 100);
	if (fd_cmd < 0) {
		printf("Could not open listen port for commands\n");
	} else {
		ioctl(fd_cmd, FIONBIO, &(int){ 1 });
	}
	fd_stat = tcp_listen(stat_port, 0, 100);
	if (fd_stat < 0) {
		printf("Could not open listen port for status\n");
	} else {
		ioctl(fd_stat, FIONBIO, &(int){ 1 });
	}

	weather = weather_create("Dwingeloo");
	if (!weather) {
		fprintf(stderr, "Could not create weather handle\n");
		exit(-1);
	}

	while (1)
	{
		time_t t;
		struct ln_date date;
		double JD;
		struct ln_hrz_posn hrz;
		int high = 0;
		fd_set fdset_rx;
		struct timeval tv;
		double pressure, temperature;
		double alt_adj;
		
		t = time(NULL);
		if (t != lastt) {
			lastt = t;
			t += TIME_OFFSET;
		
			ln_get_date_from_timet(&t, &date);
			JD = ln_get_julian_day(&date);

			/* Calculate aberration */
			ln_get_equ_aber(&object, JD, &object_aber);
			/* Calculate precession */
			ln_get_equ_prec(&object_aber, JD, &object_prec);
			/* Convert to Alt/Az */
			ln_get_hrz_from_equ (&object_prec, &dwingeloo_pos, JD, &hrz);

			if (refraction_enable) {
				/* Get current weather */
				temperature = weather_get_temperature(weather);
				pressure = weather_get_pressure(weather);
				/* Refraction adjustment */
				alt_adj = ln_get_refraction_adj(hrz.alt, pressure, temperature);
				printf("Refraction: %f @ %fC,%fmbar\n", alt_adj, temperature, pressure);
				hrz.alt += alt_adj;
			}
			
			/* libnova has an az range from 0 to 360 with 0 being South.
			   Dwingeloo has an az range from -270 to 270 with 0 being South
			   
			   Stars that stay north of dwingeloo (incl circumpolar)
			   can be tracked in either positive or negative north.
			   So we take the nearest.
			   Other stars pass through south.
			 */
			printf("dec: %f Dwingeloo lat: %f => ",
			    object.dec, dwingeloo_pos.lat);
			if (object.dec > dwingeloo_pos.lat) {
				printf("Always north of zenith\n");
				if (traceval_az_deg < 0.0)
					hrz.az -= 360.0;
			} else {
				printf("Passes south of zenith\n");
				if (hrz.az > 180.0) {
					if (traceval_az_deg > 150.0 &&
					    hrz.az < 270.0) {
						printf("Stay on positive side to prevent jump\n");
					} else {
						if (traceval_az_deg > 180.0 &&
						    hrz.az > 270.0 &&
						    tracking_command_send_p180 &&
						    tracking_enabled) {
							tracking_enabled = false;
							printf("Going over 270 degree border, disabling\n\n");
						}
						hrz.az -= 360.0;
					}
				} else {
					if (traceval_az_deg < -150.0 &&
					    hrz.az >= 90.0) {
						printf("Already on negative side, stay there\n");
						hrz.az -= 360.0;
					}
				}
			}
			
			if (dt_model_enable) {
				double daz, del;
				
				daz = dt_model_azimuth_delta(hrz.az, hrz.alt);
				del = dt_model_elevation_delta(hrz.az, hrz.alt);
				printf("model: %g %g\n", daz, del);
				hrz.az += daz;
				hrz.alt += del;
			}

			printf("Setpoint: %02dh%02dm%04.1f %s%03dd%02dm%04.1f %d %f %f\n",
			    track_hms.hours, track_hms.minutes, track_hms.seconds,
			    track_dms.neg ? "-" : " ",
			    track_dms.degrees, track_dms.minutes, track_dms.seconds,
			    (unsigned int)t, hrz.az, hrz.alt);

			if (tracking_enabled && tracking_command_last_az_valid) {
				if (fabs(hrz.az - tracking_command_last_az) > 1.0) {
					printf("Az going to fast to track (%e), disable tracking\n",
					    fabs(hrz.az-tracking_command_last_az));
					tracking_enabled = false;
					setpoint_command_speed(sp_command_az, 0.0);
					setpoint_command_speed(sp_command_el, 0.0);
				}
			}

			if (tracking_enabled) {
				if (hrz.az > 180.0)
					tracking_command_send_p180 = true;
				setpoint_command_setpoint_time(sp_command_az,
				    hrz.az * 2 * M_PI / 360.0, t);
				setpoint_command_setpoint_time(sp_command_el,
				    hrz.alt * 2 * M_PI / 360.0, t);

				tracking_command_last_az_valid = true;
				tracking_command_last_az = hrz.az;
			} else {
				tracking_command_send_p180 = false;
				tracking_command_last_az_valid = false;
				tracking_command_last_az = 0.0;
			}
		}

		FD_ZERO(&fdset_rx);
	
		if (fd_stat >= 0) {
			FD_SET(fd_stat, &fdset_rx);
			if (fd_stat >= high)
				high = fd_stat + 1;
		}
		if (fd_cmd >= 0) {
			FD_SET(fd_cmd, &fdset_rx);
			if (fd_cmd >= high)
				high = fd_cmd + 1;
		}
		if (trace_fd(traceval_el) < 0) {
			printf("Reconnect elevation trace\n");
			sleep(1);
			trace_init(trace_host, trace_port, el_trace_name,
			    traceval_el);
		}
		if (trace_fd(traceval_el) >= 0) {
			FD_SET(trace_fd(traceval_el), &fdset_rx);
			if (trace_fd(traceval_el) >= high)
				high = trace_fd(traceval_el) + 1;
		}
		if (trace_fd(traceval_az) < 0) {
			printf("Reconnect azimuth trace\n");
			sleep(1);
			trace_init(trace_host, trace_port, az_trace_name,
			    traceval_az);
		}
		if (trace_fd(traceval_az) >= 0) {
			FD_SET(trace_fd(traceval_az), &fdset_rx);
			if (trace_fd(traceval_az) >= high)
				high = trace_fd(traceval_az) + 1;
		}
		fdset_cmd_clients(&fdset_rx, &high);
		
		tv.tv_sec = 1;
		tv.tv_usec = 0;

		select(high, &fdset_rx, NULL, NULL, &tv);

		if (FD_ISSET(fd_stat, &fdset_rx)) {
			new_stat_client(fd_stat);
		}
		if (FD_ISSET(fd_cmd, &fdset_rx)) {
			new_cmd_client(fd_cmd);
		}
		if (trace_fd(traceval_az) > 0 &&
		    FD_ISSET(trace_fd(traceval_az), &fdset_rx)) {
			trace_handle(traceval_az);
		}
		if (trace_fd(traceval_el) > 0 &&
		    FD_ISSET(trace_fd(traceval_el), &fdset_rx)) {
			trace_handle(traceval_el);
		}
		check_cmd_clients(&fdset_rx);
		
		output();
	}

}