#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <poll.h>

#include <math.h>

#define BEAGLE_REAL
#define PORTNUM 42000

int state=0;
#define STATE_STARTUP 0     /* System not stable yet, wait */
#define STATE_TIME_RDY 1    /* Time was set by network, network should be up */

/* Max number of clients */
#define MAX_CONNECTS 20

int debugm=0;
int ledUpdate=0;
FILE *logConnects=NULL;

struct pollfd pfds[MAX_CONNECTS];
time_t connectTime[MAX_CONNECTS];
int connectStatus[MAX_CONNECTS];        /* Where are we in authorization process. */
					/* 0 is not used */
					/* 1 is connect set */
					/* 2 is "OKAY" has been received && */
int numPfds;

char *YN = "NY";
char *doorPos[] = {"OPEN", "UNKNOWN", "CLOSED"};
char *lightPos[] = {"OFF", "ON", "TIMED", "ON"};
char *faultStrings[] = {"", "NOSTART", "TOOLONG" };

char wbuf[2048];                                    /* Generic buffer for writing */
#define UNKNOWN 0
#define CLOSED 1
#define OPEN -1
#define OFF 0
#define ON_MANUAL 1
#define ON_TIMED 2
#define NOFAULT 0
#define NOSTART 1
#define TOOLONG 2
int lightStatus=OFF, doorStatus=UNKNOWN, doorEnable=1;
int lightTarget=OFF, doorTarget=UNKNOWN, enableTarget=1;
int doorPowerGPIO, doorButtonGPIO, lightGPIO, senseOpenGPIO, senseCloseGPIO;
int doorSensorInhibit=0, fault=NOFAULT;
time_t autoLightOffTime, doorActivateTime;
int lightOnTime = 300;  /* 5 minutes */
int maxDoorOpenTime, maxDoorCloseTime;
static char lastOpenClose[128];
/*
#define BEAGLE_REAL
*/

/* Parse string of form 12:30 into number of minutes past midnight */
static int parseTime(char *s)
{
int j, k, minutes;

    for(j=0; j<10 && s[j]; j++)
	{
	if( s[j] == '-' )
	    return(2000);   /* Not set */
	if( s[j] == ':' )
	    {
	    minutes = atoi(s) * 60;
	    goto MIN;
	    }
	}
    return(2000);   /* Badly formatted */

MIN:
    j=j+1;
    for(k=j; s[k] && k < 10; k++)
	;
    if( s[k] )
	return(2000);
    return( atoi(&s[j]) + minutes);
}

void makeMinuteString(int t, char *s)
{
int hours, minutes;

    if( t < 0 || t >= 1440 )
	strcpy(s, "-");
    else
	{
	hours = t/60;
	minutes = t%60;
	sprintf(s, "%02d:%02d", hours, minutes);
	}
}

setupListenSocket(void)
{
int i, listenSocket;
unsigned int x;
unsigned char *flipit, sv;
struct sockaddr_in in;
struct sockaddr_in6 in6;

    listenSocket = socket(PF_INET6, SOCK_STREAM, 0);
    x = INADDR_ANY;
    memcpy((void *) &(in6.sin6_addr), (void *) &in6addr_any, sizeof(in6));
    in6.sin6_family = AF_INET6;
    in6.sin6_port = PORTNUM;
    flipit = (unsigned char *) &in6.sin6_port;
    sv = flipit[0];
    flipit[0] = flipit[1];
    flipit[1] = sv;

    if( bind(listenSocket, (const struct sockaddr*) &in6, sizeof(in6)) != 0 )
	{
	printf("Failed to bind listen socket\n");
	return(0);
	}
    if( listen(listenSocket, 5) != 0 )
	{
	printf("Failed to set up listen on socket\n");
	return(0);
	}
    pfds[0].fd = listenSocket;
    pfds[0].events = POLLIN | POLLOUT;
    numPfds=1;
    return(1);
}

/* Write the buffer to the remote if it is still active */
sendCmd(char *buf, int ix)
{
int len;

    if( pfds[ix].fd != -1 )
	{
	len = strlen(buf);
	if( write(pfds[ix].fd, buf, len) != len)
	    {
	    close(pfds[ix].fd);
	    pfds[ix].fd = -1;
	    connectStatus[ix] = 0;
	    printf("Write failed on ix %d; closing\n", ix);
	    }
	}
}


