#include <ezButton.h>
#include <Adafruit_MLX90614.h>
#include <Servo.h>            // attach Servo library (http://www.arduino.cc/playground/ComponentLib/Servo)
#include <pins_arduino.h>     // attach arduino pins header file to determine which board type is being used
Adafruit_MLX90614 mlx = Adafruit_MLX90614();

// Firefly Firmata
#define BAUDRATE 9600       // Set the Baud Rate to an appropriate speed
#define BUFFSIZE 512          // buffer one command at a time

char buffer[BUFFSIZE];        // declare buffer
uint8_t bufferidx = 0;        // a type of unsigned integer of length 8 bits
char *parseptr;
char buffidx;
int counter = 0;
int numcycles = 1000;

#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)                                                // declare variables for STANDARD boards
  Servo Servo13, Servo12, Servo11, Servo10, Servo9, Servo8, Servo6, Servo5, Servo3;
  Servo SERVO_CONFIG[] = {Servo13, Servo12, Servo11, Servo10, Servo9, Servo8, Servo6, Servo5, Servo3};       // declare array of Servo objects
  int WRITE_PIN_CONFIG[] = {13,12,11,10,9,8,6,5,3}; 
  int READ_APIN_CONFIG[] = {0,1,2,3,4,5};
  int READ_DPIN_CONFIG[] = {2,4,7}; 
#endif

#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega16U4__)                                               // declare variables for LEONARDO board
  Servo Servo13, Servo12, Servo11, Servo10, Servo9, Servo8, Servo6, Servo5, Servo3;       
  Servo SERVO_CONFIG[] = {Servo13, Servo12, Servo11, Servo10, Servo9, Servo8, Servo6, Servo5, Servo3};       // declare array of Servo objects
  int WRITE_PIN_CONFIG[] = {13,12,11,10,9,8,6,5,3}; 
  int READ_APIN_CONFIG[] = {0,1,2,3,4,5};
  int READ_DPIN_CONFIG[] = {2,4,7};
#endif

