#include <SoftwareSerial.h>

//The RSO variable controls RS485 transmit (RSO is short for RS485OUT)
#define RSOrx 9 // RO pin on Max485
#define RSOtx 11 // DI pin on Max485
#define RSOinout 10 // RE and DE pins on Max485
SoftwareSerial RS485OUT(RSOrx, RSOtx); //main variable for RS485 transmission e.g. RS485OUT.println()


//The RSI variable controls RS485 receive (RSI is short for RS485IN)
#define RSIrx 5 // RO pin on Max485
#define RSItx 7 // DI pin on Max485
#define RSIinout 6 // RE and DE pins on Max485
SoftwareSerial RS485IN(RSIrx, RSItx); //main variable for RS485 data reception e.g. if (RS485IN.available()),,,  RS485IN.readString(),,, RS485IN.readStringUntil('\n')

//Neutral RS485 variables that apply to RS485IN and RS485OUT
#define RS485Transmit HIGH
#define RS485Receive LOW
#define baudrate 9600 // Adjust to match the master's baud rate
#define syncRate 50 // Controls rate of void loop on Arduino, important for syncing RS485

//Arduino hard reset button variables
int resetButton = 18;
int resetState; //analog read of resetButton
int resetTrigger = 49; //wire connected to reset pin that can trigger it
int resetCount; //counts to 5 when button pressed
int prevresetState;



//Pump emergency on/off variables
int pumpSwitch = 19;
int pumpState;

//Analog joystick variables (track control)
int leftTrack = A15;
int lTrack;
int lTrackVal; // converts from lTrack and goes over RS485 for track control
int rightTrack = A14;
int rTrack;
int rTrackVal; // converts from rTrack and goes over RS485 for track control

//ROV light control pins and variables
int lightState;
int prevlightState;
int lightChannel; //Toggle value for controlling external ROV lights, to be sent over RS485
int ROVlightToggle = 51; //pin for left joystick button, controls light when pressed

//Debugging lights
int rxLED = 13; // yellow LED blinks when RS485OUT line is receiving data
int txLED = 12; // green LED blinks when RS485IN line is sending data
int lTrackLED = 4; // orange LED light for left ROV track control. Solid light when forward or backward
int rTrackLED = 3; // orange LED light for right ROC track control. Solid light when forward or backward
int pumpLED = 2; // red LED for ROV pump state. Solid light when pump on.
int ROVlightLED = 8; // blue LED indicator for external ROV lights. 

//Camera channel lights and relay
int cameraToggle = 53; //pin for right joystick button, controls camera channels
int camState;
int prevcamState;
int channelVal; //Channel value for camera relay, to be sent over RS485
int cam1 = 17; //front camera
int cam2 = 16; //back camera
int cam3 = 15;
int cam4 = 14;
int camRelay1 = 46;
int camRelay2 = 44;
int camRelay3 = 42;
int camRelay4 = 40;
int slideVoltage = A4; //represents camera line that swaps between cameras
int liveCamera;


void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);

//RS485OUT (send data) pin setup
pinMode(RSOinout, OUTPUT);
digitalWrite(RSOinout,RS485Transmit);
RS485OUT.begin(baudrate);

//RS485IN (receive data) pin setup
pinMode(RSIinout, OUTPUT);
digitalWrite(RSIinout,RS485Receive);
RS485IN.begin(baudrate);

//Pump control pin setup
pinMode(pumpSwitch,INPUT);

//Arduino reset button pin setup
pinMode(resetButton, INPUT);

//Joystick pin setup
pinMode(leftTrack, INPUT);
pinMode(rightTrack, INPUT);
pinMode(ROVlightToggle, INPUT_PULLUP);
pinMode(cameraToggle, INPUT_PULLUP);

//Debug lights pin setup
pinMode(rxLED, OUTPUT);
pinMode(txLED, OUTPUT);
pinMode(lTrackLED, OUTPUT);
pinMode(rTrackLED, OUTPUT);
pinMode(pumpLED, OUTPUT);
pinMode(ROVlightLED, OUTPUT);

//Camera relay
pinMode(camRelay1, OUTPUT);
pinMode(camRelay2, OUTPUT);
pinMode(camRelay3, OUTPUT);
pinMode(camRelay4, OUTPUT);

//Initialize camera channel relay
channelVal = 1;
prevcamState = 0;

//Initialize ROV external lights
lightChannel = 0;
prevlightState = 0;

//Reset button counter
resetCount = 0;
prevresetState = 0;

}

