// Skeleton sketch for UDS connection to Radio-SkyPipe 
 // there are a lot of commented out print statements that
 // can be helpful in debugging.

#include <SPI.h>
#include <Ethernet.h>


 int POLL; // if =1 then data is polled by RSP using a GETD command
 int STAT; // -1 = we were just stopped by a KILL command 0 = startup state 1 = INIT rcvd 2 = Ready to Go 3= Running


// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network.
// gateway and subnet are optional:
byte mac[] = { 
  0x90, 0xA2, 0xDA, 0x0D, 0xFB, 0x03 };
IPAddress ip(192,168,1,60);
IPAddress gateway(192,168,1,1);
IPAddress subnet(255, 255, 255, 0);


// the default UDS port = 1377
EthernetServer server(1377);
//boolean alreadyConnected = false; // whether or not the client was connected previously

void setup() {
  // initialize the ethernet device
  Ethernet.begin(mac, ip, gateway, subnet);
  // start listening for clients
  
  STAT == 0;
  POLL == 0;
  //Serial.begin(9600); // connect to the serial port
  server.begin();
  delay(1000);
  //Serial.println("Arduino Ethernet UDS");
  //Serial.print("server is at ");
  //Serial.println(Ethernet.localIP());
     }


 int incomingByte;
 long dat; 
 void loop() {
     // if we are pushing the data to RSP then we need to
     // establish our timing for sending new data.
     // here we are just doing a delay of 100ms to get a
     // sample rate of about 10 samples / sec.
  
  EthernetClient client = server.available();
 
  while (client){
     if (POLL == 0 && STAT ==3){
     delay(100); 
     } 

    while (client.available() > 0) {

     // read the oldest byte in the serial buffer:
     incomingByte = client.read();
      //Serial.print (incomingByte);  //debug
     // if it's an K we stop (KILL):
     if (incomingByte == 'K') {
         Serial.println("^^1002DEAD"); // Just for troubleshooting
         //Serial.write(255);
         //Serial.println("Arduino UDS");
         //GET PAST THE REST OF THE WORD by Reading it.
         delay(10); // not sure why these delays were needed
         incomingByte = client.read();
         delay(10);
         incomingByte = client.read();
         delay(10);
         incomingByte = client.read();
         incomingByte = 0;
         STAT=-1 ;
         }
         // if it's a capital I run the INIT code if any
         if (incomingByte == 'I' && STAT ==0) {
                 //INIT
                 // GET RID OF 'NIT'
                 delay(10);
                 incomingByte = client.read();
                 delay(10);
                 incomingByte = client.read();
                 delay(10);
                 incomingByte = client.read();
                 incomingByte = 0;
                 STAT = 1 ;
                 //Serial.println("^^1002 INITIALIZED "); 
                 //Serial.write(255);
         } 
         // if it's an L (ASCII 76) RSP will POLL for data
                 if (incomingByte == 'L') {
                 POLL = 1;
                 // GET RID OF 'L'
                 delay(10);
                 incomingByte = client.read();
                 incomingByte = 0;
                 //Serial.println("^^1002 POLLING ");
                 //Serial.write(255); 
         }

         // H sets it to push
         if (incomingByte == 'H') {
                 POLL = 0;
                 //Serial.println("^^1002 PUSHING ");
                 //Serial.write(255); 
         }

         // if it's a C then Radio-SkyPipe is requesting number of channels
         if (incomingByte == 'C') {
                 // change the last digit to = digit of channels of data (ex. 1)
                 delay(10);
                 client.print("^^20131");
                 client.write(255); // print result;
                 STAT = 2; // ready to go
         }
         if (incomingByte == 'A' ) {
                 // A means STAT was requested so send UDS ready message
                 delay(10);
                 client.print("^^1001");
                 client.write(255); // print result;;
                 // GET RID OF 'T'
                 delay(10);
                 incomingByte = client.read();
                 incomingByte = 0;
                 STAT=3;
                 
         }
         // if it's an D we should send data to RSP:
         if (incomingByte == 'D' && POLL == 1 ) {
                 //Serial.println(" DATA REQUEST RECEIVED ");
                 dat = analogRead(A0); // Replace this with a call to whatever data collection routine
                 //Serial.println(dat);
                 client.print("#0"); // # followed by channel number of data
                 client.print(dat); 
                 client.write(255);
                 client.print("^^3001"); // This tells RSP to time stamp it
                 client.write(255); // all commands end with this character.
         }

         if (STAT== -1){
         STAT = 0;
     } 


     } 
     // we are finished processing any incoming commands from the PC
     // and we are not being polled so get a sample and send it
     if (POLL == 0 && STAT == 3) {
                 
                 dat = analogRead(A0); // Replace this with a call to whatever data collection routine
                 Serial.println(dat);
                 client.print("#0"); // # followed by channel number of data
                 client.print(dat); 
                 client.write(255);
                 client.print("^^3001"); // This tells RSP to time stamp it
                 client.write(255); // all commands end with this character.
     }
      
      if (!client.connected()) {
        //Serial.println();
        //Serial.println("disconnecting.");
        client.stop(); 
      } 
} // end of while (client)

 }