#include <SoftwareSerial.h>

#define BAUD_RATE 2400
#define D5 (14)
#define D6 (12)
const short int BUILTIN_LED1 = 2; //GPIO2

byte msg1[] = {0x00};
byte msg2[] = {0x40,0x23,0x9C}; //  #CMD_TYPE (x40)  Type   checksum
byte msg3[] = {0x49,0x00,0x00,0xB6}; //  #CMD_MODE (x49)  # Modes  # Views   checksum
byte msg4[] = {0x52,0x00,0xC2,0x01,0x00,0x6E}; //  #CMD_SPEED (x52)  baud (LSB first)   checksum
byte msg5[] = {0x5F,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x02,0xA0}; //  #CMD_VERSION (x5F)  Hardware_V  Software_V   checksum
byte msg6[] = {0xA0,0x00,0x4C,0x50,0x46,0x32,0x2D,0x44,0x45,0x54,0x45,0x43,0x54,0x00,0x00,0x00,0x00,0x00,0x1D}; //  #CMD_INFO|bytes|mode  0 name 0   checksum
byte msg7[] = {0x98,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x20,0x41,0x07}; //  #CMD_INFO|3|mode  1 (raw) raw_min  raw_max   checksum
byte msg8[] = {0x98,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0xC8,0x42,0xEF}; //  #CMD_INFO|3|mode  2 (pct) pct_min  pct_max   checksum
byte msg9[] = {0x98,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x20,0x41,0x05}; //  #CMD_INFO|3|mode  3 (SI) si_min  si_max   checksum
byte msg10[] = {0x80,0x04,0x00,0x7B}; //  #CMD_INFO|size|mode  0,4,symbol   checksum
byte msg11[] = {0x88,0x05,0x10,0x00,0x62}; //  #CMD_INFO|1|mode  5 (FM) Function Map-In   -out   checksum
byte msg12[] = {0x90,0x80,0x01,0x00,0x03,0x00,0xED}; //  #CMD_INFO|2|mode  x80 sample_size   Data_type   Figures   Decimals   checksum

int isConnect = 0;
// Reminder: the buffer size optimizations here, in particular the isrBufSize that only accommodates
// a single 8N1 word, are on the basis that any char written to the loopback SoftwareSerial adapter gets read
// before another write is performed. Block writes with a size greater than 1 would usually fail.
SoftwareSerial swSer;

int intCounter=0;

void setup() {
  pinMode(BUILTIN_LED1, OUTPUT);
  digitalWrite(BUILTIN_LED1, HIGH);
  Serial.begin(115200);
  Serial.println("LEGO UART Custom Sensor");

  InitSensor();

}

void loop() {
  intCounter++;
  send_value(intCounter);

  if (intCounter>=100){
    intCounter=0;
  }

  delay(200);
}

int send_value(byte iByte){

  byte iValue = iByte & 0xFF;
  byte payload[] = {0,0,0};
  payload[0]=0xC0;
  payload[1]=iValue;
  payload[2]=0xFF ^ 0xC0 ^ iValue; //  payload = bytes([0xC0, value, 0xFF ^ 0xC0 ^ value])
  swSer.write(payload,sizeof(payload));

 }

void InitSensor(){

  Serial.println("Init sensor");

  //rx, tx
  swSer.begin(BAUD_RATE, SWSERIAL_8N1, D5, D6, false, 95, 11);

  swSer.write(msg1,sizeof(msg1));
  swSer.write(msg2,sizeof(msg2));
  swSer.write(msg3,sizeof(msg3));
  swSer.write(msg4,sizeof(msg4));
  swSer.write(msg5,sizeof(msg5));
  swSer.write(msg6,sizeof(msg6));
  swSer.write(msg7,sizeof(msg7));
  swSer.write(msg8,sizeof(msg8));
  swSer.write(msg9,sizeof(msg9));
  swSer.write(msg10,sizeof(msg10));
  swSer.write(msg11,sizeof(msg11));
  swSer.write(msg12,sizeof(msg12));

  delay(10);
  int incomingByte;
  byte b = 0x04;
  swSer.write(b); //ACK
  Serial.println("Wait for ACK from the smart hub");
  int startMillis  = millis();
  int currentMillis = startMillis;
    while ((currentMillis - startMillis) < 2000){
       if (swSer.available() > 0){

         incomingByte = swSer.read();
         //Serial.println(incomingByte);
         if (incomingByte==4){
           Serial.println("ACK Received");
           isConnect=1;
           swSer.end();
           break;
         }
       } else {
         currentMillis=millis();
       }
    }

    if (isConnect==0){
      Serial.println("Timeout: reset sensor");
      delay(1000);
      ESP.restart();
    } else {
      Serial.println("Connection OK");
      digitalWrite(BUILTIN_LED1, LOW);
      swSer.begin(115200, SWSERIAL_8N1, D5, D6, false, 95, 11);
    }
}