#include "max6675.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

/**********************************************************/
int thermoDO_1 = 2;
int thermoCS_1 = 3;
int thermoCLK_1 = 4;
int vccPin_1 = 5;
int gndPin_1 = 6;

int FrntInv = 9;      // LED connected to digital pin 10
int RearInv = 10;      // LED connected to digital pin 10

MAX6675 thermocouple_1(thermoCLK_1, thermoCS_1, thermoDO_1);

/**********************************************************/

int TqTgt = 0;
int TqTgtFrnt = 0;
int TqTgtRear = 0;
int Temp = 0 ;
int TqTempCorr = 0;
int TempMax = 30;
int AccThrRaw = 0;
int TqSplitRaw = 0;
float Voltage = 0;
int AccThr = 0;
int TqSplitBias = 0;
int CurSnsFrntRaw = 0;
int CurSnsRearRaw = 0;
int VoltSnsFrntRaw = 0;
int VoltSnsRearRaw = 0;
float  CurSnsFrnt = 0;
float  CurSnsRear = 0;
float  VoltSnsFrnt = 0;
float  VoltSnsRear = 0;
float TracFrntWatt = 0;
float TracRearWatt = 0;
int TqTgtMax = 255;
int TqTgtFrntFinal = 0;
int TqTgtRearFinal = 0;
int TracFrntMax = 350;
int TracRearMax = 350;
double KpF = 0.1;
double KiF = 0.01;
double KpR = 0.1;
double KiR = 0.01;
double iErrFrnt = 0;
double pErrFrnt = 0;
double CorrFrnt = 0;
double pErrRear = 0;
double iErrRear = 0;
double CorrRear = 0;

unsigned long millisnow = 0;
unsigned long millislast = 0;
int LoopTime = 0;
float QAmp = 0;
float AmpHour = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2); // set the LCD address to 0x27 for a 16 chars and 2 line display
/*********************************************************/

void setup() {

  Serial.begin(9600);
  pinMode(FrntInv, OUTPUT);  // sets the pin as output
  pinMode(RearInv, OUTPUT);  // sets the pin as output
  pinMode(vccPin_1, OUTPUT); digitalWrite(vccPin_1, HIGH);
  pinMode(gndPin_1, OUTPUT); digitalWrite(gndPin_1, LOW);

  Serial.println("MAX6675 test");
  // wait for MAX chip to stabilize
  delay(500);

  lcd.init();         //initialize the lcd
  lcd.backlight();    //open the backlight
}

