#include <Wire.h>
#include <math.h>
#include <Arduino.h>

// ====================== Pin Configuration ======================

// I2C Pins (connected from ESP32 to MP6546)
#define SDA_PIN 21
#define SCL_PIN 22

// MP6546 Address pool (for multi-slave mode, max 3 devices on one bus)
uint8_t MP6546_ADDRS[] = {0x07, 0x08, 0x09};
uint8_t detectedDevices[3];  // Detected MP6546 addresses
uint8_t numDevices = 0;      // Number of detected drivers

// Enable Pin (controls the EN pin of MP6546)
#define EN_PIN 4

// ====================== Register Map ======================

#define REG_CONFIG 0x00   // Configuration register
#define REG_BIAS 0x01     // ADC Bias register for HA/HB SIN/COS inputs
#define REG_UD 0x08       // d-axis voltage reference
#define REG_UQ 0x09       // q-axis voltage reference
#define REG_ANGLE 0x0C    // Rotor angle register (read only)
#define REG_FAULT 0x0D    // Fault status register

// ====================== System Constants ======================

#define VIN 8.0  // Voltage on the motor driver supply (change based on your setup)
void scanI2CDevices();
void setConfiguration(uint8_t addr);
void setBias(uint8_t addr, uint16_t v_offset);
void setUD_UQ(uint8_t addr, float ud, float uq);
uint16_t readAngle(uint8_t addr);
void checkFaults(uint8_t addr);
void rotateMotor(float speed);
uint16_t readRegister(uint8_t addr, uint8_t reg);
void writeRegister(uint8_t addr, uint8_t reg, uint16_t value);
void setThetaBias(uint8_t addr, uint8_t pole_pairs);
void setAlignMod(uint8_t addr);

// ====================== Setup ======================

void setup() {
  // Configure I2C pins with internal pull-ups
  pinMode(SDA_PIN, INPUT_PULLUP);
  pinMode(SCL_PIN, INPUT_PULLUP);

  // Enable MP6546 chip by setting EN pin HIGH
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);
  delay(10);  // Hold low for stability
  digitalWrite(EN_PIN, HIGH);
  delayMicroseconds(150);  // MP6546 samples ADDR pin after ~100us

  // Initialize I2C bus
  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(1000000);  // Set I2C clock to 1MHz (Fast-mode plus)

  Serial.begin(115200);
  Serial.println("MP6546 Linear Hall Mode Setup");

  // Scan for MP6546 devices on I2C bus
  scanI2CDevices();

  // Configure each detected MP6546 driver
  for (uint8_t i = 0; i < numDevices; i++) {
    setConfiguration(detectedDevices[i]); // Set driver mode and protection settings
    setAlignMod(detectedDevices[i]);
    setThetaBias(detectedDevices[i], 7); // Example pole pair count (change based on your motor)
    setBias(detectedDevices[i], 1650);    // Set SIN/COS bias voltage offset (for linear Hall sensors)
  }
  delay(100);
}

// ====================== Loop ======================

void loop() {
  // Apply a rotating field (FOC) with increasing UQ current
  rotateMotor(20000);  // The "20000" here is a speed-like factor for the sine wave generation

  // Read the rotor angle for each detected motor driver
  for (uint8_t i = 0; i < numDevices; i++) {
    uint16_t angle_raw = readAngle(detectedDevices[i]);
    float angle_deg = angle_raw * 360.0 / 32767.0; // Convert raw angle to degrees (15-bit resolution)

    Serial.print("Angle from Address 0x");
    Serial.print(detectedDevices[i], HEX);
    Serial.print(": ");
    Serial.print(angle_deg);
    Serial.println(" deg");

    // Check for any fault flags
    checkFaults(detectedDevices[i]);
  }
  delay(500);
}

// ====================== I2C Device Scanner ======================

void scanI2CDevices() {
  Serial.println("\nScanning I2C Bus...");

  byte error, address;
  numDevices = 0;

  // Check predefined MP6546 addresses (0x07, 0x08, 0x09)
  for (uint8_t i = 0; i < 3; i++) {
    address = MP6546_ADDRS[i];
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0) {
      Serial.print("MP6546 found at address 0x");
      Serial.println(address, HEX);
      detectedDevices[numDevices] = address;
      numDevices++;
    }
  }

  if (numDevices == 0) {
    Serial.println("No MP6546 devices found\n");
  } else {
    Serial.println("I2C scan done\n");
  }
  delay(1000);
}

// ALIGN_MOD: Bit 10 = 0 (external sensor mode)
void setAlignMod(uint8_t addr) {
  uint16_t value = 0;
  // ALN_MD = 0 --> external sensor angle source
  value &= ~(1 << 10);
  writeRegister(addr, 0x0A, value);
  Serial.println("ALIGN_MOD register set to external sensor mode");
}
// Set pole pairs in THETA_BIAS register
void setThetaBias(uint8_t addr, uint8_t pole_pairs) {
  uint16_t value = (pole_pairs & 0x0F); // Lower 4 bits = pole pairs
  writeRegister(addr, 0x02, value);
  Serial.print("Pole pairs set to: ");
  Serial.println(pole_pairs);
}


