#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] = {0, 0, -23, -23};
int8_t car_retreat[4] = {0, 0, 23, 23};
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() {
  // Use specific SDA and SCL pins for ESP32
  Wire.begin(2, 15, 100000); // SDA, SCL, Frequency

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

  Serial.begin(115200); // Initialize serial communication
}

void loop() {
  carControl();
  while (1);
}

/*
 * Control Motor Through IIC Communication
 */

bool WireWriteDataArray(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;
}

/*
 * Vehicle movement instructions
 */

void carControl() {
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_forward, 4);
  delay(2000);
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_stop, 4);
  delay(1000);

  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_retreat, 4);
  delay(2000);
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_stop, 4);
  delay(1000);

  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_turnLeft, 4);
  delay(600);
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_stop, 4);
  delay(1000);

  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_turnRight, 4);
  delay(600);
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)car_stop, 4);
  delay(1000);
}