#define RXD2    16  // GPIO16
#define TXD2    17  // GPIO17
#define BLOCK_VALVE  23  // GPIO23
#define TANK_VALVES  22  // GPIO22
#define BLINKER 21  // GPIO21
#define PB_START 34 // GPIO34

#define XBOF_sym  0xFF // pre-amble
#define BOF_sym   0xC0  // komt 1x na een aantal XBOF's
#define EOF_sym   0XC1  // einde van het telegram

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <LiquidCrystal_I2C.h>
#include <millisDelay.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define I2C_SDA 2
#define I2C_SCL 4

// declare an SSD1306 display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
// Declare a 20x4 LCD
LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 20, 4);

#define maxTel  256
char irdiStr[maxTel+1];  // meer dan lang genoeg met terminating zero for string processing
char line[4][80];
int idx = 0;
bool screenRefresh = false;
bool tempOK = false;
bool pressOK = false;
bool armed = false;
bool startReleased = false;
millisDelay tankValvesDelay;
millisDelay overrideDelay;
millisDelay blinkDelay;
uint8_t blockValve = LOW;
uint8_t tankValves = LOW;
uint8_t blinker = LOW;

enum  operating_states {
  WAITING = 0,
  STOP_TEMP,
  STOP_PRES,
  MONITOR,
  PRE_OVERRIDE,
  OVERRIDE_PRESS
};
operating_states state = WAITING;

void goWaiting() {
  state = WAITING;
  sprintf(line[2],"%s", "Waiting for start");
  screenRefresh = true;
  armed = false;
}

void goTempStop() {
    state = STOP_TEMP;
    sprintf(line[2],"%s", "Temperature to low");
    screenRefresh = true;
}

void goPressStop() {
    state = STOP_PRES;
    sprintf(line[2],"%s", "Pressure to low");
    screenRefresh = true;
}

void goMonitoring() {
  state = MONITOR;
  sprintf(line[2],"%s", "Monitoring");
  screenRefresh = true;
}

void writeOutputs() {
  digitalWrite(BLOCK_VALVE, blockValve);
  digitalWrite(TANK_VALVES, tankValves);
  digitalWrite(BLINKER, blinker);
}

void setup() {
  // put your setup code here, to run once:
  Serial2.begin(38400, SERIAL_8N1, RXD2, TXD2);
  Serial.begin(115200);
  pinMode(BLOCK_VALVE,OUTPUT);
  pinMode(TANK_VALVES,OUTPUT);
  pinMode(BLINKER,OUTPUT);
  blockValve = LOW;
  tankValves = LOW;
  blinker = LOW;
  writeOutputs();

  pinMode(PB_START, INPUT);

  // OLED initialisation
  Wire.begin(I2C_SDA, I2C_SCL);
 
  // initialize OLED display with address 0x3C for 128x64
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
  delay(2000);         // wait two seconds for initializing
  goWaiting();

  LCD.init();
  LCD.backlight();
  // blinkDelay.stop();
}

void rewriteScreen() {
  char *ptr;

  if (screenRefresh) {
    // LCD.clear();  // dit flikkert wel in simulatie, maar hoe kun je dit verbeteren?
    screenRefresh = false;
    oled.clearDisplay();
    oled.setTextSize(1);      // set text size
    oled.setTextColor(WHITE); // set text color
    for( int i=0; i<4; i++) {
      oled.setCursor(0, 10*i);    // set position to display
      oled.print(line[i]);
      Serial.println(line[i]);
      // 20x4 LCD. Beetje vervelend, OLED graden is 0xF7, LCD graden is 0xDF
      // while( (ptr = strchr(line[i],'\xF7')) != NULL) {
      //   *ptr = '\xDF';
      // }
      // LCD.setCursor(0,i);
      // LCD.print(line[i]);
    }
    oled.display();
  }
}

void writeTemp(float t) {
  // Kelvin to Celcius
  // blijkbaar is 247 graden ipv 248 bij de SSD1306
  sprintf(line[0],"temp = %.1f \xF7\x43", t-273.1);  // Should be 273.15, but SAE J2799 specifies only 1 decimal
  // sprintf(line[0],"temp = %.1f \xDF\x43", t-273.1);  // Should be 273.15, but SAE J2799 specifies only 1 decimal
  screenRefresh = true;
}

void writePress(float p) {
  // MPascal to bar
  sprintf(line[1],"press = %.1f bar", p*10.0);
  screenRefresh = true;
}

void processTelegram() {
  Serial.println("processing");
  Serial.println(irdiStr);
  // Serial.println(strlen(irdiStr));
  for( int i=0; i<strlen(irdiStr); i++) {
    if( strncmp(&irdiStr[i], "|ID", 3) == 0) {
      // ID field, should be |ID=SAE J2799
      Serial.println("ID-field");
      while(irdiStr[++i] != '|') ;
      i--;
      continue;
    }
    if( strncmp(&irdiStr[i], "|MP", 3) == 0) {
      // temperature field, should be |MT=abc.d
      int v1,v2;
      sscanf(&irdiStr[i],"|MP=%d.%d|", &v1,&v2);
      float p = (float)v1 + float(v2)/10.0;
      writePress(p);
      Serial.print("MP-field : ");
      Serial.println(p);
      // geen hysterese, want we hoeven alleen maar te stoppen
      if (p <= 0.8) {
        // onder de 8 bar
        pressOK = false;
      } else {
        pressOK = true;
      }
      while(irdiStr[++i] != '|') ;
      i--;
      continue;
    }
    if( strncmp(&irdiStr[i], "|MT", 3) == 0) {
      // temperature field, should be |MT=abc.d
      int v1,v2;
      sscanf(&irdiStr[i],"|MT=%d.%d|", &v1,&v2);
      float t = (float)v1 + float(v2)/10.0;
      writeTemp(t);
      Serial.print("MT-field : ");
      Serial.println(t);
      // wel hysterese, want we moeten weer verder kunnen als we weer ontdooit zijn
      // en er nog steeds druk in het systeem zit
      if (tempOK && t <= 253.1) {
        // onder de -20
        tempOK = false;
      }
      if (!tempOK && t > 263.1) {
        // boven de -10
        tempOK = true;
      }
      while(irdiStr[++i] != '|') ;
      i--;
      continue;
    }
  }
  Serial.println("ready");
}