/* Process command coming from pfds index ix */
processCmd(char *cmd, time_t curTime, int ix)
{
char name[24], flag[24], RType[24];
int i, j, req;
double R25, RRef, ROffset;

    if( strncasecmp(cmd, "LON", 3) == 0)
	{
	lightTarget |= ON_MANUAL;
	}
    else if( strncasecmp(cmd, "LOFF", 4) == 0)
	{
	lightTarget = OFF;
	}
    else if( strncasecmp(cmd, "DOPEN", 5) == 0)
	{
	if( doorStatus != OPEN )
	    {
	    if( clickDoorButton(OPEN) )
		{
		autoLightOffTime = curTime + lightOnTime;
		lightTarget |= ON_TIMED;
		if( doorStatus != UNKNOWN )
		    doorSensorInhibit = 1;
		doorActivateTime = curTime;
		fault = NOFAULT;
		}
	    }
	}
    else if( strncasecmp(cmd, "DCLOSE", 6) == 0)
	{
	if( doorStatus != CLOSED )
	    {
	    if( clickDoorButton(CLOSED) )
		{
		autoLightOffTime = curTime + lightOnTime;
		lightTarget |= ON_TIMED;
		if( doorStatus != UNKNOWN )
		    doorSensorInhibit = 1;
		doorActivateTime = curTime;
		fault = NOFAULT;
		}
	    }
	}
    else if( strncasecmp(cmd, "DENABLE", 7) == 0)
	{
	enableTarget = 1;
	}
    else if( strncasecmp(cmd, "DDISABLE", 8) == 0)
	{
	enableTarget = 0;
	}
    else    /* A status request */
	printf("Bad command %s\n", cmd);

    return(1);
}

/* Called after connection established so therm knows what current state is */
sendAll(int ix, time_t curTime)
{
char msg[1024];

/* Send target temps first, so client has them when mode is sent */
	sprintf(msg, "RDY %c %s %s %s",  YN[doorEnable], doorPos[doorStatus+1], lightPos[lightStatus], ctime(&curTime));
	sendCmd(msg, ix);
	sprintf(msg, "QD %s\n", lastOpenClose);
	sendCmd(msg, ix);
}

processStatus(char *cmd, time_t curTime, int ix)
{
int i, j, k, t;

    if( ix <= 0 || ix >= numPfds )
	return(0);
    if( pfds[ix].fd == -1 )
	return(0);
    if( strncasecmp(cmd, "ISTATUS", 5) == 0)
	{   /* Send door position back */
	if( fault == NOFAULT )
	    {
	    if( doorActivateTime )
		sprintf(wbuf, "QSTATUS MOVE%d %c %s\n", ((int)(curTime-doorActivateTime)),
			 YN[doorEnable], lightPos[lightStatus]);
	    else
		sprintf(wbuf, "QSTATUS %s %c %s\n", doorPos[doorStatus+1], YN[doorEnable], lightPos[lightStatus]);
	    }
	else
	    sprintf(wbuf, "QSTATUS %s %c %s\n", faultStrings[fault], YN[doorEnable], lightPos[lightStatus]);
	sendCmd(wbuf, ix);
	}
}

/* Click the door button for 1 second to initiate an open/close */
/* Note one shot will time it to be about 1/2 second to door opener */
clickDoorButton(int target)
{
char gpioString[128];
FILE *fp;

    if( doorEnable == 0 )
	{
	if( debugm )
	    printf("Can't initiate door movement when door is disabled\n");
	return(0);
	}
    sprintf(gpioString, "/sys/class/gpio/gpio%d/direction", doorButtonGPIO);
    fp = fopen(gpioString, "w");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open door button gpio\n");
	return(0);
	}
    if( debugm )
	printf("Depress door button\n");
    fprintf(fp, "high\n");
    fclose(fp);
    usleep(1000);   /* Wait 1 mSec to ensure one shot trigger */
    fp = fopen(gpioString, "w");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open door button gpio\n");
	}
    else
	{
	if( debugm )
	    printf("Release door button\n");
	fprintf(fp, "low\n");
	fclose(fp);
	}
    doorTarget = target;
    return(1);
}

