#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED setup
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// RS485 control pins
#define DE_PIN 20
#define RE_PIN 10

#define THIS_ID        200
#define PING           0x01 // QUERY THE WORKING STATUS
#define READ_DATA      0x02 // READ DATA
#define WRITE_DATA     0x03 // WRITE DATA
#define REGWRITE_DATA  0x04 // QUEUES MOVES FOR ACTION COMMAND
#define ACTION         0x05 // TRIGGERS REG WRITE WRITES
#define SYNCWRITE_DATA 0x83 // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
#define RESET          0x06 // RESET TO FACTORY DEFAULT

void setup() {
  // Initialize Serial1 for RS485 (TX=21, RX=9)
  Serial.begin(115200);
  Serial1.begin(1000000, SERIAL_8N1, 9, 21);

  // Set DE and RE as outputs
  pinMode(DE_PIN, OUTPUT);
  pinMode(RE_PIN, OUTPUT);

  SetModeReceive();

  // Initialize OLED
  Wire.begin(3, 4);  // SDA=3, SCL=4
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    while (true)
      ;  // Halt if display fails
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Waiting for RS485...");
  display.display();
}

void loop() {
  if (Serial1.available()) {
    uint8_t buffer[32];
    int count = 0;

    // Read all available bytes
    while (Serial1.available() && count < sizeof(buffer)) {
      buffer[count++] = Serial1.read();
    }

    // Display on Serial
    Serial.print("recv: ");
    for (int i = 0; i < count; i++) {
      Serial.print("0x");
      if (buffer[i] < 0x10) Serial.print("0");
      Serial.print(buffer[i], HEX);
      Serial.print(" ");
    }
    Serial.println();

    // Display on OLED
    display.clearDisplay();
    display.setCursor(0, 0);
    display.setTextSize(1);
    display.println("Received:");
    display.setCursor(0, 12);
    display.setTextSize(1);
    if (buffer[4] == PING) {
      if (buffer[2] == THIS_ID){
        sendPingResponse();
        Serial.println("ME!");
        display.print("!");
      }
      Serial.println("PING");

      display.print("PING");
      display.print("  ");
      display.println(buffer[2]);
    } else if (buffer[4] == WRITE_DATA) {
      Serial.println("WRITE_DATA");
      uint16_t position = (buffer[7] << 8) | buffer[6];
      display.print("WRITE_DATA");
      display.print("  ");
      display.print(buffer[2]);
      display.print("  ");
      display.println(position);
    }

    for (int i = 0; i < count; i++) {
      display.print("0x");
      if (buffer[i] < 0x10) display.print("0");
      display.print(buffer[i], HEX);
      display.print(" ");
    }
    

    
    display.display();
  }
}


void SetModeTransmit() {
  digitalWrite(DE_PIN, HIGH); 
  digitalWrite(RE_PIN, HIGH);
  delay(10);
}

void SetModeReceive() {
  digitalWrite(DE_PIN, LOW);   
  digitalWrite(RE_PIN, LOW);
  delay(10);
}

// Send a ping command to a specific servo
void sendPingResponse() {
    uint8_t packet[6];

    packet[0] = 0xFF; // Header
    packet[1] = 0xFF; // Header
    packet[2] = THIS_ID;   // Servo ID
    packet[3] = 0x03; // Length = instruction + address + checksum
    packet[4] = 0x00; // NO ERROR

    // Calculate checksum
    uint8_t sum = 0;
    for (int i = 2; i < 5; i++) sum += packet[i];
    packet[5] = ~sum; // Checksum

    // Send packet
    SetModeTransmit();
    Serial1.write(packet, sizeof(packet));
    Serial1.flush();
    SetModeReceive();

    // if (PRINT_SENT_PACKETS) {
    //     Serial.print("Sent ping packet: ");
    //     for (int i = 0; i < 6; i++) {
    //         Serial.print("0x");
    //         if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
    //         Serial.print(packet[i], HEX);
    //         Serial.print(" ");
    //     }
    //     Serial.println();
    // }
}