//include Libraries
//16x2 LCD
#include <Arduino.h>
#include <LiquidCrystal_I2C.h> //SDA = B7[A4], SCL = B6[A5] STM32/[Arduino]
LiquidCrystal_I2C lcd(0x27, 16, 2);

int POT1 = A0;
int POT2 = A6;
int POT3 = A7;
const int pulse = 50;
const int pulsewidthOffset = 3;
const int pulsewidth = pulse - pulsewidthOffset;
//const int Delay1 = 850;
uint16_t DelayTrig1;
uint16_t DelayTrig2;
int DelayValue1;
int DelayValue2;
int DelayValueMap1;
int DelayValueMap2;

//Declare Analog Pin Names
#define LD1_TX_RS422_EN A1
#define LD2_TX_RS422_EN A2
#define LD3_TX_RS422_EN A3

//Declare Digital Pin Names
#define LD1_RX_RS422_IN 0
#define LD2_RX_RS422_IN 1
#define TRIG_IN 2
#define ENC_CLK 3
#define ENC_DATA 4
#define ENC_SWH 5
#define LD3_RX_RS422_IN 6
#define LD1_TRIG 7
#define LD2_TRIG 8
#define LD3_TRIG 9
#define AUTO_TRIG 10
#define LD1_RS422_TRIG 11
#define LD2_RS422_TRIG 12
#define LD3_RS422_TRIG 13


void setup ()
{
//Make Analog inputs A1, A2 and A3 Digital Outputs)
pinMode(LD1_TX_RS422_EN, OUTPUT);
pinMode(LD2_TX_RS422_EN, OUTPUT);
pinMode(LD3_TX_RS422_EN, OUTPUT);

//Declare Digital Pins to be Input or Output
pinMode(LD1_RX_RS422_IN, INPUT);
pinMode(LD2_RX_RS422_IN, INPUT);
pinMode(TRIG_IN, INPUT);
pinMode(ENC_CLK, INPUT);
pinMode(ENC_DATA, INPUT);
pinMode(ENC_SWH, INPUT);
pinMode(LD3_RX_RS422_IN, INPUT);
pinMode(LD1_TRIG, OUTPUT);
pinMode(LD2_TRIG, OUTPUT);
pinMode(LD3_TRIG, OUTPUT);
pinMode(AUTO_TRIG, OUTPUT);
pinMode(LD1_RS422_TRIG, OUTPUT);
pinMode(LD2_RS422_TRIG, OUTPUT);
pinMode(LD3_RS422_TRIG, OUTPUT);

//Enable pullups on digital inputs
pinMode(LD1_RX_RS422_IN, INPUT_PULLUP);
pinMode(LD2_RX_RS422_IN, INPUT_PULLUP);
pinMode(LD3_RX_RS422_IN, INPUT_PULLUP);
pinMode(TRIG_IN, INPUT_PULLUP);
pinMode(ENC_CLK, INPUT_PULLUP);
pinMode(ENC_DATA, INPUT_PULLUP);
pinMode(ENC_SWH, INPUT_PULLUP);

 attachInterrupt(digitalPinToInterrupt(TRIG_IN), Sequence, RISING);

//Serial.begin(9600);
//Initialise LCD
lcd.begin(16,2);
lcd.backlight();

}


void loop() {
  // put your main code here, to run repeatedly:
  //Test all output pins
/*
digitalWrite(LD1_TX_RS422_EN, HIGH);
digitalWrite(LD2_TX_RS422_EN, HIGH);
digitalWrite(LD3_TX_RS422_EN, HIGH);

digitalWrite(LD1_TRIG, HIGH);
digitalWrite(LD2_TRIG, HIGH);
digitalWrite(LD3_TRIG, HIGH);
digitalWrite(AUTO_TRIG, HIGH);
digitalWrite(LD1_RS422_TRIG, HIGH);
digitalWrite(LD2_RS422_TRIG, HIGH);
digitalWrite(LD3_RS422_TRIG, HIGH);
*/
//Read all Analog and Digital Pins 
int DT1=analogRead(POT1);
int DT2=analogRead(POT2);
int DT3=analogRead(POT3);

DelayValueMap1 = map(DT2, 0, 1023, 1000, 2000);
DelayValueMap2 = map(DT3, 0, 1023, 1000, 2000);
    
DelayTrig1=(DelayValueMap1-50);
DelayTrig2=(DelayValueMap2-50);

bool LD1_OUT=digitalRead(LD1_RX_RS422_IN);
bool LD2_OUT=digitalRead(LD2_RX_RS422_IN);
bool LD3_OUT=digitalRead(LD3_RX_RS422_IN);
bool EXT_TRIG=digitalRead(TRIG_IN);
bool ENC_PRESS=digitalRead(ENC_SWH);

//Write readings to LCD

lcd.clear();
lcd.setCursor(0,0);
lcd.print(DelayTrig1);
//lcd.setCursor(7,0);
//lcd.print(DT2);
lcd.setCursor(0,1);
lcd.print(DelayTrig2);

if (LD1_OUT == 0) {
    lcd.setCursor(7,1);
  lcd.print("LD1_RX");
}

if (LD2_OUT == 0) {
    lcd.setCursor(7,1);
  lcd.print("LD2_RX");
}

if (LD3_OUT == 0) {
    lcd.setCursor(7,1);
  lcd.print("LD3_RX");
}

if (EXT_TRIG == 0) {
    lcd.setCursor(7,1);
  lcd.print("TRIG_IN");
}

if (ENC_PRESS == 0) {
    lcd.setCursor(7,1);
  lcd.print("ENC_SWH");
}
/*
delay(1000);

//Turn all output pins OFF
digitalWrite(LD1_TX_RS422_EN, LOW);
digitalWrite(LD2_TX_RS422_EN, LOW);
digitalWrite(LD3_TX_RS422_EN, LOW);

digitalWrite(LD1_TRIG, LOW);
digitalWrite(LD2_TRIG, LOW);
digitalWrite(LD3_TRIG, LOW);
digitalWrite(AUTO_TRIG, LOW);
digitalWrite(LD1_RS422_TRIG, LOW);
digitalWrite(LD2_RS422_TRIG, LOW);
digitalWrite(LD3_RS422_TRIG, LOW);
//Wait 1 second before starting again
delay(1000);
*/
}

void Sequence() {
  //state = !state;
  noInterrupts();
  digitalWrite(LD1_TRIG, HIGH);
  //digitalWrite(ledPin, HIGH);
  //delayMicroseconds(pulsewidth);
  delay(pulsewidth);
  digitalWrite(LD1_TRIG, LOW);
  //digitalWrite(ledPin, LOW);
  //delayMicroseconds(DelayTrig1);
  delay(DelayTrig1);  
  digitalWrite(LD2_TRIG, HIGH);
  //digitalWrite(ledPin, HIGH);
  //delayMicroseconds(pulsewidth);
  delay(pulsewidth);
  digitalWrite(LD2_TRIG, LOW);
  //digitalWrite(ledPin, LOW);
  //delayMicroseconds(DelayTrig2);
  delay(DelayTrig2);
  digitalWrite(LD3_TRIG, HIGH);
  //digitalWrite(ledPin, HIGH);
  //delayMicroseconds(pulsewidth);
  delay(pulsewidth);
  digitalWrite(LD3_TRIG, LOW);
  //digitalWrite(ledPin, LOW);
  interrupts(); 
}