#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)                        // declare variables for MEGA boards
  Servo Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8, Servo9, Servo10, Servo11, Servo12, Servo13, Servo32, Servo33, Servo34, Servo35, Servo36, Servo37, Servo38, Servo39, Servo40, Servo41, Servo42, Servo43, Servo44, Servo45, Servo46, Servo47, Servo48, Servo49, Servo50, Servo51, Servo52, Servo53;
  Servo SERVO_CONFIG[] = {Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8, Servo9, Servo10, Servo11, Servo12, Servo13, Servo32, Servo33, Servo34, Servo35, Servo36, Servo37, Servo38, Servo39, Servo40, Servo41, Servo42, Servo43, Servo44, Servo45, Servo46, Servo47, Servo48, Servo49, Servo50, Servo51, Servo52, Servo53};  // declare array of Servo objects
  int WRITE_PIN_CONFIG[] = {2,3,4,5,6,7,8,9,10,11,12,13,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53}; 
  int READ_APIN_CONFIG[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
  int READ_DPIN_CONFIG[] = {22,23,24,25,26,27,28,29,30,31};
#endif

#if defined(__SAM3X8E__)                 // declare variables for DUE boards
  Servo FDAC0, FDAC1, Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8, Servo9, Servo10, Servo11, Servo12, Servo13, Servo32, Servo33, Servo34, Servo35, Servo36, Servo37, Servo38, Servo39, Servo40, Servo41, Servo42, Servo43, Servo44, Servo45, Servo46, Servo47, Servo48, Servo49, Servo50, Servo51, Servo52, Servo53;  
  Servo SERVO_CONFIG[] = {FDAC0, FDAC1, Servo2, Servo3, Servo4, Servo5, Servo6, Servo7, Servo8, Servo9, Servo10, Servo11, Servo12, Servo13, Servo32, Servo33, Servo34, Servo35, Servo36, Servo37, Servo38, Servo39, Servo40, Servo41, Servo42, Servo43, Servo44, Servo45, Servo46, Servo47, Servo48, Servo49, Servo50, Servo51, Servo52, Servo53};  // declare array of Servo objects
  int WRITE_PIN_CONFIG[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53}; //Note: first two values correspond to the DAC pins
  int READ_APIN_CONFIG[] = {0,1,2,3,4,5,6,7,8,9,10,11};
  int READ_DPIN_CONFIG[] = {22,23,24,25,26,27,28,29,30,31};
#endif

ezButton button(8);

// Pins
  const int ButtonPin = 8;  // Button
  const int enA = 2;  // Airpumps
  const int enB = 3;
  const int in1 = 4;
  const int in2 = 5;
  const int in3 = 6;
  const int in4 = 7;

  const int xenA = A1; // Suction and Valve
  const int xenB = A4;
  const int xin1 = A3;
  const int xin2 = A2;
  const int xin3 = 9;
  const int xin4 = 10;
  const int xLED = 11;

  const int PressureIn = A0;  // Pressure Sensor
  const int Valve = 8; // Valve
  const int SuctionPin = A5;
  const int KukaPin = A0;
//
// Variables
  unsigned long LastCount = 0;
  unsigned long count = 0;
  
  int KukaToIno;  
  int InoToKuka;
//
// Constants 
  const int CureTemp = 33;
  const int minPressure = 102; 
  const int maxPressure = 105;
//

void setup() { 
  // Firefly
    Init();                       //set initial pinmodes
    Serial.begin(BAUDRATE);       // Start Serial communication
    #if defined(__SAM3X8E__)      //if the connected board is an Arduino DUE
      analogReadResolution(12);   //Set the analog read resolution to 12 bits (acceptable values between 1-32 bits).  This is only for DUE boards
      analogWriteResolution(12);  // Set the analog write resolution to 12 bits (acceptable values between 1-32 bits).  This is only for DUE boards
    #endif
  //
  // PinModes
    pinMode(xenA, OUTPUT);
    pinMode(xenB, OUTPUT);
    pinMode(xin1, OUTPUT);
    pinMode(xin2, OUTPUT);
    pinMode(xin3, OUTPUT);
    pinMode(xin4, OUTPUT);
    pinMode(xLED, OUTPUT);

    pinMode(enA, OUTPUT);
    pinMode(enB, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);
  //
  // Button
    button.setDebounceTime(100); // set debounce time to 100 milliseconds
    button.setCountMode(COUNT_FALLING);  
  //
  // Teperature Sensor
    while (!Serial);
    if (!mlx.begin()) {
      Serial.println("Error connecting to MLX sensor. Check wiring.");
      while (1);
    }
  //
}

void loop() {
// Firefly
  if(Serial){
    ReadSerial();                       // read and parse string from serial port and write to pins
    if (counter >= numcycles){          // Wait every nth loop 
      ReadInputs();                     // get input data and print data to the serial port
      counter = 0;                      // reset the counter
    }
    counter ++;                         // increment the writecounter
  }
//
// Button and Variables
  button.loop();
  count = button.getCount();
  
  int CountIn3 = count % 3 + 1;
  int objTemp;
  int ambTemp;
  int PressureValue; 
  String FillState;
  String CuredState;
  unsigned long TimeCured;

  objTemp = ObjectTemperature();
  ambTemp = AmbientTemperature();
  PressureValue = AirPressureValue();
  FillState = CheckFillState(CountIn3);
  CuredState = CheckPlasterCured(CureTemp, objTemp);
  TimeCured = CheckTime(CuredState);

//
// Print Values
  Serial.print(TimeCured); Serial.print(" ");
  Serial.print(CountIn3); Serial.print(" ");
  Serial.print(FillState); Serial.print(" ");
  Serial.print(CuredState); Serial.print(" ");
  Serial.print(objTemp); Serial.print(" ");
  Serial.print(ambTemp); Serial.print(" ");
  Serial.println(PressureValue);
  digitalWrite(SuctionPin, HIGH);
//
// Hold AirPressure

  if(CountIn3 > 1){
    HoldAirPressure(PressureValue, minPressure, maxPressure);
  }
  else{    
    //air pump off
    digitalWrite(enA, LOW);
    digitalWrite(enB, LOW);
    // Turn on motor A & B
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
//
// Deflate 
  if(TimeCured >= 20000){
    Deflate();
  } 
//
}


int ObjectTemperature(){  //done
  int objTemp;
  objTemp = (mlx.readObjectTempC());
  return objTemp;
}

int AmbientTemperature(){ //done
  int ambTemp;
  ambTemp = (mlx.readAmbientTempC());
  return ambTemp;
}

int AirPressureValue(){ // done
  int PressureValue;
  PressureValue = analogRead(PressureIn);
  return PressureValue;
}

void HoldAirPressure(int PressureValue, int minPressure, int maxPressure){  // done
  if(PressureValue >= maxPressure){    
    //air pump off
    digitalWrite(enA, LOW);
    digitalWrite(enB, LOW);
    // Turn on motor A & B
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
  else if(PressureValue <= minPressure){
    //air pump on
    digitalWrite(enA, HIGH);
    digitalWrite(enB, HIGH);
    // Turn on motor A & B
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW); 
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
} 

String CheckFillState(int CountIn3){  // done
  String FillState;
  if(CountIn3 == 3){
    FillState = "Full";
  }
  else{
    FillState = "Empty";
  }
  return FillState;
}

String CheckPlasterCured(const int CureTemp, int objTemp){  // done
  String CuredState;
  if(objTemp >= CureTemp){
    CuredState = "Cured";
  }
  else if(objTemp <= CureTemp){
    CuredState = "NotCured";
  }
  return CuredState;
}

unsigned long CheckTime(String CuredState){
  unsigned long t = 0;
  if(CuredState == "Cured"){
    t = millis() % 20100;
  }
  return t;
}

void Deflate(){
  //air pump off
  digitalWrite(enA, LOW);
  digitalWrite(enB, LOW);
  // Turn on motor A & B
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  unsigned long StartTime = millis();
  while((millis() - StartTime) < 10000){
    digitalWrite(xenA, HIGH);
    digitalWrite(xin1, LOW);
    digitalWrite(xin2, HIGH);

    digitalWrite(xenB, HIGH);
    digitalWrite(xin3, HIGH);
    digitalWrite(xin4, LOW);

    digitalWrite(xLED, HIGH);

    if((millis() - StartTime) >= 10000){
      break;
    }
  }
  digitalWrite(xenA, LOW);
  digitalWrite(xin1, LOW);
  digitalWrite(xin2, LOW);

  digitalWrite(xenB, LOW);
  digitalWrite(xin3, LOW);
  digitalWrite(xin4, LOW);

  digitalWrite(xLED, LOW);

  Serial.println("FULLY_DEFLATED");
  delay(10000);
}



/*==============================================================================
 * FUNCTIONS()
 *============================================================================*/

/*
* Initializes the digital pins which will be used as inputs
*/
void Init(){
  int len = sizeof(READ_DPIN_CONFIG)/sizeof(READ_DPIN_CONFIG[0]); //get the size of the array
  for(int i = 0; i < len; i++){
    pinMode(READ_DPIN_CONFIG[i], INPUT);
  }
}

/* 
* Reads the incoming ADC or digital values from the corresponding analog and digital input  
* pins and prints the value to the serial port as a formatted commma separated string
*/
void ReadInputs(){ 
  int len = sizeof(READ_APIN_CONFIG)/sizeof(READ_APIN_CONFIG[0]); //get the size of the array
  for(int i = 0; i < len; i++){
    int val = analogRead(READ_APIN_CONFIG[i]);  //read value from analog pins
    Serial.print(val); Serial.print(",");   
  }
  len = sizeof(READ_DPIN_CONFIG)/sizeof(READ_DPIN_CONFIG[0]); //get the size of the array
  for(int i = 0; i < len; i++){
    int val = digitalRead(READ_DPIN_CONFIG[i]); //read value from digital pins
    Serial.print(val); Serial.print(",");   
  }
  Serial.println("eol");  //end of line marker
}

/*
* Retrieve the latest incoming serial value and split the string at the comma delimeter.
* When a comma is found, the value is offloaded to a temporary variable and written
* to the corresponding digital pin.
*/
void ReadSerial(){
  char c;    // holds one character from the serial port
  if (Serial.available()) {
    c = Serial.read();         // read one character
    buffer[bufferidx] = c;     // add to buffer
    if (c == '\n') {  
      buffer[bufferidx+1] = 0; // terminate it
      parseptr = buffer;       // offload the buffer into temp variable
      int len = sizeof(WRITE_PIN_CONFIG)/sizeof(WRITE_PIN_CONFIG[0]); //get the size of the array
      for(int i = 0; i < len; i++){
        //parse all incoming values and assign them to the appropriate variable
        int val = parsedecimal(parseptr);       // parse the incoming number
        if(i != len - 1) parseptr = strchr(parseptr, ',')+1;   // move past the ","
        WriteToPin(WRITE_PIN_CONFIG[i], val, SERVO_CONFIG[i]);         //send value out to pin on arduino board
      }    
      bufferidx = 0;                             // reset the buffer for the next read
      return;                                    // return so that we don't trigger the index increment below
    }                                            // didn't get newline, need to read more from the buffer
    bufferidx++;                                 // increment the index for the next character
    if (bufferidx == BUFFSIZE-1) bufferidx = 0;  // if we get to the end of the buffer reset for safety
  }
}

/*
* Send the incoming value to the appropriate pin using pre-defined logic (ie. digital, analog, or servo)
*/
void WriteToPin(int _pin, int _value, Servo _servo){
  if (_value >= 10000 && _value < 20000)            // check if value should be used for Digital Write (HIGH/LOW)
  {      
    if (_servo.attached()) _servo.detach();         // detach servo is one is attached to pin
    pinMode(_pin, OUTPUT);                       
    _value -= 10000;                                // subtract 10,000 from the value sent from Grasshopper 
    if (_value == 1) digitalWrite(_pin, HIGH);     
    else digitalWrite(_pin, LOW);   
  }   
  else if (_value >= 20000 && _value < 30000)       // check if value should be used for Analog Write (0-255)
  {
    if (_servo.attached()) _servo.detach();         // detach servo is one is attached to pin
    pinMode(_pin, OUTPUT);               
    _value -= 20000;                                // subtract 20,000 from the value sent from Grasshopper
    analogWrite(_pin, _value);                     
  }
  else if (_value >= 30000 && _value < 40000)       // check if value should be used for Servo Write (0-180)
  {
    _value -= 30000;                                // subtract 30,000 from the value sent from Grasshopper
    if (!_servo.attached())_servo.attach(_pin);     // attaches a Servo to the PWM pin (180 degree standard servos)                                    
    _servo.write(_value);                          
  }
  else if (_value >= 40000 && _value < 50000)       // check if value should be used for Analog Write (0-4096) for DACs
  {
    if (_servo.attached()) _servo.detach();         // detach servo is one is attached to pin
    pinMode(_pin, OUTPUT);               
    _value -= 40000;                                // subtract 40,000 from the value sent from Grasshopper
    WriteToDAC(_pin, _value);                     
  }
}

/*
* Parse a string value as a decimal
*/
uint32_t parsedecimal(char *str){
  uint32_t d = 0;
  while (str[0] != 0) {
    if ((str[0] > '50') || (str[0] < '0'))
      return d;
    d *= 10;
    d += str[0] - '0';
    str++;
  }
  return d;
}

/*
* Send the incoming value to the appropriate DAC for DUE boards. 
* Note: analogWrite resolution (default is 12 bits) is defined in the Setup function.
*/
 
void WriteToDAC(int _pin, int _value){
  #if defined(__SAM3X8E__) 
  if(_pin == 0) analogWrite(DAC0, _value);
  else if (_pin == 1) analogWrite(DAC1, _value);
  #endif
}



/*
// Function Prototypes
  void Init();
  int ObjectTemperature();
  int AmbientTemperature();
  int AirPressureValue();
  void AirPumpsOn();
  void AirPumpsOff();
  String CheckFillState();
  String CheckPlasterCured();
  int MeasureTimeCured();
  void Deflate();
//  */

// Old Code

    // for (unsigned long df = 0; df <= 5000; df++){
    //   Deflate();
    //   if (df >= 5000){
    //     df = 0;
    //     break;
    //   }
    // }

  // Deflate(CuredState, Valve, SuctionPin);

  // PrintValues(CountIn3, FillState, CuredState, objTemp, ambTemp, PressureValue);

  // while(PressureValue <= minPressure && (CountIn3 == 2 || CountIn3 == 3))
  // {
  //   AirPumpsOn();
  // }
  // // else{
  // //   AirPumpsOff();
  // // }

  // if(TimeCured > 180){
  //   Deflate_02();
  //   if(Deflate_02() >= 60){
  //     TimeCured = 0;
  //   }
  // }


  /*
  int Deflate_02(){
    int DeflateTimeCount;
    unsigned long t;
    digitalWrite(Valve, HIGH);
    t = millis();

    if(t >= 1000){
      ++ DeflateTimeCount;
      t = 0;
    }
    if(DeflateTimeCount > 60){
      DeflateTimeCount = 0;
    }
    return DeflateTimeCount;
  } */

  /*
  void PrintValues(int CountIn3, String FillState, String CuredState, int objTemp, int ambTemp, int PressureValue){
    Serial.print(CountIn3); Serial.print(" ");
    Serial.print(FillState); Serial.print(" ");
    Serial.print(CuredState); Serial.print(" ");
    Serial.print(objTemp); Serial.print(" ");
    Serial.print(ambTemp); Serial.print(" ");
    Serial.println(PressureValue);
  } */


//