/*
	Command generator for moon tracking

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

#include <aalib/aalib.h>
#include <utils/weather.h>
#include <utils/dt_model.h>

#include <dt_port_numbers.h>
#include <utils/dt_host.h>
#include <utils/command_server.h>
#include <utils/status_server.h>
#include <log/log.h>


char *command_host = "localhost";
int command_port = CONSOLE_COMMAND_PORT;
int stat_port = CONSOLE_MOON_STAT_PORT;
int cmd_port = CONSOLE_MOON_CMD_PORT;
char *az_command_spg = "Azimuth_Setpoint";
char *el_command_spg = "Elevation_Setpoint";

bool dt_model_enable = false;
bool switch_enabled = false;

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

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

struct weather *weather;


static int handle_cmd(char *name, char *val)
{
	int i;

	printf("name %s value %s\n", name, val);
	for (i = 0; compensation_switches[i].name; i++) {
		if (!strcmp(name, compensation_switches[i].name)) {
			printf("found switch %s\n", 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;
	char statline[800];
	int ret, i;
	char *statlinepos;
	
	now = time(NULL);
	if (now <= last)
		return;
	
	last = now;
	
	statlinepos = statline;
	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);
}




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;
	struct weather *weather;
	double az, el, prev_az = 0;

	time_t lastt = 0;

	signal(SIGPIPE, SIG_IGN);

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

	do {
		sp_command_az = command_open_simple(command_host, command_port,
		    az_command_spg, "console/moontracker",
		    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/moontracker",
		    COMMAND_VALUE_TYPE_FLOAT);
		if (!sp_command_el) {
			printf("Could not open connection for el commands\n");
			sleep(1);
		}
	} while (!sp_command_el);

	dt_model_init();

	aalib_init();


	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_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;
		int high = 0;
		fd_set fdset_rx;
		struct timeval tv;
		
		t = time(NULL);
		if (t != lastt) {
			lastt = t;
			t += 5;
		
//			jul 26 2008 5:39 utc
//			t =  1217050740;
		
			aalib(3, t, 
			    weather_get_temperature(weather),
			    weather_get_pressure(weather),
			    &az, &el);

			az -= 180.0;

			printf("%s", ctime(&t));
			printf("Setpoint: %d %f %f\n\n",
			    (unsigned int)t, az, el);
			if (dt_model_enable) {
				double daz, del;
				
				daz = dt_model_azimuth_delta(az, el);
				del = dt_model_elevation_delta(az, el);
				printf("model: %g %g\n", daz, del);
				az += daz;
				el += del;
			}

			if (switch_enabled) {
				/* Detect 'jump' at north crossing */
				if (fabs(prev_az - az) > 180.0) {
					log_send(LOG_T_WARNING,
					    "Disabling tracker to prevent large azimuth jump");
					switch_enabled = false;
				} else {
					struct command_entry entry;
					
					entry.type = COMMAND_PTYPE_SETPOINT_TIME;
					entry.t.tv_sec = t;
					entry.t.tv_nsec = 0;
					entry.value.f = az * 2 * M_PI / 360.0;
					command_send(sp_command_az, &entry);
					if (el < 0.0)
						el = 0.0;
					entry.value.f = el * 2 * M_PI / 360.0;
					command_send(sp_command_el, &entry);
				}
			} else {
				struct command_entry entry;
				/* not enabled, send idle just in case we were
				   still moving */
				   
				entry.type = COMMAND_PTYPE_SPEED;
				entry.value.f = 0.0;
				command_send(sp_command_az, &entry);
				command_send(sp_command_el, &entry);
			}
			prev_az = az;
		}

		FD_ZERO(&fdset_rx);
	
		command_server_fdset_add(cmd_srv, &fdset_rx, &high);
		status_server_fdset_add(stat_srv, &fdset_rx, &high);
		
		tv.tv_sec = 1;
		tv.tv_usec = 0;

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

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

	}
}