//CNCExtension Hal Component

component cncextension                 "Comunication with the CNCextension board";

pin in bit ch0;
pin in bit ch1;
pin in bit ch2;

pin in bit wr0;
pin in bit wr1;
pin in bit wr2;
pin in bit wr3;

pin in bit opt;

pin in signed pwm;
pin in bit pwmPrime;

pin out bit button2;
pin out signed encoder;

pin in bit enableMpg = 0;

pin out bit xEnable = 1;
pin out bit yEnable = 0;
pin out bit zEnable = 0;

option singleton yes;               // makes no sense to have more than one of these components running
option userspace yes;

license "GPL";
;;

#include <stdio.h>    /* Standard input/output definitions */
#include <stdlib.h> 
#include <stdint.h>   /* Standard types */
#include <string.h>   /* String function definitions */
#include <unistd.h>   /* UNIX standard function definitions */
#include <fcntl.h>    /* File control definitions */
#include <errno.h>    /* Error number definitions */
#include <termios.h>  /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <hal.h>
#include <sys/types.h>       // For data types
#include <sys/socket.h>      // For socket(), connect(), send(), and recv() 
#include <arpa/inet.h>       // For inet_addr()
#include <netinet/in.h>      // For sockaddr_in
#include <netinet/tcp.h>     // TCP_KEEPCNT
#include <netdb.h>

#define _POSIX_SOURCE 1 /* POSIX compliant source */

static bool stop = false;

typedef struct MicroState
{
	hal_bit_t ch0state; 
	hal_bit_t ch1state; 
	hal_bit_t ch2state; 

	hal_bit_t wr0state; 
	hal_bit_t wr1state; 
	hal_bit_t wr2state; 
	hal_bit_t wr3state; 
	
	hal_bit_t optState; 
	
	hal_bit_t mpgState; 
	
}MicroState;

int btRead(int fd, char* buffer, int readnum, int useconds)
{
	struct timeval timeout;
	timeout.tv_sec = 0;
	timeout.tv_usec = useconds;
	
	fd_set set;
	FD_ZERO(&set);
	FD_SET(fd, &set);
	
	int rv = select(fd + 1, &set, NULL, NULL, &timeout);
	
	if(rv > 0) return read(fd, buffer, readnum);
	else return -1;
}

bool isOk( int serial )
{
	usleep(5000);
	char inBuffer[16] = "FN0000000000000\0";
	if(btRead(serial, inBuffer, 15, 1000) > 2 && strstr(inBuffer, "OK") != NULL) 
	{
		printf("got OK\n");
		return true;
	}
	else
	{
		printf("failed OK: got: %s\n", inBuffer);
		return false;
	}
}

void updatePins( MicroState* state, int serial, struct __comp_state *__comp_inst )
{
	char buffer[8];
	bzero(buffer, 8);  
	
	
	tcflush(serial, TCIOFLUSH);
	
	if(state->ch0state !=  ch0)  
	{
		sprintf(buffer, "\nCH0_%d\n", ch0);
		write(serial, buffer, strlen(buffer));  
		if(isOk(serial))state->ch0state = ch0;
	} 
	else if( ch1 !=  state->ch1state)  
	{
		sprintf(buffer, "\nCH1_%d\n", ch1);
		write(serial, buffer, strlen(buffer));
		if(isOk(serial))state->ch1state = ch1;
	} 
	else if( ch2 !=  state->ch2state)  
	{
		sprintf(buffer, "\nCH2_%d\n", ch2);
		write(serial, buffer, strlen(buffer)); 
		if(isOk(serial))state->ch2state = ch2;
		
	} 
	else if( wr0 !=  state->wr0state)  
	{
		sprintf(buffer, "\nWR0_%d\n", wr0);
		write(serial, buffer, strlen(buffer));
		/*if(isOk(serial))*/ state->wr0state = wr0;
	} 
	else if( wr1 !=  state->wr1state)  
	{
		sprintf(buffer, "\nWR1_%d\n", wr1);
		write(serial, buffer, strlen(buffer)); 
		/*if(isOk(serial))*/ state->wr1state = wr1;
	} 
	else if( wr2 !=  state->wr2state)  
	{
		sprintf(buffer, "\nWR2_%d\n", wr2);
		write(serial, buffer, strlen(buffer));
		/*if(isOk(serial))*/ state->wr2state = wr2;
	}
	else if( wr3 !=  state->wr3state)  
	{
		sprintf(buffer, "\nWR3_%d\n", wr3);
		write(serial, buffer, strlen(buffer));
		/*if(isOk(serial))*/ state->wr3state = wr3;
	}
	else if( opt !=  state->optState)  
	{
		sprintf(buffer, "\nOPT_%d\n", opt);
		write(serial, buffer, strlen(buffer));
		if(isOk(serial)) state->optState = opt;
	}
	else if(enableMpg != state->mpgState)
	{
		if(enableMpg) write(serial, "\nST_ON\n", strlen("\nST_ON\n"));
		else
		{
			write(serial, "\nST_OFF\n", strlen("\nST_OFF\n"));
			xEnable = 0;
			yEnable = 0;
			zEnable = 0;
			encoder = 0;
		}
		state->mpgState = enableMpg;
	}
}