updateDoorStatus(time_t curTime, const struct tm *tm)
{
char gpioString[128];
int state, newDoorStatus;
FILE *fp;

    sprintf(gpioString, "/sys/class/gpio/gpio%d/value", senseOpenGPIO);
    fp = fopen(gpioString, "r");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open sense open switch gpio\n");
	return(0);
	}
    newDoorStatus = UNKNOWN;
    if( fscanf(fp, "%d", &state) == 1 && state == 0 )
	newDoorStatus = OPEN;
    fclose(fp);

    sprintf(gpioString, "/sys/class/gpio/gpio%d/value", senseCloseGPIO);
    fp = fopen(gpioString, "r");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open sense close switch gpio\n");
	return(0);
	}
    if( fscanf(fp, "%d", &state) == 1 && state == 0 )
	{
	if( newDoorStatus == OPEN )
	    newDoorStatus = UNKNOWN;    /* Should not happen, both switches closed */
	else
	    newDoorStatus = CLOSED;
	}
    fclose(fp);
    if( doorStatus != newDoorStatus && curTime != 0 )
	{   /* Door changed position, probably external click so turn on light */
	autoLightOffTime = curTime + lightOnTime;
	lightTarget |= ON_TIMED;
	sprintf(lastOpenClose, "%d/%d %d:%d %s", tm->tm_mon+1, tm->tm_mday, tm->tm_hour,
					tm->tm_min, doorPos[1+newDoorStatus]);
	}
    doorStatus = newDoorStatus;
    if( doorStatus != UNKNOWN && doorActivateTime + 2 < curTime ) /* Wait at least 2 seconds to check state of door */
	doorSensorInhibit = 0;
    if( doorActivateTime )
	{
	if( debugm )
	    {
	    printf("Door motor running for %d\n", ((int)(curTime-doorActivateTime)));
	    }
	if( doorSensorInhibit == 0 )
	    {
	    if( doorStatus == doorTarget )
		{
		if( debugm )
		    printf("Door motor stopped after %d\n", ((int)(curTime-doorActivateTime)));
		doorActivateTime = 0;
		fault = NOFAULT;
		}
	    else if( doorStatus != UNKNOWN )
		fault = NOSTART;
	    else if( doorTarget == OPEN )
		{
		if( doorActivateTime + maxDoorOpenTime > curTime )
		    fault = TOOLONG;
		}
	    else if( doorTarget == CLOSED )
		{
		if( doorActivateTime + maxDoorCloseTime > curTime )
		    fault = TOOLONG;
		}
	    }
	}
    return(1);
}

/* Actually trigger the relays */
static void usrled(int which, int state)
{
char fName[1024];
FILE *fp;

    if( state )
	state = 1;
    sprintf(fName, "/sys/class/leds/beaglebone:green:usr%d/brightness", which+1);
    if( (fp=fopen(fName, "w")) == NULL)
	{
	if( debugm & 0x2 )
	    printf("Can't open %s\n", fName);
	}
    else
	{
	fprintf(fp, "%d\n", state);
	fclose(fp);
	}
}

updateLight(time_t curTime)
{
char gpioString[128];
FILE *fp;

    /* Should the light go off ? */
    if( lightStatus & ON_TIMED )
	{
	if( curTime > autoLightOffTime )
	    lightTarget = lightTarget & ~ON_TIMED;
	}
    if( lightTarget == lightStatus )
	return(1);
    sprintf(gpioString, "/sys/class/gpio/gpio%d/direction", lightGPIO);
    fp = fopen(gpioString, "w");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open light switch switch gpio\n");
	return(0);
	}
    if( lightTarget )
	fprintf(fp, "high\n");
    else
	fprintf(fp, "low\n");
    fclose(fp);
    lightStatus = lightTarget;
    return(1);
}

updateEnable()
{
char gpioString[128];
FILE *fp;

    sprintf(gpioString, "/sys/class/gpio/gpio%d/direction", doorPowerGPIO);
    fp = fopen(gpioString, "w");
    if( fp == NULL )
	{
	if( debugm )
	    printf("Failed to open door enable switch gpio\n");
	return(0);
	}
    if( enableTarget == 1 )     /* Enabled is low or inactive. Failsafe in case bone dies */
	fprintf(fp, "low\n");
    else
	fprintf(fp, "high\n");
    fclose(fp);
    doorEnable = enableTarget;
    return(1);
}

updateLEDS(int i)
{
static int doorLED=-1, lightLED=-1;

    /* Normally off, LED's controlled by irrigation normally */
    if( !ledUpdate )
	return;
    if( !(state & STATE_TIME_RDY) )
	{
	if( i & 0x1 )
	    {
	    usrled(0, 1);
	    usrled(1, 0);
	    }
	else
	    {
	    usrled(0, 0);
	    usrled(1, 1);
	    }
	return;
	}
    if( doorEnable != doorLED )
	{
	usrled(0, doorEnable);
	doorLED = doorEnable;
	}
    if( lightStatus != lightLED )
	{
	usrled(1, lightStatus);
	lightLED = lightStatus;
	}

}

