#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <DHT.h>
#include <DHT_U.h>
//a #include "DFRobot_OzoneSensor.h"
//a #include <PID_v1.h>

#define MENU_CONTROL 0
#define MENU_FRUIT 1
#define MENU_PPM_TARGET 2
#define MENU_PROCESS 3

#define SET 0
#define CUSTOM 1

#define enA 9
#define in1 8
#define in2 7

//a #define DHTTYPE DHT11
//a #define dht_dpin 10

//a #define COLLECT_NUMBER 20
//a #define ADDRESS_3 0x73
//a #define Ozone_IICAddress ADDRESS_3

const byte ROWS = 4;
const byte COLS = 4;
char hexaKeys[ROWS][COLS] = {
  {'D', '#', '0', '*'},
  {'C', '9', '8', '7'},
  {'B', '6', '5', '4'},
  {'A', '3', '2', '1'}
};
byte rowPins[ROWS] = {45, 43, 41, 39};
byte colPins[COLS] = {53, 51, 49, 47};
Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);

// --------------------------------------
// mode keypad
// A : change mode
// B : enter
// C : back & stop
// D : sensor monitor
// --------------------------------------

//a DFRobot_OzoneSensor Ozone;
//a DHT dht(dht_dpin, DHTTYPE);
LiquidCrystal_I2C lcd(0x27, 16, 2);

double setPoint, feedbackControl, outputPID;
// ====   Konstanta Sterilisasi Optimal Hasil Uji Coba (436, 4.36, 0.01)    ====
double Kp = 436, Ki = 4.36, Kd = 0.01; //--> gunakan untuk pengawetan, optimasi nanti
//a PID myPID(&feedbackControl, &outputPID, &setPoint, Kp, Ki, Kd, DIRECT);

char *ptr;

uint8_t modeControl = 0;
uint8_t setFruit = 0;
uint8_t stateMenu = 0;
uint8_t stateKeypad = 0;
uint8_t stateSetting = 0;
uint16_t startTime;

String customSetPoint = "";
String customSetPID = "";

bool setTarget = 0;
bool stateStart = false;
bool stateReachTarget = false;
bool stateInfo = false;
bool stateSerialPrint = false;

float ozoneConcentration;
float O3ppm;
float temp;
float fruit [2][2] = {
  {0.5, 0.4}, // banana || pear
  {5.0, 5.0}  
};
void setup(){
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(enA, 0);

  // Inisiasil LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(3, 0);
  lcd.print("SIKOMOBUAS");
  lcd.setCursor(6, 1);
  lcd.print ("BRIN");
  delay(3000);
  
  //a dht.begin();
  Serial.begin(9600);
  Serial.println("time(s), mode_control, fruit, set_point, O3, Kp, Ki, Kd, temp, outputPWMControl");
  //a while(!Ozone.begin(Ozone_IICAddress)){
  //a  Serial.println("I2c device number error !");
  //a  delay(1000);
  //a }
  //Serial.println("I2C connection success !");
  //a Ozone.SetModes(MEASURE_MODE_PASSIVE);
  //a myPID.SetMode(AUTOMATIC);
  lcd.clear();
}

void loop(){
  char key = customKeypad.getKey();
  keypadInput(key);
  sensorRead();
  if(!stateSetting)
    listMenu();
  else
    setting();
  ozoneControl();
  dataLogSerial();
}

void dataLogSerial()
{
  if(stateSerialPrint){    
    static uint32_t printTime;
    if (millis() - printTime > 100) {
      printTime = millis();
      float secondTime = (millis() - startTime) / 100;
      
      char str_setPoint[6], str_O3ppm[6], str_temp[6];
      char str_Kp[6], str_Ki[6], str_Kd[6];
      char buffer_print[50];

      dtostrf(setPoint, 4, 2, str_setPoint);
      dtostrf(O3ppm, 4, 2, str_O3ppm);
      dtostrf(temp, 4, 2, str_temp); //error bug, aneh
      dtostrf(Kp, 4, 2, str_Kp);
      dtostrf(Ki, 4, 2, str_Ki);
      dtostrf(Kd, 4, 2, str_Kd);

      //timeStamp(ms), mode, fruit, set point, o3, kp, ki, kd, temp
      sprintf(buffer_print,",%d,%d,%s,%s,%s,%s,%s,", modeControl, setFruit, str_setPoint, str_O3ppm, str_Kp, str_Ki, str_Kd); 
      Serial.print(secondTime/10, 1);
      Serial.print(buffer_print);
      Serial.println(temp);
    }
  }
}

