/*------------------------------------------------------------------------*/  
#include <Arduino.h>  
#include <driver/mcpwm.h>

const int LO1 = 12;                        // PWM output pin Low Side
const int HO1 = 13;                        // PWM output pin High Side
const int LO2 = 27;                        // PWM output pin Low Side
const int HO2 = 14;                        // PWM output pin High Side
const int LO3 = 25;                        // PWM output pin Low Side
const int HO3 = 26;                        // PWM output pin High Side

const float freqCarr = 20000.0;            // Carrier freq. Hz
const int tmrRegVal = int(((((1/freqCarr)/2) / 62.5E-9)-1)+0.5);   // timer period calc

float freqMod = 50.0;                      // Modulation freq. Hz
int amplitude = int(0.9 * tmrRegVal);
int sampleNum = int(freqCarr/freqMod);     // samples per cycle sine wave
int i;                                     // PWM index number
float radVal = 2 * PI / sampleNum;
const int offset120 = 1/3.0 * sampleNum;
const int offset240 = 2/3.0 * sampleNum;

/*-------------------------- Define the ISR function ---------------------*/
void IRAM_ATTR MCPWM_ISR(void*) { 
  float sineValue_0, sineValue_120, sineValue_240;
  int sineVal_0, sineVal_120, sineVal_240;

  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x011C, BIT(3));            // INT_CLR_PWM_REG // clear interrupt  
   
  sineValue_0 =  amplitude * sin (radVal * i);
  sineVal_0 = int (sineValue_0);  

  sineValue_120 =  amplitude * sin (radVal * (i + offset120));
  sineVal_120 = int (sineValue_120);  

  sineValue_240 =  amplitude * sin (radVal * (i + offset240));
  sineVal_240 = int (sineValue_240);       

  if (sineVal_0 == 0)   WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0040, 0);    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0044, 0);  
  if (sineVal_0 > 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0040, sineVal_0);         
  if (sineVal_0 < 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0044, abs(sineVal_0));         

  if (sineVal_120 == 0)   WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0078, 0);    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x007C, 0);           
  if (sineVal_120 > 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0078, sineVal_120);         
  if (sineVal_120 < 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x007C, abs(sineVal_120));           

  if (sineVal_240 == 0)   WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x00B0, 0);   WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x00B4, 0);           
  if (sineVal_240 > 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x00B0, sineVal_240);         
  if (sineVal_240 < 0)    WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x00B4, abs(sineVal_240));           
         
  i++;
  if (i > sampleNum ) i=0;                      // reset i if > 1 cycle    
}

/*------------------------------------------------------------------------*/
void setup() {
  pinMode(23, OUTPUT);
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, LO1);   // Use GPIO 12 for MCPWM0A
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, HO1);   // Use GPIO 13 for MCPWM0B
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, LO2);   // Use GPIO 12 for MCPWM0A
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, HO2);   // Use GPIO 13 for MCPWM0B
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, LO3);   // Use GPIO 12 for MCPWM0A
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, HO3);   // Use GPIO 13 for MCPWM0B

  mcpwm_config_t pwm_config;
  pwm_config.frequency = freqCarr * 2 ;             // up-down ctr->freq*2
  pwm_config.cmpr_a = 0;                            // duty cycle to 0%
  pwm_config.cmpr_b = 0;                            // duty cycle to 0%
  pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // phase freq correct 
  pwm_config.duty_mode = MCPWM_DUTY_MODE_0;         // active high PWM
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); 
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config);   
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); 

  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0000, 0);                 // PWM_CLK_CFG_REG 
  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0004, tmrRegVal << 8);    // PWM_TIMER0_CFG0_REG 
  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0014, tmrRegVal << 8);    // PWM_TIMER0_CFG1_REG 
  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0024, tmrRegVal << 8);    // PWM_TIMER0_CFG2_REG 
  // Attach ISR function to MCPWM interrupt
  mcpwm_isr_register(MCPWM_UNIT_0, MCPWM_ISR, NULL, ESP_INTR_FLAG_IRAM, NULL);
  WRITE_PERI_REG(DR_REG_PWM0_BASE + 0x0110, BIT(3));          // INT_ENA_PWM_REG enable interrupt on TEZ
}

/*------------------------------------------------------------------------*/
void loop() {
  // nothing to do right now.....
}