static void doTargets(time_t curTime)
{

    if( lightStatus != lightTarget || lightStatus & ON_TIMED)
	updateLight(curTime);
    if( doorEnable != enableTarget )
	updateEnable();


}

looper(void)
{
const struct tm *tm;
struct tm tmx;
struct sockaddr_storage from;
struct sockaddr *from4;
struct sockaddr_in *from4in;
struct sockaddr_in6 *from6in;
unsigned char *flipit, sv;

int i, ii, j, k, dir, offset, rc, rcx, len, updated;
int shift;
unsigned int check;
char cmdBuffer[2048];
time_t curTime, initTime, lastTime;
int loopCntr=0;
FILE *fp;
unsigned int *peerIP32;
unsigned char peerIP64[16];


    peerIP32 = (unsigned int *) peerIP64;
    from4 = (struct sockaddr *) &from;
    from4in = (struct sockaddr_in *) &from;
    from6in = (struct sockaddr_in6 *) &from;

    time(&initTime);
    if( debugm )
    	printf("INITTIME %u %s\n", initTime, ctime(&initTime));
    lastTime = initTime;


again:
    rc = poll(pfds, numPfds, 1000);     /* 1 second between checks */
    time(&curTime);
    tm = localtime(&curTime);

    if( !(state & STATE_TIME_RDY ) )
	{   /* Could end up being up to 2 minutes if time gets inited after exactly 1 minutes */
	if( curTime - lastTime > 60 )
	    {
	    if( debugm )
		printf("Probably time adjust; ignored and reset initTime to %s", ctime(&curTime));
	    initTime = curTime;
	    }
	lastTime = curTime;

	if( curTime - initTime > 60 )
	    {
	    if( debugm )
		{
		printf("TIME RDY %s\n", ctime(&curTime));
		fflush(stdout);
		}
	    state |= STATE_TIME_RDY;
	    }
	}

    if( loopCntr == 10 )
	{
	loopCntr=0;
	}
    if( loopCntr == 0 || doorActivateTime + maxDoorOpenTime > curTime )
	updateDoorStatus(curTime, tm); /* Do once every 10 seconds, no need to check that often unless door in motion */
    doTargets(curTime);
    updateLEDS(loopCntr);
    loopCntr++;

    if( pfds[0].revents & POLLIN )  /* Somebody wants to connect */
	{
	len = sizeof(struct sockaddr_storage);
	/* 2 INET, 10 is INET6 */
	rcx = accept(pfds[0].fd, from4, &len);
	if( rcx >= 0 )
	    {   /* Add it in to the group we watch */
	    memset(peerIP64, 0, 16);
	    if( from4in->sin_family == AF_INET6 )
		{
		memcpy(peerIP64, from6in->sin6_addr.__in6_u.__u6_addr8, 16);
		if( logConnects )
		    fprintf(logConnects, "Connection from %d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d.%d %s",
			from6in->sin6_addr.__in6_u.__u6_addr8[0],
			from6in->sin6_addr.__in6_u.__u6_addr8[1],
			from6in->sin6_addr.__in6_u.__u6_addr8[2],
			from6in->sin6_addr.__in6_u.__u6_addr8[3],
			from6in->sin6_addr.__in6_u.__u6_addr8[4],
			from6in->sin6_addr.__in6_u.__u6_addr8[5],
			from6in->sin6_addr.__in6_u.__u6_addr8[6],
			from6in->sin6_addr.__in6_u.__u6_addr8[7],
			from6in->sin6_addr.__in6_u.__u6_addr8[8],
			from6in->sin6_addr.__in6_u.__u6_addr8[9],
			from6in->sin6_addr.__in6_u.__u6_addr8[10],
			from6in->sin6_addr.__in6_u.__u6_addr8[11],
			from6in->sin6_addr.__in6_u.__u6_addr8[12],
			from6in->sin6_addr.__in6_u.__u6_addr8[13],
			from6in->sin6_addr.__in6_u.__u6_addr8[14],
			from6in->sin6_addr.__in6_u.__u6_addr8[15],
			ctime(&curTime));
		}
	    else if( from4in->sin_family == AF_INET )
		{
		*peerIP32 = from4in->sin_addr.s_addr;
		if( logConnects )
		    fprintf(logConnects, "Connection from %u %s",
			from4in->sin_addr.s_addr, ctime(&curTime));
		}
	    else if( logConnects )
		fprintf(logConnects, "Connection from unknown family %s", ctime(&curTime));

	    for(i=1; i<numPfds; i++)
		{
		if( pfds[i].fd == -1 )
		    break;
		}
	    if( i < MAX_CONNECTS )
		{
		pfds[i].fd = rcx;
		pfds[i].events = POLLIN | POLLHUP;
		pfds[i].revents =0;
		if( i == numPfds )
		    numPfds++;
		connectStatus[i] = 1;
		connectTime[i] = curTime;
		}
	    else
BX:
		close(rcx);     /* At limit, close immediately */
	    }
	goto again;
	}

    /* Run thru all the descriptors to see who wants something */
    for(i=1; i<numPfds; i++)
	{
	if( connectStatus[i] == 1 && curTime > connectTime[i] + 5 )
	    {
	    if( logConnects )
		{
		fprintf(logConnects, "Close illegal connection %s", ctime(&curTime));
		fflush(logConnects);
		}
	    goto CLOSEIT;
	    }
	if( pfds[i].revents & (POLLHUP|POLLERR|POLLNVAL) )
	    {   /* Connection closed */
CLOSEIT:
	    close(pfds[i].fd);
	    pfds[i].fd = -1;
	    connectStatus[i] = 0;
	    continue;
	    }
	if( pfds[i].revents & POLLIN )
	    {
	    if( debugm)
		printf("Data ready on %d\n", i);
	    for(j=0; j<2048; j++)
		{
		if( read(pfds[i].fd, &cmdBuffer[j], 1) != 1 )
		    {
		    if( debugm )
			printf("Read error on %d closing\n", i);
		    goto BAD;
		    }
		if( cmdBuffer[j] == '\n')
		    {
		    cmdBuffer[j] = '\0';
		    if( j && cmdBuffer[j-1] == '\r' )
			cmdBuffer[j-1] = '\0';
		    if( debugm)
			printf("CHAN %d CMD %s\n", i, cmdBuffer);
		    if( connectStatus[i] == 0 ) /* Should not happen */
			continue;
		    else if( connectStatus[i] == 1 ) /* Should be exact string OKAY */
			{
			if( strcmp(cmdBuffer, "OKAY") != 0 )
			    {
			    close(pfds[i].fd);
			    pfds[i].fd = -1;
			    connectStatus[i] = 0;
			    if( logConnects )
				{
				fprintf(logConnects, "No OKAY on chan %s", ctime(&curTime));
				fflush(logConnects);
				}
			    }
			else
			    {
			    connectStatus[i] = 2;
			    sendAll(i, curTime);
			    }
			}
		    else if( cmdBuffer[0] == 'D' || cmdBuffer[0] == 'L' )
			processCmd(cmdBuffer, curTime, i);
		    else
			processStatus(cmdBuffer, curTime, i);
		    /* Process request */
		    break;
		    }
		}
	    if( j == 1024 )
		{
printf("CMD TOO LONG CHAN %d closing\n", i);
BAD:
		close(pfds[i].fd);
		connectStatus[i] = 0;
		pfds[i].fd = -1;
		}
	    }
	}
    goto again;
    /* NOTREACHED */
}