// Tampilan Pilihan Mode
void listMenu(){
  switch(stateMenu)
  {    
    case MENU_CONTROL:    
      stateStart = false;
      lcd.setCursor(0, 0);
      lcd.print("MODE CONTROL:");
      lcd.setCursor(0, 1);
      if(stateKeypad>1)
        stateKeypad = 0;
      modeControl = stateKeypad;
      if(!modeControl)
        lcd.print("1. Sterilization");
      else   
        lcd.print("2. Preservation ");
    break;    

    case MENU_FRUIT:    
      stateStart = false;
      lcd.setCursor(0, 0);
      lcd.print("Fruit:");
      lcd.setCursor(0, 1);
      if(stateKeypad>1)
        stateKeypad = 0;
      setFruit = stateKeypad;
      if(!setFruit)
        lcd.print("1. Banana  ");
      else   
        lcd.print("2. Pear    ");
    break;    

    case MENU_PPM_TARGET:    
      stateStart = false;
      lcd.setCursor(0, 0);
      if(!modeControl)
        lcd.print("Steril: ");
      else   
        lcd.print("Preser: ");

      if(!setFruit)
        lcd.print("Banana");
      else   
        lcd.print("Pear  ");
      lcd.setCursor(0, 1);
      lcd.print("PPM>");

      if(stateKeypad>1)
        stateKeypad = 0;
      setTarget = stateKeypad;
      if(!setTarget){
        lcd.print("Default:");
        lcd.print(fruit[modeControl][setFruit]);
      }
      else{   
        lcd.print("Custom: ");
        lcd.print(customSetPoint);
      }
    break;    

    case MENU_PROCESS:    
      stateStart = true;
      lcd.setCursor(0, 0);
      if(!modeControl)
        lcd.print("Sterilization...");
      else   
        lcd.print("Preservation....");  

      lcd.setCursor(0, 1);
      lcd.print("STATUS: ");
      if(!stateReachTarget)
        lcd.print("Control... ");
      else   
        lcd.print("Established");  

    break;    
  }    
}

// Menu Awal
void keypadInput(char key){
  //change mode
  if(key == 'A'){
    stateKeypad++;
    lcd.clear();
  }
  //enter
  else if(key == 'B'){
    stateKeypad = -1;
    if(stateSetting < 4)
      lcd.clear();
    if(!stateSetting){
      stateMenu++;
      if(stateMenu>3)
        stateMenu = 3;
    }
    else if(stateSetting >= 2 && stateStart == false)
    {
      stateSetting++;
      customSetPID = "";
    }
  }
  //back
  else if(key == 'C'){
    lcd.clear();
    if(!stateSetting){
      stateMenu--;
      if(stateMenu<0 || stateMenu>10)
        stateMenu = 0;
    }
    stateSetting = 0;
    stateSerialPrint = false;

    if(stateMenu == MENU_CONTROL)
      stateKeypad = modeControl;
    else if(stateMenu == MENU_FRUIT)
      stateKeypad = setFruit;
    else if(stateMenu == MENU_PPM_TARGET)
      stateKeypad = setTarget;
  }
  //monitor
  else if(key == 'D'){
    lcd.clear();
    stateSetting++;
    if(stateSetting == 3)
      stateSetting = 0;     
  }

  if((stateMenu == MENU_PPM_TARGET && setTarget == CUSTOM) || stateSetting>=4 ){
    if(key == '#'){
      key = "";
      customSetPoint = "";
      customSetPID = "";
      lcd.clear();
    }
    else if(key == 'A' || key == 'B' || key == 'C' || key == 'D')
    {
      // do nothing
    }
    else
    {
      if(key == '*')
        key = '.';
      if(stateSetting>=4)
        customSetPID += String(key);
      else
        customSetPoint += String(key);
      
    }
  }

  if(stateStart && key == '#'){
    stateSerialPrint = true;
    startTime = millis();
  }
}

