#include <Encoder.h>
#include <SPI.h>         // needed for Arduino versions later than 0018
#include <Ethernet.h>
#include <EthernetUdp.h>         // UDP library from: bjoern@cs.stanford.edu 12/30/2008
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>



// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {  
	0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED 
};

IPAddress ip(192, 168, 16, 200);
IPAddress remoteIp(192, 168, 16, 10);

unsigned int localPort = 8888;      // local port to listen on
unsigned int remotePort = 8888;
// buffers for receiving and sending data
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet,
char  ReplyBuffer[100];// = "acknowledged";       // a string to send back
uint16_t adcresult;
// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;

uint8_t adc=0;
uint8_t firstbyte=0x0;
uint8_t pinmap[6] = {4,5,6,7,8,9};
uint8_t dacpinmap[1] = {3};
int nout = 0;

Encoder myEnc(8, 9);
long oldPosition  = -999;
long counters[2];


void setup() 
{
	// start the Ethernet and UDP:
	Ethernet.begin(mac,ip);
	Udp.begin(localPort);

	Serial.begin(9600);
}


char *skip_spaces(const char *str)
{
	while (isspace(*str))
		++str;
	return (char *)str;
}


char rxbyte;
char workbuf[100];
char rxtokens[5];
char *newtoken;
const char s[2] = ",";
char *token;



void decodeRx(char* rxbuffer, int buflen)
{
	int i = 0;
	memset(workbuf,'\0',sizeof(workbuf));
	if(rxbuffer[0] == '[')
	{
		strncpy(workbuf,&rxbuffer[5],buflen);
		i = 0;
	}
	/* get the first token */
	token = strtok(workbuf, s);
	newtoken = skip_spaces(token);
	rxtokens[i] = atoi(newtoken);
	i++;
	/* walk through other tokens */
	while( token != NULL ) 
	{
		token = NULL;
		token = strtok(NULL, s);
		newtoken = skip_spaces(token);
		rxtokens[i] = atoi(newtoken);
		i++;
	}

	for (char x = 0; x < 4; x++)
	{
		rxbyte = rxtokens[x];

		if(x == 0) // get the analog output value
		{
			analogWrite(dacpinmap[x], rxbyte);
		}
		else if(x == 1) // get the number of output pins
		{
			nout = rxbyte;
			for (char s = 0; s < 6; s++)
			{
				if(s < nout) // set only the output pins
				{
					pinMode(pinmap[s], OUTPUT);
				}
				else // set the input pins
				{
					pinMode(pinmap[s], INPUT);
				}


			}
		}

		if (x == 2) // get the values for output pins
		{
			uint8_t  out = 0;
			for (int z = 0; z < nout; z++)
			{
				out = ((rxbyte >> z) & 1);
				digitalWrite(pinmap[z], out);
				out = 0;
			}
		}
		if (x == 3) // get the pullup values for input pins
		{
			for (int n = 0; n < 6; n++)
			{
				uint8_t dir = 0; 
				if (n >= nout) // only for input pins
				{
					dir	= ((rxbyte >> n) & 1);
					digitalWrite(pinmap[n],dir);
				}
			}
		}

	}
}


void encodeTx()
{
	Udp.beginPacket(remoteIp, remotePort);
	uint8_t v = 0;
	for(int c = 0; c < 6; c++)
	{

		adcresult = analogRead(c);
		sprintf(ReplyBuffer,"%4d", adcresult);

		Udp.write(ReplyBuffer);
		Udp.write(",");

		int dummy = c;

		if (c >= nout){
			int moredummy = c;
			if(digitalRead(pinmap[c])){
				v |= (1<<c);
			}
		}
	}
	sprintf(ReplyBuffer,"%3d", v);
	Udp.write(ReplyBuffer);
	Udp.write(",");
	adcresult = counters[0];
	sprintf(ReplyBuffer,"%4d", adcresult);
	Udp.write(ReplyBuffer);
	Udp.write(",");
	adcresult = counters[1];
	sprintf(ReplyBuffer,"%4d", adcresult);
	Udp.write(ReplyBuffer);

	Udp.endPacket();

}


void loop() 
{

	encodeTx();

	// if there's data available, read a packet
	int packetSize = Udp.parsePacket();
	if(packetSize)
	{
		Udp.read(packetBuffer,UDP_TX_PACKET_MAX_SIZE);
		decodeRx(packetBuffer,packetSize);   
	}

	long newPosition = myEnc.read();
	if (newPosition != oldPosition) {
		counters[0] = oldPosition;
		counters[1] = newPosition;
		oldPosition = newPosition;
		//Serial.println(newPosition);
	}

	delay(1);

}