main(int argc, char **argv)
{
int i, j;

    sleep(15);      /* Wait 15 seconds before starting up to allow time settling */

    debugm = 0;
    for(i=1; i<argc; i++)
	{
	if( strcmp(argv[i], "-d") == 0 )
	    debugm = 1;
	else if( strcmp(argv[i], "-v") == 0 )
	    ledUpdate = 1;
	else if( strcmp(argv[i], "-l") == 0 && i+1 < argc )
	    {
	    logConnects = fopen(argv[i+1], "w");
	    i++;
	    }
	if( strcmp(argv[i], "-h") == 0 )
	    {
USAGE:
	    printf("USAGE: %s [-d] [-v] [-l logConnectFile]\n", argv[0]);
	    printf("-d for debug, -v to update LED's -l to log connections, all debug messages to stdout\n");
	    exit(44);
	    }
	}

    strcpy(lastOpenClose, "Not set");
    doorPowerGPIO = 60; /* Pin 12 active low or disabled so a boot fail keeps th power on */
    lightGPIO = 31; /* Pin 13 */
    doorButtonGPIO = 50; /* Pin 14 */
    senseOpenGPIO = 51; /* Pin 16 */
    senseCloseGPIO = 48; /* Pin 15 */
    if( setupListenSocket() == 0 )
	exit(44);

    updateEnable();
    lightStatus=1;
    updateLight(0);
    updateDoorStatus(0, NULL);
    doorTarget = doorStatus;
    maxDoorOpenTime = 18;       /* Maximum time to open door */
    maxDoorCloseTime = 18;      /* Maximum time to close door */
    looper();
}