//============================================================================
//
//      A a n d e r a a   W e a t h e r   S t a t i o n
//
//	Reads data from Aanderaa box
//
//
//	Rev.	Date		By	Description
//	-----------------------------------------------
//	0.1	9 Jun 2003	OM	Test version
//
//      Compile like this: cc -o aanderaa aanderaa.c -lm -lpthread
//============================================================================



#define _GNU_SOURCE
#include <getopt.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <string.h>
#include <pthread.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <curses.h>
#include <signal.h>
#include <stdbool.h>
#include <time.h>

#include <sys/socket.h>  //UDP
#include <resolv.h>
#include <arpa/inet.h>
#include <ctype.h>

#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <complex.h>
#include "/home/linacq/OBS2003/shared_memory_common.h"

#define NO_OF_AANDERAA_CHANNELS 11
#define AANDERAA_STRING_LENGTH 65
#define AANDERAA_PARAMETER_FILE "aanderaa.conf"
#define AANDERAA_PORT "/dev/ttyC0"	// ttyCx = Cyclades 8-port card
#define ASCII 0
#define BINARY 1
#define LOGFILE "testlog.txt"		// log file name
#define BUFSIZE		1024
#define TRUE 1
#define FALSE 0
#define METERS_PER_SECOND  0.514444		// 1852 m (1 nm) / 3600 

void read_Aanderaa();
void shared_mem_read();
void initialize_serial_port(int FLAG, int baudrate, const char *com);
void ctrl_c_handler();


//	T y p e   d e c l a r a t i o n  

typedef struct arg_t  {
  const char *com;
  const char *profile;
} arguments;


typedef struct {	// For one line in Aanderaa parameter file
int ch;
char name[50];
int max;
int min;
int avr;
float A;
float B;
float C;
float D;
char units[12];
char format[5];
} parameter;


//	V a r i a b l e    d e c l a r a t i o n

pthread_t	shared_mem_thread;
arguments	shared_mem_arg;

parameter par_list[20];			// Array of parameters for each Aanderaa sensor
pthread_t	aanderaa_thread;
arguments	aanderaa_arg;
FILE *log_fd;
int k=0;
int aanderaa_data_ready=FALSE;
int shared_mem_data_ready=FALSE;

char aanderaa_string[250];
char raw_data_string[250];

char *str_ptr=aanderaa_string;

int running = 1;
void *shared_memory = (void *)0;
struct shared_use_st *shared_stuff;
int shmid;

char GPRMC_string[TEXT_SIZE];
char GPHDT_string[TEXT_SIZE];
int  GPRMC_flag = FALSE;
int  GPHDT_flag = FALSE;

int UDP_alive = FALSE;




//==============================================================
//	              M a i n   p r o g r a m
//=============================================================