void loop() {
  // put your main code here, to run repeatedly:
//Variable setup
lTrack=analogRead(leftTrack);
rTrack=analogRead(rightTrack);
pumpState=digitalRead(pumpSwitch);
resetState=digitalRead(resetButton);
camState=digitalRead(cameraToggle);
lightState=digitalRead(ROVlightToggle);
liveCamera=analogRead(slideVoltage); // Which camera is live

//RS485OUT Data Transmission Control
RS485OUT.print(lTrackVal);
RS485OUT.print(",");
RS485OUT.print(rTrackVal);
RS485OUT.print(",");
RS485OUT.print(pumpState);
RS485OUT.print(",");
RS485OUT.print(lightChannel);
RS485OUT.print(",");
RS485OUT.print(channelVal);
RS485OUT.print(",");
RS485OUT.print(resetCount);
RS485OUT.print(",");
RS485OUT.println(liveCamera);


//RS485IN Data Receive Control
 if (RS485IN.available()) {
    String receivedData = RS485IN.readStringUntil('\n'); // Read until newline
    Serial.println(receivedData);

 }



//Camera channel switcher (four channels) and debug LEDs
if (camState == 0 && prevcamState == 0){ //toggle controls
    prevcamState = 1;
    channelVal = channelVal + 1;}
    
  if (camState == 1 && prevcamState == 1){
    prevcamState = 0;}
    
  if (channelVal == 5){
    channelVal = 1;}

    //Debug lights representing camera channels
    if (channelVal == 1){ //debug light for cam1
      digitalWrite(cam1, HIGH);
      digitalWrite(camRelay1, HIGH);
    } else {
      digitalWrite(cam1, LOW);
      digitalWrite(camRelay1, LOW);}

    if (channelVal == 2){ //debug light for cam2
      digitalWrite(cam2, HIGH);
      digitalWrite(camRelay2, HIGH);
    } else {
      digitalWrite(cam2, LOW);
      digitalWrite(camRelay2, LOW);}

    if (channelVal == 3){ //debug light for cam3
      digitalWrite(cam3, HIGH);
      digitalWrite(camRelay3, HIGH);
    } else {
      digitalWrite(cam3, LOW);
      digitalWrite(camRelay3, LOW);}

    if (channelVal == 4){ //debug light for cam4
      digitalWrite(cam4, HIGH);
      digitalWrite(camRelay4, HIGH);
    } else {
      digitalWrite(cam4, LOW);
      digitalWrite(camRelay4, LOW);}

//Toggle control for ROV external lights
if (lightState == 0 && prevlightState == 0){
  prevlightState = 1;
  lightChannel = lightChannel +1;}

  if (lightState == 1 && prevlightState == 1){
    prevlightState = 0;}

  if (lightChannel == 3){
    lightChannel = 0;
  }
    //Debug light control for ROV lights. PWM half brightness for ch 1, full brightness for ch 2
    if (lightChannel == 1){
      analogWrite(ROVlightLED, 5);
    } else if (lightChannel == 2){
      analogWrite(ROVlightLED, 255);
    } else {
      analogWrite(ROVlightLED, 0);}



//Track control converter from analog to control code. lTrackVal and rTrackVal are sent over RS485
// Back left = 5, Forward left = 7, Back right = 91, Forward right == 23
lTrackVal = 0;
rTrackVal = 0;
if (lTrack < 100){
    lTrackVal = 5;
    analogWrite(lTrackLED, 5);
  } else if (lTrack > 950){
    lTrackVal = 7;
    analogWrite(lTrackLED, 255);
  } else {
    lTrackVal =0;
    analogWrite(lTrackLED, 0);
  }
  if (rTrack < 100){
    rTrackVal = 91;
    analogWrite(rTrackLED, 5);
  } else if (rTrack > 950){
    rTrackVal = 23;
    analogWrite(rTrackLED, 255);
  } else {
    rTrackVal = 0;
    analogWrite(rTrackLED, 0);
  }


//Reset arduino button control. Hold button for ~5 seconds to reset arduino
if (resetState == 1 && prevresetState == 0 && resetCount <5){
  resetCount = resetCount + 1;
  delay(1000);}
  else if (resetCount ==5){ //when resetCount hits 37 the arduino will restart
    Serial.println("RESET COMPLETE");
    digitalWrite(lTrackLED, HIGH);
    digitalWrite(rTrackLED, HIGH);
    digitalWrite(cam1, HIGH);
    digitalWrite(cam2, HIGH);
    digitalWrite(cam3, HIGH);
    digitalWrite(cam4, HIGH);
    digitalWrite(rxLED, HIGH);
    digitalWrite(txLED, HIGH);
    digitalWrite(ROVlightLED, HIGH);
    digitalWrite(pumpLED, HIGH);
    delay(100);
    pinMode(resetTrigger, OUTPUT);
    digitalWrite(resetTrigger, HIGH);
  }
  if (resetState == 0 && resetCount >0){
    resetCount = 0;
  }


//Pump power switch debug light
if (pumpState == 1){
  digitalWrite(pumpLED, HIGH);
  } else {
    digitalWrite(pumpLED, LOW);
  }


//print values to serial monitor
Serial.print("LTrack: ");
Serial.print(lTrack);
Serial.print(", RTrack: ");
Serial.print(rTrack);
Serial.print(", Pump: ");
Serial.print(pumpState);
Serial.print(", ROV Lights Low/High: ");
Serial.print(lightChannel);
Serial.print(", Camera: ");
Serial.print(channelVal);
Serial.print(", Reset Counter: ");
Serial.print(resetCount);
Serial.print(", liveCamera: ");
Serial.println(liveCamera);

delay(syncRate);
}