#include <Arduino.h>
#include <Ticker.h>
#include <FunctionalInterrupt.h>
#include "BluetoothSerial.h"


#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

BluetoothSerial SerialBT;

const byte interruptPin = 35; //rotor interrupt pin 35 for left encoder
const byte directionPin = 33; //rotor direction pin 33 for right encoder
const byte brake = 22; //brake output
const byte act1 = 27; //braille row1
const byte act2 = 21; //braille row2
const byte act3 = 32; //braille row3
const byte BL1 = 4; //button L middle+
const byte BL2 = 25; //button L middle-
const byte BL3 = 0; //button L thumb+
const byte BL4 = 2; //button L thumb-
const byte BR1 = 26; //button R middle+ 19 for old board, 26 for remapped, 26 for new board
const byte BR2 = 18; //button R middle- 34 with added pullup resistor, 18 for remapped, 18 for new board
const byte BR3 = 23; //button R thumb+
const byte BR4 = 5; //button R thumb-
const byte LED_PIN = 19; //Green LED
const unsigned int strsize = 256; //max string buffer size


unsigned long lastDetection = 0; //rotor interrupt detection time (ms)
unsigned long BTtime0 = 0; //BT interrupt detection time (ms)
unsigned long SWtime0 = 0; //Button interrupt detection time (ms)
unsigned long debounceTime = 30; //sensor debounce time (ms)
unsigned long BTdelay0 = 10; //BT debounce time (ms)
unsigned long SWdelay0 = 300; //Button debounce time (ms)
unsigned long trig0 = 0; //Interrupt response timer (ms)

byte i = 0; //conversion table index
bool directdrv = 0;
bool sensor1 = 0;
bool sensor2 = 0;
bool sensor1p = 0;
bool sensor2p = 0;
bool swL1 = 1;
bool swL2 = 1;
bool swL3 = 1;
bool swL4 = 1;
bool swR1 = 1;
bool swR2 = 1;
bool swR3 = 1;
bool swR4 = 1;
bool swL1p = 1;
bool swL2p = 1;
bool swL3p = 1;
bool swL4p = 1;
bool swR1p = 1;
bool swR2p = 1;
bool swR3p = 1;
bool swR4p = 1;
bool row1p = 0;
bool LEDstate = 1;
String BTstring = ""; //bluetooth input
String BTheader = ""; //bluetooth header
String brlcode = "70707070402010224466775533070707"; //braille string container
String blinkcode = ""; //blinker code
unsigned int brstr = brlcode.length(); //initial string length
unsigned int ringstep = 0; //character index counter
byte byte1 = 0; //temp byte output
char tmp1[] = {"00"}; //temp hex output

//Ticker
Ticker blinker;

float blinkerPace = 0.5;  //seconds
float userPace = 0.5; //user defined pace

void BTread() {
  if (SerialBT.available()) {
    BTstring = SerialBT.readStringUntil('\n');
    BTstring.trim();
    BTheader = BTstring;
    BTheader.remove(5); //Extract header in the first 5 character
    
    //store braille output if BT string contains braille code
    if (BTheader == "[brl]"){
      blinker.detach();
      blinkerPace = 0.125; //set blinker pace
      blinker.attach(blinkerPace, blink);
      brlcode = BTstring;
      brlcode.remove(0,5); //remove header
      brstr = min(brlcode.length(), strsize); //Limit string length to max allocated memory

      ringstep = 0;
    }
    if (BTheader == "[blk]"){
      blinkcode = BTstring;
      blinkcode.remove(0,5); //remove header
      blinker.detach();
      userPace = max(0.1,blinkcode.toDouble()); //set blinker pace minimum 0.1
      blinkerPace = userPace;
      blinker.attach(blinkerPace, blink);
    }
  }
}

void blink() {
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  //Read bluetooth input every blink
  BTread();
}

//pin interrupt critical function
SemaphoreHandle_t syncSemaphore;
 
void IRAM_ATTR handleInterrupt() {
    xSemaphoreGiveFromISR(syncSemaphore, NULL);
}
 
