#include <SoftwareSerial.h>
#include "ELMduino.h"
#include <Wire.h>
#include <AutoPID.h>

//Prototypes
int32_t ReadKPH(); //OBDII
void WriteMCP(int Value, int Addr); //DAC over I2C

//DAC Address
  #define MCP1 0x60
  #define MCP2 0x61

//OBDII Things
  SoftwareSerial mySerial(2, 3); // RX, TX
  ELM327 myELM327;

//PID Settings
  #define OUTPUT_MIN 0
  #define OUTPUT_MAX 1023
  #define KP 7
  #define KI 3
  #define KD 7//1
  double speed, setPoint, outputVal;
  AutoPID myPID(&speed, &setPoint, &outputVal, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);

//IO
  int TPS1_inPIN = A0;
  int TPS2_inPIN = A1;
  int ClutchPIN = 10;
  int BrakePIN = 9;
  int ResetPIN = 8;
  int IncrPIN = 7;
  int DecrPIN = 6;
  int SetPIN = 5;
//Variables
  bool CC_on = false;
  bool CalibrationMode = false;
  bool MinCalibrated = false;

  int tempSig1 = 0;
  int Sig1_Min = 0;
  int Sig1_Max = 0;
  
  int tempSig2 = 0;
  int Sig2_Min = 0;
  int Sig2_Max = 0;

  int ThrottlePosIn = 0;
  int ThrottlePosOut = 0;
  int TempThrottlePos = 0;

  bool throttleLock = false;

  int CO_Done = false;
//Millis Delays
unsigned long time1 = 0;
unsigned long time2 = 0;
unsigned long time3 = 0;
unsigned long time4 = 0;
unsigned long time5 = 0;
unsigned long time6 = 0;

//Run Once Variables
bool Run1 = false;
bool Run2 = false;

void setup() {

  Serial.begin(115200);
  mySerial.begin(115200);
  //myELM327.begin(mySerial, true, 2000);

  Wire.begin();
  pinMode(3, OUTPUT);                                                                  //test
  pinMode(A2, INPUT);                                                                  //test
  pinMode(TPS1_inPIN, INPUT);
  pinMode(TPS2_inPIN, INPUT);
  pinMode(ClutchPIN, INPUT_PULLUP);
  pinMode(BrakePIN, INPUT_PULLUP);
  pinMode(ResetPIN, INPUT_PULLUP);
  pinMode(IncrPIN, INPUT_PULLUP);
  pinMode(DecrPIN, INPUT_PULLUP);
  pinMode(SetPIN, INPUT_PULLUP);

  //if speed is more than 10 kph below or above setpoint, OUTPUT will be set to min or max respectively
  myPID.setBangBang(200);
  //set PID update interval to 500ms
  myPID.setTimeStep(500);



  CC_on = false;
  CalibrationMode = true;
}