// ====================== Register Access ======================

// Writes a 16-bit value to the specified register on the MP6546
void writeRegister(uint8_t addr, uint8_t reg, uint16_t value) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(value >> 8);   // MSB
  Wire.write(value & 0xFF); // LSB
  Wire.endTransmission();
}

// Reads a 16-bit value from the specified register on the MP6546
uint16_t readRegister(uint8_t addr, uint8_t reg) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(addr, (uint8_t)2);
  uint16_t value = 0;
  if (Wire.available() == 2) {
    value = Wire.read() << 8;
    value |= Wire.read();
  }
  return value;
}

// ====================== MP6546 Configuration ======================

// Set up the driver for:
// - Linear Hall mode (HA/HB)
// - FOC (Vector Control)
// - Basic protections (OCP, SCP)
void setConfiguration(uint8_t addr) {
  uint16_t config = 0;
  config |= (1 << 15); // Enable Over-Current Protection (OCP_EN)
  config |= (1 << 14); // Enable Short-Circuit Protection (SCP_EN)
  config |= (1 << 13); // Enable Standby functionality (STDBY_EN)
  config |= (1 << 9);  // AMOD = 1 -> Linear Hall Mode (use HA/HB inputs)
  config |= (0 << 8);  // THETA_DIR = 0 (normal angle direction)
  config |= (0b01 << 6); // FLT_N_SVM = 600Hz filter for internal FOC
  config |= (0 << 4); // DRV_MOD = 0 (Vector Control mode enabled)
  config |= (0 << 3); // SPI_3 = 0 (not used for linear mode)
  config |= (0b001);  // FLT_N = 600Hz filter on reported angle
  writeRegister(addr, REG_CONFIG, config);

  Serial.print("Configured in Linear Hall mode @ Address: 0x");
  Serial.println(addr, HEX);
}

// Sets the VA_BIAS and VB_BIAS registers
// This calibrates the ADC for SIN/COS signal offset
void setBias(uint8_t addr, uint16_t v_offset) {
  uint8_t bias = (v_offset - 500) / 6.445; // Formula from datasheet
  uint16_t bias_reg = (bias << 8) | bias;  // Apply same bias to VA_BIAS and VB_BIAS
  writeRegister(addr, REG_BIAS, bias_reg);
  
  Serial.print("Bias set to: ");
  Serial.print(bias, HEX);
  Serial.print(" for Address: 0x");
  Serial.println(addr, HEX);
}

// ====================== Motor Commutation ======================

// Sends d/q axis voltage commands (FOC inputs)
// Only UQ is being modulated here with a sine wave
void setUD_UQ(uint8_t addr, float ud, float uq) {
  uint16_t UD, UQ;
  float constant = 512 * sqrt(3) / VIN; // Scaling factor for MP6546 FOC engine

  // Map ud/uq to 10-bit range [-512, 511]
  UD = (ud >= 0) ? constant * ud : constant * ud + 1024;
  UQ = (uq >= 0) ? constant * uq : constant * uq + 1024;

  UD &= 0x1FF;
  UQ &= 0x1FF;

  writeRegister(addr, REG_UD, UD);
  writeRegister(addr, REG_UQ, UQ);

  Serial.print("UD: ");
  Serial.print(UD);
  Serial.print(" UQ: ");
  Serial.println(UQ);
}

// Creates a rotating vector in the d/q frame by modulating UQ with a sine wave
void rotateMotor(float speed) {
  static float angle = 0;
  float ud = 0; // d-axis is set to 0
  float uq = speed * sin(angle); // q-axis varies sinusoidally

  angle += 0.1;
  if (angle > 2 * PI) {
    angle = 0;
  }

  for (uint8_t i = 0; i < numDevices; i++) {
    setUD_UQ(detectedDevices[i], ud, uq);
  }
}

// ====================== Monitoring & Fault Handling ======================

// Reads the angle register from MP6546 (15-bit value)
uint16_t readAngle(uint8_t addr) {
  uint16_t angle = readRegister(addr, REG_ANGLE);
  uint8_t fault = (angle >> 15) & 0x01;
  angle &= 0x7FFF; // Mask out fault flag (bit 15)

  if (fault) {
    Serial.print("Fault Detected on Address: 0x");
    Serial.println(addr, HEX);
  }
  return angle;
}

// Reads the fault register and reports protection events
void checkFaults(uint8_t addr) {
  uint16_t fault = readRegister(addr, REG_FAULT);

  if (fault & (1 << 11)) Serial.println("Short-Circuit Protection Triggered");
  if (fault & (1 << 10)) Serial.println("Thermal Shutdown Triggered");
  if (fault & (1 << 9)) Serial.println("Over-Voltage Protection Triggered");
  if (fault & (1 << 8)) Serial.println("Over-Current Protection Triggered");
}