/*
	Command generator for J2000.0 positions

	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2011, 2013, 2014
	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 <command/command.h>

#include <utils/weather.h>
#include <utils/dt_model.h>
#include <trace/trace.h>
#include <dt_port_numbers.h>
#include <utils/command_server.h>
#include <utils/status_server.h>
#include <utils/dt_host.h>
#include <log/log.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 trace *traceval_az;
struct trace *traceval_el;
float traceval_az_deg;

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;


/* Reverse of ln_get_equ_aber() */
static void get_equ_mean_from_aber(struct ln_equ_posn * position, double JD, struct ln_equ_posn * mean_position)
{
	struct ln_equ_posn mean;
	
	ln_get_equ_aber(position, JD, &mean);
	
	mean_position->ra = position->ra - (mean.ra - position->ra);
	mean_position->dec = position->dec - (mean.dec - position->dec);
	
	if (mean_position->dec > 90.0) {
		mean_position->dec = 180.0 - mean_position->dec;
		mean_position->ra -= 180.0;
	}
	while (mean_position->ra < 0.0) {
		mean_position->ra += 360.0;
	}
	while (mean_position->ra >= 360.0) {
		mean_position->ra -= 360.0;
	}
}


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;
		if (strlen(adec) && adec[0] =='+') {
			i = 1;
		} else {
			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;
	}
}

int handle_cmd(char *command)
{
	char *ara, *adec, *switches, *ptr;
	double oldra, olddec;
	
	ara = strtok_r(command, " \t", &ptr);
	adec = strtok_r(NULL, " \t", &ptr);
	switches = strtok_r(NULL, " \t", &ptr);
	
	if (!ara || !adec)
		return -1;
	
	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) {
		log_send(LOG_T_INFO, "New J2000 coordinates: ra=%f, dec=%f",
		    object.ra, 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;
				}
			}
		}
	}
	return 0;
}

void output(struct status_server *stat_srv)
{
	static time_t last = 0;
	time_t now;
	time_t now_t = time(NULL);
	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;
	char statline[200];
	int ret, i;
	char *statlinepos;
	int do_output = 1;
	double pressure, temperature, alt_adj;
	
	if (traceval_az->value.t.tv_sec != traceval_el->value.t.tv_sec) {
		do_output = 0;
	}
	now = traceval_az->value.t.tv_sec;
	if (now_t - now > 2) {
		printf("No trace for more than 2 seconds, using last known position\n");
		now = now_t;
	}
	
	if (now <= last) {
		do_output = 0;
	}
	
	if (do_output == 0) {
		return;
	} else {
		last = now;
	}
	
	hrz.az = traceval_az->value.value.f * 360.0 / (2*M_PI);
	traceval_az_deg = hrz.az;
	hrz.alt = traceval_el->value.value.f * 360.0 / (2*M_PI);
	
	ln_get_date_from_timet(&now, &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);
	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)now);
	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");

	status_server_send(stat_srv, statline);
}

static void handler_interval(struct trace *trace, 
    struct timespec *interval, enum trace_interval_type type)
{
	printf("interval:  %ld.%09ld\n", interval->tv_sec, interval->tv_nsec);
}


int main(int argc, char **argv)
{
	struct command *sp_command_az = NULL;
	struct command *sp_command_el = NULL;
	struct command_server *cmd_srv;
	struct status_server *stat_srv;
	time_t lastt = 0;
	struct timespec t_int;

	dt_model_init();

	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);

	log_client_start(dt_host_console(), CONSOLE_LOG_PORT_IN, 
	    LOG_T_DEBUG, LOG_T_INFO, "console/j2000tracker");

	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 = command_open_simple(command_host, command_port,
		    az_command_spg, "console/j2000tracker",
		    COMMAND_VALUE_TYPE_FLOAT);
		if (!sp_command_az) {
			printf("Could not open connection for az commands\n");
			sleep(1);
		}
	} while (!sp_command_az);
	do {
		sp_command_el = command_open_simple(command_host, command_port,
		    el_command_spg, "console/j2000tracker",
		    COMMAND_VALUE_TYPE_FLOAT);
		if (!sp_command_el) {
			printf("Could not open connection for el commands\n");
			sleep(1);
		}
	} while (!sp_command_el);
	
	traceval_az = trace_open(trace_host, trace_port);
	traceval_el = trace_open(trace_host, trace_port);
	trace_name_set(traceval_az, 0, az_trace_name);
	trace_name_set(traceval_el, 0, el_trace_name);
	trace_autorecover(traceval_az, true);
	trace_autorecover(traceval_el, true);
	traceval_az->handler_interval = handler_interval;
	traceval_el->handler_interval = handler_interval;

	t_int.tv_sec = 1;
	t_int.tv_nsec = 0;
	trace_interval_set(traceval_az, &t_int, TRACE_INTERVAL_TYPE_INTERVAL);
	trace_interval_set(traceval_el, &t_int, TRACE_INTERVAL_TYPE_INTERVAL);


	cmd_srv = command_server_create(cmd_port, 0, 100);
	if (!cmd_srv) {
		printf("Could not open listen port for commands\n");
	} else {
		command_server_handler_raw_set(cmd_srv, handle_cmd);
	}
	stat_srv = status_server_create(stat_port, 0, 100);
	if (!cmd_srv) {
		printf("Could not open listen port for status\n");
	}

	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;
							log_send(LOG_T_WARNING,
							    "Going over 270 degree border, disabling");
						}
						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) {
					struct command_entry entry;
					
					log_send(LOG_T_WARNING,
					    "Az going to fast to track (%e), disable tracking",
					    fabs(hrz.az-tracking_command_last_az));
					tracking_enabled = false;
					
					entry.type = COMMAND_PTYPE_SPEED;
					entry.value.f = 0.0;
					command_send(sp_command_az, &entry);
					command_send(sp_command_el, &entry);
				}
			}

			if (tracking_enabled) {
				struct command_entry entry;
				
				if (hrz.az > 180.0)
					tracking_command_send_p180 = true;
				
				entry.type = COMMAND_PTYPE_SETPOINT_TIME;
				entry.t.tv_sec = t;
				entry.t.tv_nsec = 0;
				entry.value.f = hrz.az * 2 * M_PI / 360.0;
				command_send(sp_command_az, &entry);
				entry.value.f = hrz.alt * 2 * M_PI / 360.0;
				command_send(sp_command_el, &entry);

				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);
	
		status_server_fdset_add(stat_srv, &fdset_rx, &high);
		command_server_fdset_add(cmd_srv, &fdset_rx, &high);
		trace_fd_set(traceval_el, &fdset_rx, &high);
		trace_fd_set(traceval_az, &fdset_rx, &high);

		tv.tv_sec = 1;
		tv.tv_usec = 0;

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

		trace_handle(traceval_az, &fdset_rx);
		trace_handle(traceval_el, &fdset_rx);

		command_server_fdset_handle(cmd_srv, &fdset_rx);
		status_server_fdset_handle(stat_srv, &fdset_rx);
		
		output(stat_srv);
	}

}