#include <Wire.h>

/*
 * Communication properties
 */

#define I2C_ADDR 0x34

#define ADC_BAT_ADDR                  0
#define MOTOR_TYPE_ADDR               20
#define MOTOR_ENCODER_POLARITY_ADDR   21
#define MOTOR_FIXED_PWM_ADDR          31
#define MOTOR_FIXED_SPEED_ADDR        51
#define MOTOR_ENCODER_TOTAL_ADDR      60

/*
 * Initialize Motor Type
 */

#define MOTOR_TYPE_WITHOUT_ENCODER       0
#define MOTOR_TYPE_TT                    1
#define MOTOR_TYPE_N20                   2
#define MOTOR_TYPE_JGB37_520_12V_110RPM  3

/*
 *  Motor properties
 */

uint8_t MotorType = MOTOR_TYPE_JGB37_520_12V_110RPM;
uint8_t MotorEncoderPolarity = 0;

int8_t car_forward[4] = {-23, -23, 0, 0};
int8_t car_retreat[4] = {23, 23, 0, 0};
int8_t car_turnLeft[4] = {20, -20, 0, 0};
int8_t car_turnRight[4] = {-20, 20, 0, 0};
int8_t car_stop[4] = {0, 0, 0, 0};

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // Wait for serial initialization
  }

  // Use specific SDA and SCL pins for ESP32
  Wire.begin(21, 22, 100000); // SDA, SCL, Frequency

  delay(200);
  writeMotorData(MOTOR_TYPE_ADDR, &MotorType, 1);
  delay(5);
  writeMotorData(MOTOR_ENCODER_POLARITY_ADDR, &MotorEncoderPolarity, 1);

  Serial.println("Vehicle Control Initialized.");
}

void loop() {
  displayMenu();
  handleUserInput();
}

/*
 * Function to write data to I2C device
 */
bool writeMotorData(uint8_t reg, uint8_t *val, unsigned int len) {
  Wire.beginTransmission(I2C_ADDR);
  Wire.write(reg);
  for (unsigned int i = 0; i < len; i++) {
    Wire.write(val[i]);
  }
  if (Wire.endTransmission() != 0) {
    return false;
  }
  return true;
}

/*
 * Display the interactive menu
 */
void displayMenu() {
  Serial.println("Choose an option:");
  Serial.println("1. Move Forward");
  Serial.println("2. Move Backward");
  Serial.println("3. Turn Left");
  Serial.println("4. Turn Right");
  Serial.println("5. Stop");
  Serial.println("Enter your choice: ");
}

/*
 * Handle user input and control the vehicle accordingly
 */
void handleUserInput() {
  while (!Serial.available()) {
    // Wait for user input
  }

  String input = Serial.readStringUntil('\n');
  int choice = input.toInt();

  switch (choice) {
    case 1:
      moveForward();
      break;
    case 2:
      moveBackward();
      break;
    case 3:
      turnLeft();
      break;
    case 4:
      turnRight();
      break;
    case 5:
      stop();
      break;
    default:
      Serial.println("Invalid choice. Please select again.");
      break;
  }
}

/*
 * Functions to control the vehicle movements
 */
void moveForward() {
  writeMotorData(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_forward, 4);
  Serial.println("Moving Forward");
}

void moveBackward() {
  writeMotorData(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_retreat, 4);
  Serial.println("Moving Backward");
}

void turnLeft() {
  writeMotorData(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_turnLeft, 4);
  Serial.println("Turning Left");
}

void turnRight() {
  writeMotorData(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_turnRight, 4);
  Serial.println("Turning Right");
}

void stop() {
  writeMotorData(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_stop, 4);
  Serial.println("Stopping");
}