void loop()  {

  millisnow = millis();

  AccThrRaw =       analogRead(A0);   // analogRead values go from 0 to 1023
  TqSplitRaw =      analogRead(A1);   // analogRead values go from 0 to 1023
  CurSnsFrntRaw =   analogRead(A2);
  CurSnsRearRaw =   analogRead(A3);
  VoltSnsFrntRaw =  analogRead(A6);
  VoltSnsRearRaw =  analogRead(A7);

  Voltage = AccThrRaw * (5 / 1023.0);   // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V):

  AccThr = map(AccThrRaw, 190, 840, 0, 100);  //convert the range 0.9V - 4.1V to 0%- 100%

  if (AccThr > 100) {
    AccThr = 100;
  }
  else if (AccThr < 0) {
    AccThr = 0;
  }
  else {
    AccThr = AccThr;
  }

  TqTgt = map(AccThr, 0, 100, 0, 255);

  Temp = thermocouple_1.readCelsius();  // read the temperature

  TqTempCorr = 2 * (Temp - TempMax);      // Calculate the units off correction

  TqTgt = TqTgt - TqTempCorr;             // reduce total torque target with the temp correction

  // contain the torque target within 0-255
  if (TqTgt < 0) {
    TqTgt = 0;
  }
  else if (TqTgt > 255) {
    TqTgt = 255;
  }
  else {
    TqTgt = TqTgt;
  }

  // map the torque spli bias from negative torque target to positive torque target
  TqSplitBias = map(TqSplitRaw, 0, 1023, -TqTgt, TqTgt);

  if (TqSplitRaw > 532) {
    TqTgtFrnt = TqTgt - TqSplitBias;
    TqTgtRear = TqTgt;
  }
  else if (TqSplitRaw < 492) {
    TqTgtFrnt = TqTgt;
    TqTgtRear = TqTgt  + TqSplitBias;
  }
  else {
    TqTgtFrnt = TqTgt;
    TqTgtRear = TqTgt;
  }

  // contain the torque target within 0-255
  if (TqTgtFrnt < 0) {
    TqTgtFrnt = 0;
  }
  else if (TqTgtFrnt > 255) {
    TqTgtFrnt = 255;
  }
  else {
    TqTgtFrnt = TqTgtFrnt;
  }
  // contain the torque target within 0-255
  if (TqTgtRear < 0) {
    TqTgtRear = 0;
  }
  else if (TqTgtRear > 255) {
    TqTgtRear = 255;
  }
  else {
    TqTgtRear = TqTgtRear;
  }

  CurSnsFrnt = CurSnsFrntRaw * (40 / 1023.0) - 20; // adding -20 to account for +/-20 type sensor
  CurSnsRear = CurSnsRearRaw * (40 / 1023.0) - 20; // adding -20 to account for +/-20 type sensor

  VoltSnsFrnt = VoltSnsFrntRaw * (50 / 1023.0);
  VoltSnsRear = VoltSnsRearRaw * (50 / 1023.0);

  TracFrntWatt = CurSnsFrnt * VoltSnsFrnt;
  TracRearWatt = CurSnsRear * VoltSnsRear;

  pErrFrnt = TracFrntWatt - TracFrntMax;
  iErrFrnt = iErrFrnt + (pErrFrnt / 10);
  pErrRear = TracRearWatt - TracRearMax;
  iErrRear = iErrRear + (pErrRear / 10);

  TracFrntMax = 350;
  TracRearMax = 350;

  if (TracFrntWatt < 350) {
    TracFrntMax = TracFrntWatt;
    KpF = 0;
    KiF = 0;
    iErrFrnt = 0;
    pErrFrnt = 0;
  }
  else {
    KpF = 0.1;
    KiF = 0.01;
    TracFrntMax = TracFrntMax;
  }

  if (TracRearWatt < 350) {
    TracRearMax = TracRearWatt;
    KpR = 0;
    KiR = 0;
    iErrRear = 0;
    pErrRear = 0;
  }
  else {
    KpR = 0.1;
    KiR = 0.01;
    TracRearMax = TracRearMax;
  }

  CorrFrnt = pErrFrnt * KpF + iErrFrnt * KiF;
  CorrRear = pErrRear * KpR + iErrRear * KiR;

  TqTgtFrntFinal = TqTgtFrnt - CorrFrnt; //
  TqTgtRearFinal = TqTgtRear - CorrRear;

  analogWrite(FrntInv, TqTgtFrntFinal); // analogWrite values from 0 to 255
  analogWrite(RearInv, TqTgtRearFinal);      // analogWrite values from 0 to 255

  // Serial.print(" Acc " );
  // Serial.print(AccThr);
  // Serial.print(" voltage " );
  // Serial.print(Voltage);
  // Serial.print(" TqTgt " );
  // Serial.print(TqTgt);
  // Serial.print(" TqSplitRaw " );
  // Serial.print(TqSplitRaw);
  // Serial.print(" TqTgtFrnt " );
  // Serial.print(TqTgtFrnt);
  // Serial.print(" TqTgtRear " );
  // Serial.print(TqTgtRear);
  // Serial.print(" T ");
  // Serial.print(Temp);
  // Serial.print(" T1 = ");
  // Serial.println(thermocouple_1.readCelsius());

  Serial.print(" CurSnsFrnt " );
  Serial.print(CurSnsFrnt);
  Serial.print(" CurSnsRear " );
  Serial.print(CurSnsRear);

  // Serial.print(VoltSnsFrnt);
  // Serial.print(" VoltSnsFrnt " );
  // Serial.print(VoltSnsRear);
  // Serial.print(" VoltSnsRear " );
  // Serial.print(TracFrntWatt);
  // Serial.print(" TracFrntWatt " );
  // Serial.print(TracRearWatt);
  // Serial.print(" TracRearWatt " );

  // Serial.print(" pErrFrnt " );
  // Serial.print(pErrFrnt);
  // Serial.print(" iErrFrnt " );
  // Serial.print(iErrFrnt);
  // Serial.print(" CorrFrnt " );
  // Serial.print(CorrFrnt);
  // Serial.print(" TracFrntWatt " );
  // Serial.print(TracFrntWatt);
  // Serial.print(" TracFrntMax " );
  // Serial.print(TracFrntMax);
  // Serial.print(" TqTgtFrnt ");
  // Serial.print(TqTgtFrnt);
  // Serial.print(" TqTgtFrntFinal ");
  // Serial.print(TqTgtFrntFinal);
  // Serial.print(" TqTgtRearFinal ");
  // Serial.print(TqTgtRearFinal);

  Serial.print(" millislast " );
  Serial.print(millislast);
  Serial.print(" millisnow " );
  Serial.print(millisnow);
  Serial.print(" LoopTime " );
  Serial.print(LoopTime);
  Serial.print(" QAmp " );
  Serial.print(QAmp);
  Serial.print(" AmpHour " );
  Serial.print(AmpHour);

  Serial.println();


  lcd.setCursor(0, 0); // set the cursor to column 1, line 0
  lcd.print("Acc "); // Print a message to the LCD.
  lcd.setCursor(4, 0); // set the cursor to column 1, line 0
  lcd.print(AccThr);
  lcd.setCursor(0, 1); // set the cursor to column 1, line 0
  lcd.print(TqTgt);
  lcd.setCursor(4, 1); // set the cursor to column 1, line 0
  lcd.print(TqTgtFrnt);
  lcd.setCursor(8, 1); // set the cursor to column 1, line 0
  lcd.print(TqTgtRear);

  lcd.setCursor(12, 1); // set the cursor to column 1, line 0
  lcd.print(Temp); // Print a message to the LCD.
  lcd.setCursor(14, 1); // set the cursor to column 1, line 0
  lcd.print((char)223); // Print a message to the LCD.
  lcd.setCursor(15, 1); // set the cursor to column 1, line 0
  lcd.print("C"); // Print a message to the LCD.
  //lcd.clear();

  delay(200);

  millislast = millis();
  LoopTime = millislast - millisnow;
  QAmp = (CurSnsFrnt + CurSnsRear) / 1000 * LoopTime;
  AmpHour = AmpHour + (QAmp / 3600);

}