void mpg(int serial, struct __comp_state *__comp_inst)
{
	char buffer[64]="DND RE";
	if(btRead(serial, buffer, 64, 100) >= 7 && strncmp(buffer, "MPG_", 3) == 0)
	{
		if( buffer[4] == '0' ) 
		{   
			xEnable = 1;
			yEnable = 0;
			zEnable = 0;
		}
		else if( buffer[4] == '1' ) 
		{   
			xEnable = 0;
			yEnable = 1;
			zEnable = 0;
		}
		else if( buffer[4] == '2' ) 
		{   
			xEnable = 0;
			yEnable = 0;
			zEnable = 1;
		}
		button2 = buffer[5] == '1';
		encoder = atoi(buffer+6);
	}
}

static int fillAddr(const char address[], unsigned short port, struct sockaddr_in *addr) 
{
	memset(addr, 0, sizeof(addr));
	addr->sin_family = AF_INET;       // Internet address

	struct hostent *host;  // Resolve name
	if ((host = gethostbyname(address)) == NULL) 
	{
		return -1;
	}
	addr->sin_addr.s_addr = *((unsigned long *) host->h_addr_list[0]);

	addr->sin_port = htons(port);     // Assign port in network byte order
	return 0;
}

void pwmFunc(int fd, struct __comp_state *__comp_inst)
{
	static unsigned i = 0;
	static bool pwmPrimeLast = false;
	static bool on = false;
	++i;
	if(i >= 1000) i = 0;
	if(i == 0 && pwm > 0 || (pwmPrime && !pwmPrimeLast))
	{
		char mesg[] = "\nPWM_05\n";
		do 
		{
			write(fd, mesg, sizeof(mesg));
		} while(!isOk(fd));
		pwmPrimeLast = pwmPrime;
		on = true;
	}
	else if(!pwmPrime && on && (i == pwm*10 || pwmPrimeLast || pwm == 0))
	{
		char mesg[] = "\nPWM_00\n";
		do 
		{
			write(fd, mesg, sizeof(mesg));
		} while(!isOk(fd));
		pwmPrimeLast = pwmPrime;
		on = false;
	}
}

void user_mainloop(void)
{
	MicroState state;
	memset(&state, 0, sizeof(state));
	FOR_ALL_INSTS()  //needed for resaons
	{ 
		int tcpsocket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
		
		struct sockaddr_in destAddr;
		fillAddr("localhost", 6856, &destAddr);
		
		// Try to connect to serial multiplexer
		if (connect(tcpsocket, (struct sockaddr*) &destAddr, sizeof(destAddr)) < 0) 
		{
			printf("can not connect to server\n");
			tcpsocket = -1;
		}
		
		write(tcpsocket, "\nST_OFF\n", strlen("\nST_OFF\n"));
		
		while(tcpsocket != -1)
		{
			usleep(5000);
			updatePins(&state, tcpsocket, __comp_inst);
			mpg(tcpsocket, __comp_inst);
			pwmFunc(tcpsocket, __comp_inst);
		}
		close(tcpsocket);
	}
	exit(0);
}