#include <Wire.h>

const uint8_t I2C_ADDRESS = 0x58;
String inputBuffer = "";

void setupClock8MHz() {
  pinMode(9, OUTPUT);
  // Stop Timer1
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  // Toggle OC1A on compare match
  TCCR1A = _BV(COM1A0);
  // Set timer mode to CTC (Clear Timer on Compare Match) with OCR1A as TOP
  TCCR1B = _BV(WGM12);
  OCR1A = 0;  // Compare value
  // No prescaling -> timer counts at 16 MHz CPU clock
  TCCR1B |= _BV(CS10);
}

void setup() {
  Wire.begin();
  Serial.begin(9600);
  while (!Serial);
  Serial.println("Serial-to-I2C bridge. Commands:");
  Serial.println("W 0x10 0x1234");
  Serial.println("R 0x10");
  Serial.println(" ");
  setupClock8MHz();
}

void loop() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n') {
      processCommand(inputBuffer);
      inputBuffer = "";
    } else if (c != '\r') {
      inputBuffer += c;
    }
  }
}

void processCommand(String cmd) {
  cmd.trim();
  Serial.println(cmd);
  if (cmd.length() < 3) {
    Serial.println("Invalid command.");
    return;
  }

  char commandType = toupper(cmd.charAt(0));
  cmd = cmd.substring(1);
  cmd.trim();

  if (commandType == 'W') {
    // Parse: W <reg> <value>
    int spaceIndex = cmd.indexOf(' ');
    if (spaceIndex == -1) {
      Serial.println("Invalid write syntax. Use: W <reg> <value>");
      return;
    }

    String regStr = cmd.substring(0, spaceIndex);
    String valStr = cmd.substring(spaceIndex + 1);

    uint8_t reg = (uint8_t) strtol(regStr.c_str(), NULL, 0);
    uint16_t val = (uint16_t) strtol(valStr.c_str(), NULL, 0);

    uint8_t lsb = val & 0xFF;
    uint8_t msb = (val >> 8) & 0xFF;

    Wire.beginTransmission(I2C_ADDRESS);
    Wire.write(reg);
    Wire.write(lsb);  // Send LSB first
    Wire.write(msb);  // Then MSB
    byte result = Wire.endTransmission();

    if (result == 0) {
      Serial.println("Write OK");
    } else {
      Serial.print("Write error: ");
      Serial.println(result);
    }

  } else if (commandType == 'R') {
    // Parse: R <reg>
    uint8_t reg = (uint8_t) strtol(cmd.c_str(), NULL, 0);

    // Send register address with repeated start
    Wire.beginTransmission(I2C_ADDRESS);
    Wire.write(reg);
    byte result = Wire.endTransmission(false);  // Repeated start

    if (result != 0) {
      Serial.print("Read error: ");
      Serial.println(result);
      return;
    }

    // Request 2 bytes
    Wire.requestFrom(I2C_ADDRESS, (uint8_t)2);
    if (Wire.available() >= 2) {
      uint8_t lsb = Wire.read();  // LSB first
      uint8_t msb = Wire.read();
      uint16_t val = ((uint16_t)msb << 8) | lsb;

      Serial.print("VAL: 0x");
      Serial.println(val, HEX);
    } else {
      Serial.println("Read failed.");
    }
  } else {
    Serial.println("Unknown command. Use 'R' or 'W'.");
  }
}