/**
 * Firmware of the Arduino GPS reciever.
 *
 * This code was inspired by http://quaxio.com/arduino_gps/ ,
 * http://playground.arduino.cc/Tutorials/GPS and 
 * http://blog.bouni.de/blog/2012/06/25/a-arduino-telnet-server/
 * 
 *
 * Autor: Max Knötig
 */
 
#include <SoftwareSerial.h>
#include <string.h>
#include <stdio.h>
#include <SPI.h>
#include <Ethernet.h>

#define MAX_CMD_LENGTH   15

//define led pins for display on the box
int arduLed = 8;  // on: arduino is on. blinking: serial message parsed 
int gpsLed  = 7;  // on: venus gps chip on. blinking: gps locked
int vetoLed = 6;  // on: veto active. off: veto not active ( active low btw. )

int arduLedOn = 0;
int gpsLedOn  = 0;
int vetoLedOn = 0;

//define inputs and outputs for the veto system
int vetoPin = 3;  // the veto output pin ( active low )
int navPin  = 2;  // the nav pin, from the venus gps. this is for steering the gpsLed

//globals for storing the serial nema message
int byteGPS=-1;          // will become the read byte from the Venus GPS
char cmd[7] = "$GPGGA";  // the NMEA message type we want to know
int counter = 0;         // counts how many bytes were received (max 300)
char buf[300] = "";      // buffer to store the message
String nemaMsg;          // this buffer stores the last complete nema message

//variables to save (or dump) nema message parts
int    seconds = 0;      // this will become the veto trigger criterion ( seconds = 59 -> veto )   
int    dump = 0;         // the other variables are used when sscanf the nema message
char   cdump = '0';      // for dumping reasons
char   hexdump[8]="0000000";

//the addresses for the shield (commented: ethz mode):
byte mac[]   = { 0x90, 0xA2, 0xDA, 0x0D, 0xB9, 0x32 };
byte ip[]    = { 10, 0, 100, 112 };
//byte ip[]    = { 192, 33, 103, 241 };
//byte dns[]   = { 129, 132, 98, 12 };
//byte gatew[] = { 192, 33, 96, 1 };
//byte smask[] = { 255, 255, 248, 0 };


//the globals for the ethernet communication
String etherCmd;    // store the ethernet command in here
EthernetServer server = EthernetServer(23);
EthernetClient client;
boolean connected = false;

//the state as decided by the user. 0 = veto_off, 1 = veto_60, 2 = veto_on
int state = 1;

/**
 * Setup arduino and gps
 */
void setup() {
  // define I/O pins
  pinMode(arduLed, OUTPUT);    // Initialize Arduino LED pin
  pinMode(gpsLed,  OUTPUT);    // Initialize GPS LED pin
  pinMode(vetoLed, OUTPUT);    // Initialize Veto LED pin
  pinMode(vetoPin, OUTPUT);    // Initialize Veto pin
  pinMode(navPin,  INPUT);     // Initialize Nav pin
  
  // set their startup modes
  digitalWrite(vetoPin, HIGH); // put veto to high ( no veto ) in startup mode
  digitalWrite(arduLed, HIGH); // put the arduino led high when starting
  arduLedOn = 1;
  
  //begin the ethernet and serial connections
  Ethernet.begin(mac, ip); //, dns, gatew, smask);  // this is to begin (ethz) ethernet
  Serial.begin(115200);     // the gps serial connection
  server.begin();
  
  reset();                  // reset the counters (just in case) 
}

/**
 * the loop which processes ethernet and serial connections
 */
void loop()
{
  //serial part
  byteGPS=Serial.read();         // Read a byte of the serial port
  
  if (byteGPS == -1) {           // See if the port is empty yet
    delay(1);
  } else {
      if (!handle_byte(byteGPS)) {
      reset();
      Serial.flush();
    }
  }
  
  //ethernet part
  client = server.available();    // check if the server is available

  if (client == true) {           // if there is a new client
    if (!connected) {              
      client.flush();
      connected = true;
    }

    while (client.available() > 0) { // while there is incoming data read it
      readEthernetCommand(client.read());
    }
  }

  if( digitalRead(navPin) == 0){  // test if the gps blinkes, if yes, blink the gps led as well
     digitalWrite(gpsLed, HIGH);
     gpsLedOn=1;  
  } else {
    digitalWrite(gpsLed, LOW);
    gpsLedOn=0;
  }
}

/**
 * handle the byte read from the GPS module
 */