void setting(){
  if(stateSetting == 1){
    if(!stateStart && !stateInfo){
      lcd.setCursor(0, 0);
      lcd.print("A:Change C:Back");
      lcd.setCursor(0, 1);
      lcd.print("B:Set  D:Monitor");
      delay(3000);
      lcd.clear();
      stateInfo = true;
    }
    lcd.setCursor(0, 0);
    lcd.print("Monitor ");
    lcd.setCursor(9, 0);
    lcd.print(temp, 2);
    lcd.setCursor(14, 0);
    lcd.print((char)223);
    lcd.print("C");

    lcd.setCursor(0, 1);
    lcd.print("O3:");
    lcd.setCursor(3, 1);
    lcd.print(O3ppm, 2);
    lcd.setCursor(9, 1);
    lcd.print("SP:");
    lcd.setCursor(13, 1);
    lcd.print(setPoint);
  }
  else if(stateSetting == 2){
    stateInfo = false;
    lcd.setCursor(0, 0);
    lcd.print("PID Parameter: ");

    lcd.setCursor(0, 1);
    lcd.print(Kp,1);
    lcd.setCursor(6, 1);
    lcd.print(Ki,2);
    lcd.setCursor(12, 1);
    lcd.print(Kd,2);
  }
  else if(stateSetting == 3){
    lcd.setCursor(0, 0);
    lcd.print("Set new PID?");
    lcd.setCursor(0, 1);
    lcd.print("B:Enter C:Cancel");
  }
  else if(stateSetting >= 4){
    setNewPID_interface();
  }
}

void setNewPID_interface(){
  if(stateSetting == 4){
    lcd.setCursor(1, 0);
    lcd.print("Kp");
    lcd.setCursor(0, 1);
    lcd.print(customSetPID);
    Kp = customSetPID.toDouble();
  }
  else if(stateSetting == 5){
    Ki = customSetPID.toDouble();
    lcd.setCursor(7, 0);
    lcd.print("Ki");      
    lcd.setCursor(7, 1);
    lcd.print(customSetPID);
  }
  else if(stateSetting == 6){
    Kd = customSetPID.toDouble();
    lcd.setCursor(13, 0);
    lcd.print("Kd");      
    lcd.setCursor(13, 1);
    lcd.print(customSetPID);
  }
  else if(stateSetting == 7){
    lcd.setCursor(0, 0);
    lcd.print("NewPID Parameter");
    lcd.setCursor(0, 1);
    lcd.print("has been set!   ");
    delay(2000);
    lcd.clear();
    stateSetting = 2;
  }  
}

void sensorRead(){
    //a temp = dht.readtemperature();
    //a ozoneConcentration = Ozone.ReadOzoneData(COLLECT_NUMBER);
    temp = 20; //b
    ozoneConcentration = 1000; //b
    O3ppm = ozoneConcentration / 1000;
}

void ozoneControl(){
  if(stateStart){
    setPoint = double(fruit[modeControl][setFruit]);
    if(setTarget == CUSTOM)
      setPoint = customSetPoint.toDouble();   
    feedbackControl = O3ppm;
    //a myPID.Compute();
    //a analogWrite(enA, outputPID);
    analogWrite(enA, 255);  //b
    checkStatus();
  }
  else{
    analogWrite(enA, 0);  
  }
}

void checkStatus(){
  double errorTarget = double(O3ppm)-setPoint;
  if(abs(errorTarget) < 0.05)
    stateReachTarget = true;
  else
    stateReachTarget = false;
}