#include <Arduino.h>
#include <esp32/hwtimer.h>
#include <esp32/soc.h>  // Include this line

#define TIMER_NUM 1  // Using Timer 1
#define TIMER_SOURCE_CLK TIMER_CLK_SRC_APB  // Use APB clock source
#define TIMER_DIVIDER 1  // No divider for 1 microsecond resolution

hw_timer_t *timer = nullptr; // Timer handle


bool outputStates[7][6] = {
  {0, 0, 0, 0, 0, 0}, //Dead sector
  {0, 0, 1, 0, 0, 1}, //Sector 1
  {0, 1, 1, 0, 0, 0}, //Sector 2
  {0, 1, 0, 0, 1, 0}, //Sector 3
  {0, 0, 0, 1, 1, 0}, //Sector 4
  {1, 0, 0, 1, 0, 0}, //Sector 5
  {1, 0, 0, 0, 0, 1}  //Sector 6
};

int inverterPin[6] = {
  4,  //A
  16, //A'
  17, //B
  5,  //B'
  18, //C
  19  //C'
};

int deadTime = 10;
int targetSpeed = 0;
int targetPower = 0;

int cycleInterval1 = 10;
int cycleInterval1Last = 0;

int currentSector = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  for (int i = 0; i < 6; i++) {
    pinMode(inverterPin[i], OUTPUT);
  };
  pinMode(12, INPUT);
  pinMode(14, INPUT);

  // Configure Timer 1 for CTC mode (similar to previous code)
  timer = timerBegin(TIMER_NUM, TIMER_SOURCE_CLK, TIMER_DIVIDER);
  timerAttachInterrupt(timer, &timer1CompareA_interrupt, true);
  timerAlarmWrite(timer, 1000, true); // Set compare value for 1 microsecond delay
  timerAlarmEnable(timer);

}

void loop() {
  // put your main code here, to run repeatedly:

  // 10ms interval code
  if (millis() - cycleInterval1Last > cycleInterval1) {

    map(targetSpeed, 0, 1000, analogRead(12), 4095);
    map(targetPower, 0, 100, analogRead(14), 4095);

    cycleInterval1Last = millis();
  }

  delay(1); // this speeds up the simulation
}

void setInverter(int sector) {
  for (int i = 0; i < 6; i++) {
    digitalWrite(inverterPin[i], outputStates[sector][i]);
  };
}


void timer1CompareA_interrupt() {
  // Update global variable
  setInverter(0);
  delayMicroseconds(1);
  setInverter(currentSector);
  currentSector++;
  timerAlarmWrite(timer, 1000, true); // Reload compare value for next delay
}