#include <Arduino.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
const byte directionPin = 33; //rotor direction pin
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 = 19; //button R middle+
const byte BR2 = 34; //button R middle-
const byte BR3 = 23; //button R thumb+
const byte BR4 = 5; //button R thumb-
const unsigned int strsize = 256; //max string buffer size

const bool btab1[32] = {1,1,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,0,0,1,1,0,1,1,1,0,0,0,0,0,0}; //table for braille position 1
const bool btab2[32] = {0,1,0,0,0,1,1,1,1,1,0,1,0,0,0,1,1,1,1,1,0,1,1,0,0,0,0,0,0,0,0,0}; //table for braille position 2
const bool btab3[32] = {0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,0,0,0,0,0,0}; //table for braille position 3
const bool btab4[32] = {0,0,1,1,0,1,1,0,1,1,0,0,1,1,0,1,1,0,1,1,0,0,1,1,1,0,0,0,0,0,0,0}; //table for braille position 4
const bool btab5[32] = {0,0,0,1,1,0,1,1,0,1,0,0,0,1,1,0,1,1,0,1,0,0,1,0,1,1,0,0,0,0,0,0}; //table for braille position 5
const bool btab6[32] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0}; //table for braille position 6

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 = 20; //sensor debounce time (ms)
unsigned long BTdelay0 = 20; //BT debounce time (ms)
unsigned long SWdelay0 = 300; //Button debounce time (ms)
unsigned long trig0 = 0; //Interrupt response timer (ms)

unsigned int ringstep = 0; //character index counter
unsigned int brstr = 5; //initial string length
byte i = 0; //conversion table index
bool sensor1 = 0;
bool sensor2 = 0;
bool sensor1p = 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 bout1[strsize] = {1,1,1,1,1}; //braille buffer output position 1
bool bout2[strsize] = {1,1,1,1,1}; //braille buffer output position 2
bool bout3[strsize] = {1,1,1,1,1}; //braille buffer output position 3
bool bout4[strsize] = {0,0,0,0,0}; //braille buffer output position 4
bool bout5[strsize] = {0,0,0,0,0}; //braille buffer output position 5
bool bout6[strsize] = {0,0,0,0,0}; //braille buffer output position 6
String BTstring = ""; //bluetooth input
char instr1[strsize] = {""}; //Lowercased input string


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);
  digitalWrite(act1,0);
  digitalWrite(act2,0);
  digitalWrite(act3,0);
  digitalWrite(brake,0);
  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);
 
}
 
void loop() {
 
  xSemaphoreTake(syncSemaphore, portMAX_DELAY);
  trig0 = millis();
//Respond to Bluetooth events before actuator control
  if(millis() - BTtime0 > BTdelay0){
    if (Serial.available()) {
      SerialBT.write(Serial.read());
    }
    if (SerialBT.available()) {
      BTstring = SerialBT.readStringUntil('\n');
      BTstring.trim();
      SerialBT.print("Received string ");
      SerialBT.println(BTstring);
      if(BTstring == "sa0"){
        
      } else{
        brstr = min(BTstring.length(), strsize); //Limit string length to max allocated memory
        BTstring.toLowerCase(); //loop and store indexed braille code in buffer
        BTstring.toCharArray(instr1, strsize);
        for (i = 0; i < brstr-1; i++){
          if (instr1[i] > 96) {
            bout1[i] = btab1[instr1[i]-97];
            bout2[i] = btab2[instr1[i]-97];
            bout3[i] = btab3[instr1[i]-97];
            bout4[i] = btab4[instr1[i]-97];
            bout5[i] = btab5[instr1[i]-97];
            bout6[i] = btab6[instr1[i]-97];
          } else{
            bout1[i] = 0;
            bout2[i] = 0;
            bout3[i] = 0;
            bout4[i] = 0;
            bout5[i] = 0;
            bout6[i] = 0;
          }
        }
        //Report conversion time on smartphone
        SerialBT.print(F("Braille converted length = "));
        SerialBT.print(brstr);
        SerialBT.print(F(" in "));
        SerialBT.print(millis()-trig0);
        SerialBT.println(F(" ms"));
        ringstep = 0;
      }
    }
    BTtime0 = millis();
  }
//Braille actuator control
  if(millis() - lastDetection > debounceTime){
    sensor1 = digitalRead(interruptPin);
    sensor2 = digitalRead(directionPin);
    //Rotation trigger
    if(sensor1 != sensor1p){
      if(sensor1 == sensor2){
        if(sensor1 == 1){
          digitalWrite(act1,bout1[ringstep]);
          digitalWrite(act2,bout2[ringstep]);
          digitalWrite(act3,bout3[ringstep]);
          ringstep += 1;
          if (ringstep > brstr){
            ringstep = 0;
          }
          SerialBT.printf("Right %u \n", ringstep);    
        } else{
          digitalWrite(act1,bout4[ringstep]);
          digitalWrite(act2,bout5[ringstep]);
          digitalWrite(act3,bout6[ringstep]);
        }
      } else{
        digitalWrite(act1,0);
        digitalWrite(act2,0);
        digitalWrite(act3,0);
        SerialBT.printf("Left %u \n", ringstep);      
      }
    }
    sensor1p = sensor1;
    lastDetection = millis();
  }
  
//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();
  }


  
}