void setup() {
 
  Serial.begin(115200);
  SerialBT.begin("ReadRing Prototype_2021 no.1"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
 
  syncSemaphore = xSemaphoreCreateBinary();
 
  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(directionPin, INPUT_PULLUP);
  pinMode(BL1, INPUT_PULLUP);
  pinMode(BL2, INPUT_PULLUP);
  pinMode(BL3, INPUT_PULLUP);
  pinMode(BL4, INPUT_PULLUP);
  pinMode(BR1, INPUT_PULLUP);
  pinMode(BR2, INPUT_PULLUP);
  pinMode(BR3, INPUT_PULLUP);
  pinMode(BR4, INPUT_PULLUP);
  pinMode(brake, OUTPUT);
  pinMode(act1, OUTPUT);
  pinMode(act2, OUTPUT);
  pinMode(act3, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(act1,0);
  digitalWrite(act2,0);
  digitalWrite(act3,0);
  digitalWrite(brake,0);
  digitalWrite(LED_PIN,LEDstate);
  
  attachInterrupt(digitalPinToInterrupt(interruptPin), handleInterrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(BL1), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BL2), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BL3), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BL4), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BR1), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BR2), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BR3), handleInterrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(BR4), handleInterrupt, FALLING);
//ticker
  blinker.attach(blinkerPace, blink);
}
 
void loop() {
 
  xSemaphoreTake(syncSemaphore, portMAX_DELAY);
  trig0 = millis();
//Respond to Bluetooth events
  if(millis() - BTtime0 > BTdelay0){
    BTtime0 = millis();
    BTread();
  }
  
//Braille actuator control
  if(millis() - lastDetection > debounceTime){
    sensor1 = digitalRead(interruptPin);
    sensor2 = digitalRead(directionPin);

    //Rotation trigger
    if(sensor1 != sensor1p){
      lastDetection = millis();
      if(sensor1 == sensor2){
        tmp1[0] = brlcode.charAt(ringstep);
        tmp1[1] = brlcode.charAt(ringstep + 1);
        byte1 = strtol(tmp1,NULL,16);
        //equal for sensor L as interrupt, not equal for sensor R as interrupt
        if(sensor1 == 1){
          digitalWrite(act1,bitRead(byte1,4));
          digitalWrite(act2,bitRead(byte1,5));
          digitalWrite(act3,bitRead(byte1,6));
          row1p = 1; //flag as read
          
 
        } else if(row1p == 1){
          digitalWrite(act1,bitRead(byte1,0));
          digitalWrite(act2,bitRead(byte1,1));
          digitalWrite(act3,bitRead(byte1,2));
          
          ringstep += 2;
          if (ringstep >= brstr){
            SerialBT.println(F("sw_fwd"));
            blinker.detach();
            blinkerPace = userPace; //reset blinker pace
            blinker.attach(blinkerPace, blink);
            ringstep = 0;
          }
          
          row1p = 0; //clear read flag
        }
      } else{
        digitalWrite(act1,0);
        digitalWrite(act2,0);
        digitalWrite(act3,0);
        row1p = 0; //clear read flag
        //
        if(sensor1 != 1 && ringstep >= 2){
          ringstep -= 2;
        }
  
      }
    }
    sensor1p = sensor1;
    
  }
  
//Buttons
  if(millis() - SWtime0 > SWdelay0){
    swL1 = digitalRead(BL1);
    swL2 = digitalRead(BL2);
    swL3 = digitalRead(BL3);
    swL4 = digitalRead(BL4);
    swR1 = digitalRead(BR1);
    swR2 = digitalRead(BR2);
    swR3 = digitalRead(BR3);
    swR4 = digitalRead(BR4);
    if(swL1 == 0){
      SerialBT.println(F("L1"));

    }
    if(swL2 == 0){
      SerialBT.println(F("L2"));

    }
     if(swL3 == 0){
      SerialBT.println(F("L3"));

    }
    if(swL4 == 0){
      SerialBT.println(F("L4"));

    }
    if(swR1 == 0){
      SerialBT.println(F("R1"));

    }
    if(swR2 == 0){
      SerialBT.println(F("R2"));

    }
     if(swR3 == 0){
      SerialBT.println(F("R3"));

    }
    if(swR4 == 0){
      SerialBT.println(F("R4"));

    }
   
    SWtime0 = millis();
  }

}