void loop() {
/**********************************************
*******Read and relay throttle signals*********
************if not in cuise control************
***********************************************/  
  if(CC_on == false){
    //Read Voltage Signals
    tempSig1 = analogRead(TPS1_inPIN); //   Read Value/Resolution*RefVoltage (X/1024*5)
    tempSig2 = analogRead(TPS2_inPIN);

    if(CalibrationMode == false){                                           //test
      ThrottlePosIn = map(tempSig1,Sig1_Min,Sig1_Max,0,100);
      int NowMillis3 = millis();
      if(NowMillis3 - time3 >= 500){
        Serial.print(map(tempSig1,Sig1_Min,Sig1_Max,0,100));
        Serial.print("   ");
        Serial.println(map(tempSig2,Sig2_Min,Sig2_Max,0,100));
        time3 = NowMillis3;
      }
    }
    analogWrite(3,map(tempSig1,Sig1_Min,Sig1_Max,0,255));                            //test
   
    WriteMCP(tempSig1*4, MCP1); //MCP DAC are 12 bit, meaning 4096 = 100% or 5V
    WriteMCP(tempSig2*4, MCP2);
  }
/**********************************************
*********Calibration Mode apon startup*********
************sets min and max values************
***********************************************/   
  if(CalibrationMode == true){
    if(Run1 == false){
      Serial.println("Close Throttle...");
      Serial.println("Press Set When Ready...");
      Run1 = true;
    }
    if(digitalRead(SetPIN) == false && MinCalibrated == false){
      Sig1_Min = analogRead(TPS1_inPIN);
      Sig2_Min = analogRead(TPS2_inPIN);
      Serial.print("Minimum Calibrated to ");
      Serial.print(Sig1_Min);
      Serial.print(" for Signal 1 and ");
      Serial.print(Sig2_Min);
      Serial.println(" for Signal 2");
      Serial.println("Open Throttle Fully...");
      Serial.println("Press Reset When Ready...");
      MinCalibrated = true;
    }

    if( digitalRead(ResetPIN) == false && MinCalibrated == true){
      Sig1_Max = analogRead(TPS1_inPIN);
      Sig2_Max = analogRead(TPS2_inPIN);
      Serial.print("Maximum Calibrated to ");
      Serial.print(Sig1_Max);
      Serial.print(" for Signal 1 and ");
      Serial.print(Sig2_Max);
      Serial.println(" for Signal 2");
      CalibrationMode = false;
    }
  }
/**********************************************
************To Engage Cruise Control***********
***********************************************/   
  if(CalibrationMode == false && CC_on == false && /*kph > 30 &&*/ digitalRead(SetPIN) == false){
    setPoint = ReadKPH();            
    CC_on = true;
    CO_Done = false;
    Run2 = false;
    time6 = millis();
    Serial.println("CC_Started");    
  }
/**********************************************
**********To Disengage Cruise Control**********
***********************************************/ 
  if(map(analogRead(TPS1_inPIN),Sig1_Min,Sig1_Max,0,100)>=30 && CC_on == true && (millis() - time6) > 5000){
    CC_on = false;
    tempSig1 = analogRead(TPS1_inPIN); //   Read Value/Resolution*RefVoltage (X/1024*5)
    tempSig2 = analogRead(TPS2_inPIN);
  
    WriteMCP(tempSig1*4, MCP1); //MCP DAC are 12 bit, meaning 4096 = 100% or 5V
    WriteMCP(tempSig2*4, MCP2);

    Serial.println("CC_OFF by throttle"); 
  }  
  
  if(digitalRead(ClutchPIN) == false || digitalRead(BrakePIN) == false || digitalRead(ResetPIN) == false){
    CC_on = false;
    tempSig1 = analogRead(TPS1_inPIN); //   Read Value/Resolution*RefVoltage (X/1024*5)
    tempSig2 = analogRead(TPS2_inPIN);
  
    WriteMCP(tempSig1*4, MCP1); //MCP DAC are 12 bit, meaning 4096 = 100% or 5V
    WriteMCP(tempSig2*4, MCP2);
    int NowMillis4 = millis();
    if(NowMillis4 - time4 >= 500){
      Serial.println("CC_OFF"); 
      time4 = NowMillis4;
    }
    
  }
/**********************************************
***********Cruise Control Activated************
***********************************************/ 
  if(CC_on == true){
    if(throttleLock==false){
      
      speed = ReadKPH();
      
      myPID.run(); //run pid loop



      int NowMillis5 = millis();
      if(NowMillis5 - time5 >= 500){
        if(CO_Done == false){
          if(Run2 == false){
            TempThrottlePos = map(tempSig1,Sig1_Min,Sig1_Max,0,1023);
            Run2 = true;
          }

          TempThrottlePos = TempThrottlePos-1;
          WriteMCP(map(TempThrottlePos,0,1023,Sig1_Min,Sig1_Max)*4, MCP1);
          WriteMCP(map(TempThrottlePos,0,1023,Sig2_Min,Sig2_Max)*4, MCP2);
          analogWrite(3,map(TempThrottlePos,0,1023,0,255));                            //test
          Serial.print(outputVal);
          Serial.print("  ");
          Serial.println(TempThrottlePos);
          if(outputVal >= TempThrottlePos){
            CO_Done = true;
          }
        }
        time5 = NowMillis5;
      }


      if(CO_Done == true){
        ThrottlePosOut = map(outputVal,0,1023,0,100);
        WriteMCP(map(outputVal,0,1023,Sig1_Min,Sig1_Max)*4, MCP1);
        WriteMCP(map(outputVal,0,1023,Sig2_Min,Sig2_Max)*4, MCP2);
        analogWrite(3,map(outputVal,0,1023,0,255));                            //test
      }
    }
    if(throttleLock == true){
      TempThrottlePos = map(tempSig1,Sig1_Min,Sig1_Max,0,1023);
      WriteMCP(map(outputVal,0,1023,Sig1_Min,Sig1_Max)*4, MCP1);
      WriteMCP(map(outputVal,0,1023,Sig2_Min,Sig2_Max)*4, MCP2);

    }
    
  }
/**********************************************
***************increase Setpoint***************
***********************************************/ 
  if(CC_on == true && digitalRead(IncrPIN) == false){
    int NowMillis2 = millis();
    if(NowMillis2 - time2 >= 500){
      setPoint = setPoint + 2;
      time2 = NowMillis2;
    }
  }
/**********************************************
***************Decrease Setpoint***************
***********************************************/
  if(CC_on == true && digitalRead(DecrPIN) == false){
    int NowMillis2 = millis();
    if(NowMillis2 - time2 >= 500){
      setPoint = setPoint - 2;
      time2 = NowMillis2;
    }
  }
}

/*******************************
***********FUNCTIONS************
*******************************/

int32_t ReadKPH(){
  //Read ELM OBDII
  //kph Measurment
  
  float tempKPH = map(analogRead(A2),0,1023,0,100);
  int32_t kph = (uint32_t)tempKPH;
  return kph;
  
  /*
  float tempKPH = myELM327.kph();

  if (myELM327.nb_rx_state == ELM_SUCCESS)
  {
    int32_t kph = (uint32_t)tempKPH;
    return kph
  }
  else if (myELM327.nb_rx_state != ELM_GETTING_MSG)
    myELM327.printError();
    */
}

void WriteMCP(int Value, int Addr){//Write to I2C DAC
  Wire.beginTransmission(Addr);
  Wire.write(64);                     // cmd to update the DAC
  Wire.write(Value >> 4);        // the 8 most significant bits...
  Wire.write((Value && 15) << 4); // the 4 least significant bits...
  Wire.endTransmission();
}