int main (int argc, char *argv[]) {

	char buffer[BUFSIZE];
	struct sockaddr_in addr;
	int sd, addr_size, bytes_read;

	char parameter_string[128];
	FILE *fp;

	char test[50];
	int channel;
	int k=1;

	int raw[NO_OF_AANDERAA_CHANNELS];
	float converted[NO_OF_AANDERAA_CHANNELS];
	float Nexp2,Nexp3;
	int i;
	char *p;
	char test_string[TEXT_SIZE];
	char *sub_string;
	char GPRMC_time[25];
	char GPRMC_speed[25];
	char GPRMC_course[25];
	char GPRMC_date[25];
	char GPHDT_heading[25];
	
	int $1,$3,$4,$5;
	char $2[50],$10[12],$11[5];
	float $6,$7,$8,$9;

	char time_string[80];
	#define TIME_STRING_SIZE	25
	char date_str[TIME_STRING_SIZE], time_str[TIME_STRING_SIZE];
	char time_flag;			// If no UDP telegram: Local PC time. 'A'=valid UDP time 'L'=PC local time
	char true_wind_flag; 	// If UDP telegram: 'Y' else 'N'
	char output_string[255];
	time_t local_time;
	struct tm *time_ptr;

	double complex Rel_wind_vector;
	double complex True_wind_vector;
	double complex Ship_vector;
	double complex j;
	double pi, to_degrees, to_radians;
	double Re,Im;
	float rel_wind_speed, rel_wind_dir, true_wind_speed, true_wind_dir, ship_speed, ship_heading;

	
	// Some constants needed when converting relative wind speed/direction to true values, using ship's speed and heading
	pi = 4 * atan(1);		// Tips from Octave
	to_degrees = 180.0/pi;
	to_radians = pi/180.0;
	j = csqrt(-1);


/*	rel_wind_speed = 10.8;
	rel_wind_dir   = 27.0*to_radians;
	ship_speed     = 5.6;
	ship_heading   = 317.0;*/

	// Expressing vectors as complex numbers:
	
	// First setup relative wind vector. 'rel_wind_speed' is vector length, and 'rel_wind_dir' is direction (this
	// is 'module' and 'argument' in complex notation).
	// Transform to x,y values, then use these as real and imaginary part of the complex number.
	// Remember all angles expressed in radians.
	/*Re = rel_wind_speed * cos(rel_wind_dir);
	Im = rel_wind_speed * sin(rel_wind_dir);
	Rel_wind_vector = Re + j*Im;*/
	
	// Setup 'Ship_vector'. Later we'll compensate for ship's heading.
	
	//Ship_vector     = ship_speed + j*0;
	
	// Then get true wind vector:
	/*True_wind_vector = Rel_wind_vector - Ship_vector;

	printf("pi: %f\n",pi);
	printf("real: %f\n", creal(True_wind_vector));
	printf("im: %f\n", cimag(True_wind_vector));
	printf("modul (true wind speed): %f\n", cabs(True_wind_vector));
	true_wind_dir = carg(True_wind_vector)*to_degrees;
	printf("arg: %f\n", true_wind_dir);
	true_wind_dir += ship_heading;
	printf("xxxx True wind dir: %f\n", true_wind_dir);
	if (true_wind_dir > 360.0) true_wind_dir -= 360.0;
	printf("Heading: %f\n", ship_heading);
	printf("xxxx True wind direction: %f\n", true_wind_dir);

	exit(0);*/


	(void) signal(SIGINT, ctrl_c_handler);		// Install custom ctrl-c handler

	//--------------------------------------------------------------------------
	// SHARED MEMORY stuff
	//--------------------------------------------------------------------------

	shmid = shmget((key_t)1234, sizeof(struct shared_use_st), 0666 | IPC_CREAT);
	if (shmid == -1)
	{
		fprintf(stderr, "shmget failed\n");
		exit(EXIT_FAILURE);
	}

	// Attach: Make shared memory accessible to the program

	shared_memory = shmat(shmid, (void *)0, 0);
	if (shared_memory == (void *)-1)
	{
		fprintf(stderr, "shmat failed\n");
		exit(EXIT_FAILURE);
	}

	// SUCCESS, we got shared memory

	fprintf(stderr, "Shared memory ok, attached at %X\n", (int)shared_memory);
	shared_stuff = (struct shared_use_st *)shared_memory;

	// ---------------------------------------------------------------------------
	// Read Aanderaa parameter file and put parameters into data structure.
	// Assumes this parameter file layout:
	// Ch#  Description  Calculations:max,min,avr A B C D Units Format
	// If this pattern is broken the program will probably crash (due to sscanf) !
	//----------------------------------------------------------------------------

	//printf("Parsing parameter file ");
	if((fp = fopen(AANDERAA_PARAMETER_FILE, "r"))==NULL)
	{
		fprintf(stderr, "Cannot open Aanderaa parameter file: %s\n", AANDERAA_PARAMETER_FILE);
		exit(1);
	}
	// Read file line by line ...
	while((fgets(parameter_string, sizeof(parameter_string), fp))!=NULL)
	{
		if ((parameter_string[0] != '#') &&  (parameter_string[0] != ' ')) // Assumes comments start with '#' or ' ' in first coloumn
		{
			// printf(parameter_string);
			sscanf(parameter_string, "%d", &channel);  // Wish AWK was here ...
			channel--;
			sscanf(parameter_string, "%d %s %d %d %d %f %f %f %f %s %s", &$1, $2, &$3, &$4, &$5, &$6, &$7, &$8, &$9, $10, $11);
			par_list[channel].ch     = $1;
			strcpy(par_list[channel].name, $2);		// Strings treated this way
			par_list[channel].max    = $3;
			par_list[channel].min    = $4;
			par_list[channel].avr    = $5;
			par_list[channel].A      = $6;
			par_list[channel].B      = $7;
			par_list[channel].C      = $8;
			par_list[channel].D      = $9;
			strcpy(par_list[channel].units, $10);
			strcpy(par_list[channel].format, $11);
			//printf("Ch.%d: units = %s\n", channel, par_list[channel].units);
		}
	}
	
	fclose(fp);	// Close parameter file


	//----------------------------------------------------------
	// Configure serial ports
	//----------------------------------------------------------

    initialize_serial_port(ASCII, B300, AANDERAA_PORT);

	//------------------------------------------------------------------------------
	// Start threads
	//------------------------------------------------------------------------------

	aanderaa_arg.com = AANDERAA_PORT;
	aanderaa_arg.profile = "";
	pthread_create(&aanderaa_thread, NULL, (void*) read_Aanderaa, (void *) &aanderaa_arg);
	fprintf(stderr,"Thread collecting Aanderaa data .........: ");		// Thread will print 'OK'
	sleep(1);

	shared_mem_arg.com = 0;
	shared_mem_arg.profile = "";
	pthread_create(&shared_mem_thread, NULL, (void*) shared_mem_read, (void *) &shared_mem_arg);
	fprintf(stderr,"Thread collecting shared mem UDP data ...: ");		// Thread will print 'OK'


	//==================================================================================
	//                            M A I N   L O O P
	//==================================================================================


	while (running)
	{
		//--------------------------------------------------------------------------------
		//          P r o c e s s i n g   o f   N M E A   A S C I I   s t r i n g s
		//       r e c e i v e d   f r o m   U D P   v i a   s h a r e d   m e m o r y
		// Expect to get two sentences, shown by example:
		//		$GPRMC,194135,A,6817.26,N,01417.00,E,11.6,020.,070603,00.,E*74
		//		-- format: time (UTC) lat long speed(in knots) CMG date ??
		//		$GPHDT,24.3,T
		//		-- format: heading
		//--------------------------------------------------------------------------------

		// Parsing of $GPRMC telegram starts here (should check for correct number of fields, later ...)
		if (GPRMC_flag == TRUE)
		{
			if (strstr(GPRMC_string, "$GPRMC") != NULL)		// Just checking ...
			{
				//fprintf(stderr,GPRMC_string);
				strcpy(test_string, GPRMC_string);
				// Extract time (token=2), speed (8), course (9), date (10)
				sub_string = strtok(test_string, ",");
				i = 2; // Token counter
				while ((sub_string=strtok(NULL, ",")) != NULL)
				{
					switch (i)
					{
						case 2:		strcpy(GPRMC_time, sub_string);
									break;
						case 3:
						case 4:
						case 5:
						case 6:
						case 7:		break;
						case 8:		strcpy(GPRMC_speed, sub_string);
									break;
						case 9:		strcpy(GPRMC_course, sub_string);
									break;
						case 10:	strcpy(GPRMC_date, sub_string);
									break;
						default:	break;
					}
					i++;
				}
				//fprintf(stderr, "%s %s %s %s\n", GPRMC_time, GPRMC_date, GPRMC_speed, GPRMC_course);
				GPRMC_flag = FALSE;
			}
		}
		// Parsing of $GPHDT telegram starts here (should check for correct number of fields, later ...)
		if (GPHDT_flag == TRUE)		
		{
			if (strstr(GPHDT_string, "$GPHDT") != NULL)		// Just checking ...
			{
				//fprintf(stderr,GPHDT_string);
				strcpy(test_string, GPHDT_string);
				// Extract heading (token=2)
				sub_string = strtok(test_string, ",");
				i = 2; // Token counter
				while ((sub_string=strtok(NULL, ",")) != NULL)
				{
					switch (i)
					{
						case 2:		strcpy(GPHDT_heading, sub_string);
									break;
						default:	break;
					}
					i++;
				}
				//fprintf(stderr, "%s\n", GPRMC_heading);
				GPHDT_flag = FALSE;
			}
		}

		//----------------------------------------------------------------------------------------------------------
		//                   P r o c e s s i n g   o f   A a n d e r a a   d a t a 
		// NOTE: When time permits, allow sensor channel sequence to change by reading this information from
		// Aanderaa config file. Now I'm using arrays to store raw- and converted values, and indexing assumes
		// a certain sensor sequence. Could try to index via sensor name enumeration as an intermediate step.
		//----------------------------------------------------------------------------------------------------------

		if (aanderaa_data_ready == TRUE)	// Aanderaa thread will set this flag
		{
			// First convert ASCII raw data to integers, then put these into array raw[0..(n-1)],
			// where n = number of sensor channels.
			// If file format is changed program will probably go down in flames
			// as sscanf expects that input match format specifier. Someone must make it more robust ...

			sscanf(raw_data_string, "%d %d %d %d %d %d %d %d %d %d %d", &raw[0],&raw[1],&raw[2],&raw[3],&raw[4],&raw[5],&raw[6],&raw[7],&raw[8],&raw[9],&raw[10]);

			// Loop through all raw data integers and calculate physical parameter using this expression:
			//                  Value = A + (B*N) + (C*Nexp2) + (D*Nexp3)
			// where N = raw data reading and A, B, C, D are coefficients stored in the 'par_list[]' array.
			// Converted values stored in the 'converted[]' array.

			for (i=0; i<NO_OF_AANDERAA_CHANNELS; i++)
			{
				Nexp2 = raw[i] * raw[i];
				Nexp3 = Nexp2 * raw[i];
				converted[i] = par_list[i].A + (par_list[i].B * raw[i]) + (par_list[i].C * Nexp2) + (par_list[i].D * Nexp3);
				fprintf(stderr, "input: %d, output: %.1f %s\n", raw[i], converted[i], par_list[i].units);
			}

			// If we receive info on ship's heading via UDP and shared memory, calculate true wind speed
			// and direction.
			UDP_alive = TRUE;  // Testing ...
			
			if (UDP_alive == TRUE)
			{
				//----------------------------------------------------------------------------
				// Time- and date format in UDP telegrams:      235659    301202
				// Convert this to ease later handling of data: 23:56:59  2003-12-30
				// Using brute force:
				//----------------------------------------------------------------------------
				time_str[0] = GPRMC_time[0]; time_str[1] = GPRMC_time[1]; time_str[2] = ':';
				time_str[3] = GPRMC_time[2]; time_str[4] = GPRMC_time[3]; time_str[5] = ':';
				time_str[6] = GPRMC_time[4]; time_str[7] = GPRMC_time[5]; time_str[8] = '\0';
				
				date_str[0] = '2'; date_str[1] = '0';		// Oh no a year 2100 problem ...
				date_str[2] = GPRMC_date[4]; date_str[3] = GPRMC_date[5]; date_str[4] = '-';
				date_str[5] = GPRMC_date[2]; date_str[6] = GPRMC_date[3]; date_str[7] = '-';
				date_str[8] = GPRMC_date[0]; date_str[9] = GPRMC_date[1]; date_str[10] = '\0';
				time_flag   = 'A';
				//fprintf(stderr,"UDP time conversion ok\n");
				
				//--------------------------------------------------------------------------------------------	
				// Expressing vectors as complex numbers:
				// First setup relative wind vector. 'rel_wind_speed' is vector length, and 'rel_wind_dir' is
				// direction (this is 'module' and 'argument' in complex notation).
				// Transform to x,y values, then use these as real and imaginary part of the complex number.
				// Remember all angles expressed in radians.
				//--------------------------------------------------------------------------------------------
				
				rel_wind_speed = converted[1];							// This is fragile. And remember we index from 0.
				rel_wind_dir   = converted[2]*to_radians;
				ship_speed     = converted[3];							// Use speed info from Aanderaa system
				//ship_speed     = atof(GPRMC_speed)*METERS_PER_SECOND;	// Bad: no input validation
				ship_heading   = atof(GPHDT_heading);
				fprintf(stderr, "UDP telegrams: Ship speed: %.1f, heading: %.1f\n", ship_speed, ship_heading);
				
				Re = rel_wind_speed * cos(rel_wind_dir);
				Im = rel_wind_speed * sin(rel_wind_dir);
				Rel_wind_vector = Re + j*Im;
				// Setup 'Ship_vector' 
				Ship_vector     = ship_speed + j*0;
				//fprintf(stderr,"Vector setup complete\n");
				// Then get true wind vector:
				True_wind_vector = Rel_wind_vector - Ship_vector;
				true_wind_speed = cabs(True_wind_vector);
				true_wind_dir = carg(True_wind_vector)*to_degrees;
				true_wind_dir += ship_heading;
				fprintf(stderr,"Wind dir, before check: %.1f\n", true_wind_dir);
				if (true_wind_dir < 0.0) true_wind_dir = (360.0 + true_wind_dir); // We're actually subtracting here ..
				if (true_wind_dir > 360.0) true_wind_dir -= 360.0;		// Later versions: take CMG into account
				true_wind_flag = 'Y';
				//fprintf(stderr,"Vector calculations complete\n");
			}
			else	// No UDP data, use PC internal clock and signal this with time_flag, set 'true_wind_dir' and 'true_wind_speed' to 0.0
			{
				local_time = time(NULL);			// Would be nice if PC time was ntp'ed ...
				time_ptr = localtime(&local_time);
				//strftime(time_string, 100, "%T %F", time_ptr);
				strftime(time_str, TIME_STRING_SIZE, "%T", time_ptr);	// Like "23:50:52"
				strftime(date_str, TIME_STRING_SIZE, "%F", time_ptr);	// Like "2002-11-23"
				time_flag       = 'L';
				true_wind_speed = 0.0;
				true_wind_dir   = 0.0;
				true_wind_flag  = 'N';
			}
			//---------------------------------------------------------------------------------------------------------
			// Format ioutput string. Output format example:
			// $PSHWEA,MOSBY,19:41:35,2003-06-07,A,1003.8,8.9,85.2,4.3,11.8,350.7,A*<checksum, 1 byte>
			// Sensor sequence: Barometric pressure, air temp, rel. humidity, sea temp, true wind speed, true wind dir
			//---------------------------------------------------------------------------------------------------------

			sprintf(output_string, "$PSHWEA,MOSBY,%s,%s,%c,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%c*\n",time_str,date_str,time_flag,converted[5],converted[6],converted[7],converted[8],true_wind_speed,true_wind_dir,true_wind_flag);
			write(1, output_string, strlen(output_string));
			fprintf(stderr, output_string);
			//sprintf(output_string, "%s %.1f\n", time_string, converted[5]);
			//fprintf(stdout, output_string);
			//write(1, output_string, strlen(output_string));
			//fprintf(stderr,"\nRel. Wind dir.: %.0f, Rel. wind speed: %.1f\n", converted[2], converted[1]);
			//fprintf(stderr, "%s %s %s %s %s\n\n", GPRMC_time, GPRMC_date, GPRMC_speed, GPRMC_course, GPRMC_heading);
			aanderaa_data_ready = FALSE;
		}
		usleep(500000);
		//fprintf(stderr, "!");
	}


	//-------------------------------------------------------
	// TERMINATE PROGRAM, first clean up shared memory stuff
	//-------------------------------------------------------

	if (shmdt(shared_memory) == -1)		// Detach shared memory
	{
		fprintf(stderr, "\nshmdt failed\n");	// Didn't work
		exit(EXIT_FAILURE);
	}

	fprintf(stderr, "\nDetached shared memory\n");

	exit(EXIT_SUCCESS);
}


