/***************************************************
  ESP32 talking to a PTZ camera with PelcoD protocol over RS485
  Written by Jumpjack
  v. 1.0.0 - 12/MAR/2024

 Required libraries:
 - Pelco_And_Arduino 
 - EspSoftwareSerial
 - ModbusMaster
 ****************************************************/

#include <Pelco_And_Arduino.h>


PelcoBus MyPelcoBus(32,  // RX pin (Cam to Arduino, RO pin - Receiver Output)
                    33,  // TX pin (Arduino to Cam, DI pin - Driver Input)
                    -1); // RE pin - Receiver Enable (?)
// RE pin number specified => not automatic, HIGH =  module in RX mode, LOW = module in TX mode
// RE pin = -1 => module automatically switches TX/RX
// ref: https://github.com/Pixelbo/Pelco_And_Arduino/blob/5c4c4367be48858c0e1fbb1efee25626cd4064a0/src/PelcoBus.cpp#L80

PelcoCam Camera1(&MyPelcoBus, //The pointer to the bus
                 1,           //Address of the camera
                 true);      // DisableAck: true = don't wait response from camera


constexpr int BOARD_485_RX = 32;  // yellow wire to RO (Receiver Output)
constexpr int BOARD_485_TX = 33;  // brown wire to DI (Driver Input)
constexpr int MAX485_DE = 23;  // blue wire to RE (Receiver Enable) 
constexpr int MAX485_RE = 23;  // and can be tied to DE (Driver Enable)
constexpr int MODBUS_BAUD_RATE = 9600;

constexpr int BUTTON1 = 4;
constexpr int BUTTON2 = 2;


const String firmware_version = "v1.0.0";

long lastDebounceTime = 0;  // the last time the output pin was toggled
long debounceDelay = 50;    // the debounce time; increase if the output flickers
int buttonState1 = HIGH; 
int buttonState2 = HIGH; 

#define Serial485 Serial1


void setup(void) {
  Serial.begin(115200);
Serial.println(__TIME__);  
  Serial.println(F("Starting skecth..."));
  pinMode(BUTTON1, INPUT_PULLUP);
  pinMode(BUTTON2, INPUT_PULLUP);

  Serial.print("Init 485...");      
  Serial485.begin(MODBUS_BAUD_RATE, SERIAL_8N1, BOARD_485_RX, BOARD_485_TX);
  Serial.println("Done.");


  Serial.print("Init pelco...");
  MyPelcoBus.begin(PELCO_D9600, true);
  Serial.println("Done.");

}

void loop() {
  buttonState1 = digitalRead(BUTTON1);
  buttonState2 = digitalRead(BUTTON2);

  if ( (millis() - lastDebounceTime) > debounceDelay) {  

    if  (buttonState1 == LOW)  { // Raw bytes sent to MAX485
  
      Serial.print(F("Sending raw 485 commands:\n")); 
     
      auto modbus_read_request = {
          0xFF,    0x01, 0x00, 0x4b,   0x01, 0x20,    0x00, 0x00
        };

      for (int b : modbus_read_request) {
        Serial485.write(b);
      }

      Serial.println(F("Sent 485 raw."));  
      delay(100);

      if (Serial485.available()) {
        Serial.println("Incoming MAX485 data:");
        while (Serial485.available()) {
          byte b = Serial485.read();
          Serial.printf("0x%02X, ", b);
        }
        Serial.println("=end=");
      }

      lastDebounceTime = millis(); //set the current time
    }



    if  (buttonState2 == LOW)  { // Pelco-D commands sent to MAX485

      Serial.print(F("Sending test PELCO commands:\n")); 
      
      Serial.println("\nSending SET_PAN...");
      Camera1.send_command(SET_PAN, 350);

      Serial.println("\nSending SET_TILT...");
      Camera1.send_command(SET_TILT, 2920);

      Serial.println("\nSending raw command...");
      MyPelcoBus.send_raw("FF 01 43 49 41 4f");

      Serial.println("Done sending Pelco-D commands");

      lastDebounceTime = millis(); //set the current time
    }



  }

}