int handle_byte(int byteGPS) {

  if (counter == 300) {        // if the buffer is full return 0
    return 0;
  }

  //read the first char 
  buf[counter] = byteGPS;

  if (counter == 6) {          // if 6 chars are read, check if it is the beg. of the correct message
    for(int j=0; j<6; j++) {   // if not return 0
      if (buf[j] != cmd[j]) {
        return 0;
      }
    } 
  }

  //check if recieved message is gpgga compatible
  if ( byteGPS == '\n' )  {     // when '\n' read (i.e. end of message ), check if variables are at the correct pos. then continue
      if( sscanf( buf,"$GPGGA,%2d%2d%2d.%d,%d.%d,%c,%d.%d,%c,%d,%d,%d.%d,%d.%d,M,%d.%d,M,%7s\r\n",
                  &dump,&dump,&seconds,&dump,&dump,&dump,&cdump,&dump,&dump,&cdump,&dump,&dump,&dump,&dump,&dump,&dump,&dump,&dump,&hexdump ) != 19 )
                  {
      return 0;
      }
    
    blinkLed(0);                   // everything is ok -> message parsed, blink arduino led
    decideVeto();                 // as we have found the interesting part, do something
    nemaMsg=buf;                   // save the found message to the nema string
    // remove \r and \n from the string
    nemaMsg = nemaMsg.substring(0,counter-1);
    
    return 0;                     // return 0 as we found the interesting part
  }

  //continue with the next char if none of the actions before were needed
  if(buf[0]=='$' || byteGPS=='$') {  
    counter++;                 // count one up, if it is the correct message ( the '$' at front )
  }

  return 1;                     // continue (return 1) if we cannot yet decide to stop
}

 /**
 *  decideVeto() veto something if the state and time demand it
 */
void decideVeto() {
  if( state == 0 ) {                 // veto off
      digitalWrite(vetoPin, HIGH);   // put veto to not active ( HIGH ) 
      digitalWrite(vetoLed, LOW);    // put veto Led to not active 
      vetoLedOn = 0;
  } else if( state == 2 ) {          // veto on
      digitalWrite(vetoPin, LOW);    // put veto to active 
      digitalWrite(vetoLed, HIGH);   // put veto Led to active 
      vetoLedOn = 1;
  } else if( state == 1 ) {          // veto the full minute
      if( seconds == 59 ) {          // if it is time to veto
        digitalWrite(vetoPin, LOW);  // put veto to active 
        digitalWrite(vetoLed, HIGH); // put veto Led to active 
        vetoLedOn = 1;
      } else {
        digitalWrite(vetoPin, HIGH); // put veto to not active 
        digitalWrite(vetoLed, LOW);  // put veto Led to not active 
        vetoLedOn = 0;
      }
  }  
}

/**
 *  reset() the counters if needed
 */
void reset() {
  counter = 0;
}

/**
 *  blinkLed() toggle the Led (on or off)
 * type: int, 0 = arduino LED, 1 = gps LED, 2 = veto LED 
 */
void blinkLed( int type ) {
 if(type == 0){ 
   if(arduLedOn==0) {
     digitalWrite(arduLed, HIGH);
     arduLedOn=1; 
   } else {
     digitalWrite(arduLed, LOW);
     arduLedOn=0;
   }
 }/* else if (type == 1) {
   if(gpsLedOn==0) {
     digitalWrite(gpsLed, HIGH);
     gpsLedOn=1; 
   } else {
     digitalWrite(gpsLed, LOW);
     gpsLedOn=0;
   }
  } else if (type == 2) {
   if(vetoLedOn==0) {
     digitalWrite(vetoLed, HIGH);
     vetoLedOn=1; 
   } else {
     digitalWrite(vetoLed, LOW);
     vetoLedOn=0;
   }
 }*/
}

/**
 * read ethernet command
 */
void readEthernetCommand(char c) {

  if(etherCmd.length() == MAX_CMD_LENGTH) {
    etherCmd = "";
  }

  etherCmd += c;

  if(c == '\n') {
    if(etherCmd.length() > 2) {
      // remove \r and \n from the string
      etherCmd = etherCmd.substring(0,etherCmd.length() - 2);
      parseCommand();
    }
  }
}

/**
 * parse ethernet command
 */
void parseCommand() {

  if(etherCmd.equals("quit")) {
      client.stop();
      connected = false;
  } else if(etherCmd.equals("help")) {
      server.println("-- GPS Clock Arduino Help --");
      server.println("---- Autor: Max Knoetig ----");
      server.println("--- mknoetig@phys.ethz.ch --");
      server.println("get_nema  :  get the last complete NEMA message");
      server.println("get_status:  get the status, veto_off, veto_60 or veto_on");
      server.println("veto_off  :  switch off veto");
      server.println("veto_on   :  switch on veto");
      server.println("veto_60   :  veto every pps to the full GPS UTC minute");
      server.println("quit      :  close the connection");
  } else if(etherCmd.equals("get_nema")) {
      server.println(nemaMsg);
  } else if(etherCmd.equals("get_status")) {
      if(state==1) {
        server.println("veto_60");
      } else if(state==0) {
        server.println("veto_off");
      } else if(state==2) {
        server.println("veto_on");
      } else { 
        server.println("system state unknown");
      }
  } else if(etherCmd.equals("veto_off")) {
      state=0;
      server.println("veto now off");
  } else if(etherCmd.equals("veto_60")) {
      state=1;
      server.println("veto 60 now on");
  } else if(etherCmd.equals("veto_on")) {
      state=2;
      server.println("veto now on");
  } else {
      server.println("Invalid command, type help");
  }
  etherCmd = "";
}