//==================================================================
//	T H R E A D   Aanderaa serial data
//==================================================================

void read_Aanderaa(arguments *arg)
{

	FILE *a_fd;
	char *bytes;

	a_fd = fopen(arg->com,"r");
	fprintf(stderr,"Aanderaa thread ok\n");
	while (1)
	{
		bytes = fgets(aanderaa_string, sizeof aanderaa_string, a_fd);
		//printf(">>\n");
		if (aanderaa_string[0] != '\x0a')   // Skip blank line; Aanderaa box terminates lines by <LF><LF>
		{
			if (strlen(aanderaa_string) != AANDERAA_STRING_LENGTH)  //Must ensure that string is complete
			{
				//printf("! %i char: %s\n", strlen(aanderaa_string), aanderaa_string);
			}
			else
			{
				strcpy(raw_data_string, aanderaa_string);	// aanderaa_string did not make it to main loop ....
				//printf(">> %s\n", aanderaa_string);
				aanderaa_data_ready = TRUE;  //Let Main loop handle this

			}
		}
    }
}

//=====================================================================================
//	T H R E A D   Read shared memory data, containing NMEA telegrams received via UDP.
//                Another program receives UDP and writes telegrams to shared memory
//=====================================================================================

void shared_mem_read(arguments *arg)
{
	fprintf(stderr,"Shared memory thread ok\n");
	while (1)
	{
		if (shared_stuff->GPHDT_ready == 1)			// Looking for $GPHDT telegram
		{
			strcpy(GPHDT_string, shared_stuff->UDP_GPHDT);
			shared_stuff->GPHDT_ready = 0;
			GPHDT_flag = TRUE;
		}

		if (shared_stuff->GPRMC_ready == 1)			// Looking for $GPRMC telegram
		{
			strcpy(GPRMC_string, shared_stuff->UDP_GPRMC);
			shared_stuff->GPRMC_ready = 0;
			GPRMC_flag = TRUE;
		}
		usleep(500000);
	}
	exit(0);
}