#define BLINK_TIME 500
void slowBlink() {
  if( blinkDelay.justFinished()) {
    if( blinker == HIGH)
      blinker = LOW;
    else
      blinker = HIGH;
    blinkDelay.repeat();
  }
  if( !blinkDelay.isRunning()) {
    blinkDelay.start(BLINK_TIME);
  }
}

void fastBlink() {
  if( blinkDelay.justFinished()) {
    if( blinker == HIGH)
      blinker = LOW;
    else
      blinker = HIGH;
    blinkDelay.repeat();
  }
  if( !blinkDelay.isRunning()) {
    blinkDelay.start(BLINK_TIME/2);
  }
}

void stopBlink() {
          blinkDelay.stop();
          blinker = LOW;
}

void processState() {
  bool pbStart = digitalRead(PB_START); // low = pressed!
  switch( state) {
    case WAITING:
      if (!pbStart && !armed) {
        Serial.println("armed");
        armed = true;
      }
      if (pbStart && armed) {
        Serial.println("trigger");
        armed = false;
        goMonitoring();
      }
      break;
    case STOP_TEMP:
      if (tempOK) {
        if (!pressOK) {
          goPressStop();
        } else {
          goWaiting();
        }
      }
      break;
    case STOP_PRES:
      if (pressOK) {
        if (!tempOK) {
          goTempStop();
        } else {
          goWaiting();
        }
      }
      if( !pbStart) {
        // press the start button for 5 seconds to open the valves even
        // when the pressure is below 8 bar
        slowBlink();
        sprintf(line[3],"timer=%lu", overrideDelay.remaining());
        screenRefresh = true;
        if( !overrideDelay.isRunning()) {
          overrideDelay.start(5000);
        }
        if( overrideDelay.justFinished()) {
          stopBlink();
          state = PRE_OVERRIDE;
          sprintf(line[3],"Entering PRE_OVERRIDE");
          screenRefresh = true;
          startReleased = false;
        }
      }
      else {
        if( overrideDelay.isRunning()) {
          overrideDelay.stop();
          stopBlink();
        }
      }
      break;
    case PRE_OVERRIDE:
      // the user has pressed for 5 seconds, now wait for release and press
      // within 5 seconds
      if( !overrideDelay.isRunning()) {
        overrideDelay.start(5000);
      }
      fastBlink();
      if( pbStart) {
        // start released, press again within 5 sec
        startReleased = true;
      }
      if( startReleased && !pbStart) {
        overrideDelay.stop();
        stopBlink();
        state = OVERRIDE_PRESS;
      }
      if( overrideDelay.justFinished()) {
        // not pressed again within 5 seconds, revert to STOP_PRES
        stopBlink();
        state = STOP_PRES;
      }
      break;
    case OVERRIDE_PRESS:
      // valves are open now. Keep checking the temperature
      if (!tempOK) {
        goTempStop();
      }
      // if the pressure is okay the user can let go of the
      // start button as the state will be set to MONITOR
      // This is shown on the display which is not visible for now
      if (pressOK) {
        goMonitoring();
      }
      if( pbStart) {
        // startbutton released while in override, back to STOP_PRES
        state = STOP_PRES;
      }
      break;
    case MONITOR:
      if (!tempOK) {
        goTempStop();
      }
      if (!pressOK) {
        goPressStop();
      }
      break;
  }
}


// LET OP : loop wordt continue aangeroepen als een soort sceduler
void loop() {
  char tmpStr[16];
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation, but delays every loop with 10ms!
  processState();

  while( Serial2.available()) { // get the number of characters in the UART buffer
    // and process them all
    int ch = Serial2.read();
    switch(ch) {
      case BOF_sym:
        sprintf(tmpStr,"%c", ch);
        Serial.println(tmpStr);
        idx = 0;
        break;
      case EOF_sym:
        sprintf(tmpStr,"%c", ch);
        Serial.println(tmpStr);
        processTelegram();
        break;
      default :
        if (idx < maxTel) {
          irdiStr[idx++] = ch;
          irdiStr[idx] = '\0';
        }
        break;
    }
  }
  rewriteScreen();

  // do something with the ouputs depending on the state-machine
  if((state == MONITOR) || (state == OVERRIDE_PRESS)) {
    blockValve = HIGH;
    tankValves = HIGH;
    blinker = HIGH;
  }
  if((state != MONITOR) && (state != OVERRIDE_PRESS) && (blockValve == HIGH)) {
    // no longer monitoring due to low temperature or low pressure
    blockValve = LOW;
    tankValvesDelay.start(200); // close the tankvalves after 200ms
  }
  if(tankValvesDelay.justFinished()) {
    tankValves = LOW;
    blinker = LOW;
  }
  writeOutputs();
}