//=========================================================
//	I n i t i a l i z e   s e r i a l   p o r t s
//
//	Binary mode must be re-designed - allow setting of
//	VMIN & VTIME for each serial port ....
//=========================================================

void initialize_serial_port(int FLAG, int baudrate, const char *com) {
  // FLAG = 0, configure serial port com for ascii data
  // FLAG = 1, configure serial port com for binary stream
  int fd;
  struct termios options;
  fd = open(com, O_RDWR | O_NOCTTY );
  if (fd <0) {
     perror(com);
     exit(-1);
  }
  if (FLAG == ASCII) {
    tcgetattr(fd,&options);      /* Get current setting */
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE; /* Mask the character size bits */
    options.c_cflag |= CS8;    /* Select 8 data bits */
    options.c_lflag |= (ICANON | ECHO | ECHOE);
    cfsetispeed(&options, baudrate);
    cfsetospeed(&options, baudrate);
    tcsetattr(fd, TCSANOW, &options); /* Apply changes */
    /* Finished serial settings */
  }
 else if (FLAG == BINARY) {
    tcgetattr(fd,&options);      /* Get current setting */
    cfsetispeed(&options, B9600);
    cfsetospeed(&options, B9600);

	//  c_cflag section
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE; /* Mask the character size bits */
    options.c_cflag |= CS8;    /* Select 8 data bits */

    options.c_cc[VMIN] = 228;  // 228 = NORSTAR format, bygg om, tenk paa GUNCO ogsaa!!
    options.c_cc[VTIME] = 7;

    // c_lflag section
	options.c_lflag &= ~ICANON;
	options.c_lflag &= ~ECHO;
	options.c_lflag &= ~ECHOE;
	options.c_lflag &= ~ECHONL;
	options.c_lflag &= ~ISIG;
	
	// c_iflags section
	options.c_iflag |= IGNBRK;
	options.c_iflag &= ~IGNCR;
	options.c_iflag |= IGNPAR;
	options.c_iflag &= ~INLCR;
	options.c_iflag &= ~ICRNL;
	options.c_iflag &= ~INPCK;
	options.c_iflag &= ~PARMRK;
	options.c_iflag &= ~ISTRIP;
	options.c_iflag &= ~IXOFF;
	options.c_iflag &= ~IXON;

    tcsetattr(fd, TCSANOW, &options); /* Apply changes */

    /* Finished serial settings */
  }
  close(fd);
}

//------------------------------------------------------------------
//	CTRL-C  handler. Program can then stop in an orderly way.
//------------------------------------------------------------------

void ctrl_c_handler(int sig)		// Executed on first ctrl-c
{
	running = 0;			// Flag used in main loop
	(void) signal(SIGINT, SIG_DFL);	// Next ctrl-c will act as usual
}