#include "Pin_config.h"
#include "EPWM.h"
#include "driverlib.h"
#include "device.h"
uint32_t TBCLK = DEVICE_SYSCLK_FREQ/2U;

uint16_t Phi_H2 = 0;
uint16_t Phi_H3 = 0;
uint16_t Phi_H4 = 0;


void Uptade_PWMs(EPWM_s *EPWM_variables, float32_t D_H1, float32_t D_H2, float32_t D_H3, float32_t D_H4,
 float32_t phi2, float32_t phi3, float32_t phi4){

	/// This function is called every 50us (my control loop interrupt)

        uint16_t cmp_A_H1 = D_H1 * (float32_t)EPWM_variables->TBPRD;
        uint16_t cmp_B_H1 = (1.0f - D_H1) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setCounterCompareValue(EPWM1_BASE, EPWM_COUNTER_COMPARE_A, cmp_A_H1);
        EPWM_setCounterCompareValue(EPWM1_BASE, EPWM_COUNTER_COMPARE_B, cmp_B_H1);




        uint16_t cmp_A_H2 = D_H2 * (float32_t)EPWM_variables->TBPRD;
        uint16_t cmp_B_H2 = (1.0f - D_H2) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setCounterCompareValue(EPWM2_BASE, EPWM_COUNTER_COMPARE_A, cmp_A_H2);
        EPWM_setCounterCompareValue(EPWM2_BASE, EPWM_COUNTER_COMPARE_B, cmp_B_H2);



        uint16_t cmp_A_H3 = D_H3 * (float32_t)EPWM_variables->TBPRD;
        uint16_t cmp_B_H3 = (1.0f - D_H3) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setCounterCompareValue(EPWM3_BASE, EPWM_COUNTER_COMPARE_A, cmp_A_H3);
        EPWM_setCounterCompareValue(EPWM3_BASE, EPWM_COUNTER_COMPARE_B, cmp_B_H3);
        D_H3_pre = D_H3;

        uint16_t cmp_A_H4 = D_H4 * (float32_t)EPWM_variables->TBPRD;
        uint16_t cmp_B_H4 = (1.0f - D_H4) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setCounterCompareValue(EPWM4_BASE, EPWM_COUNTER_COMPARE_A, cmp_A_H4);
        EPWM_setCounterCompareValue(EPWM4_BASE, EPWM_COUNTER_COMPARE_B, cmp_B_H4);
 
 







        if (phi2<0.0f){EPWM_setCountModeAfterSync(EPWM2_BASE, EPWM_COUNT_MODE_DOWN_AFTER_SYNC);}
        if (phi2>=0.0f){ EPWM_setCountModeAfterSync(EPWM2_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);}

        Phi_H2 = (fabsf(phi2) / 180.0f) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setPhaseShift(EPWM2_BASE, Phi_H2);




        if (phi3<0.0f){EPWM_setCountModeAfterSync(EPWM3_BASE, EPWM_COUNT_MODE_DOWN_AFTER_SYNC);}
        if (phi3>=0.0f){EPWM_setCountModeAfterSync(EPWM3_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);}

        Phi_H3 = (fabsf(phi3) / 180.0f) * (float32_t) EPWM_variables->TBPRD;

        EPWM_setPhaseShift(EPWM3_BASE, Phi_H3);




        if (phi4<0.0f){EPWM_setCountModeAfterSync(EPWM4_BASE, EPWM_COUNT_MODE_DOWN_AFTER_SYNC);}
        if (phi4>=0.0f){EPWM_setCountModeAfterSync(EPWM4_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);}

        Phi_H4 = (fabsf(phi4) / 180.0f) * (float32_t)EPWM_variables->TBPRD;

        EPWM_setPhaseShift(EPWM4_BASE, Phi_H4);



}


void Init_EPWM_variables(EPWM_s *EPWM_variables, uint32_t PWM_frequency){

     /*
    TBPRD in the microcontroller is 16-bit register (max value: 65535).
    for this configuration the minimum PWM frequncy is 1.6 KHz for UP mode and 0.8 for UP-DOWN mode. if you wish to work with lower frequncy you should 
    use clock divider settings (EPWM_setClockPrescaler);
    */

    // PWM_frequency = (PWM_frequency < 2000) ? 2000 : PWM_frequency;

    EPWM_variables->PWM_frequency = PWM_frequency;
    
    EPWM_variables->TBPRD = (TBCLK/(2UL*PWM_frequency));
    // 100 KHz PWM frequency, TBPRD = 500;

}


// THIS FUNCTION SETUP THE INITIAL SETTING OF THE EPWMs
void Init_EPWM_Setup(EPWM_s *EPWM_variables){

    EALLOW;
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_EPWM1);
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_EPWM2);
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_EPWM3);
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_EPWM4);


    SysCtl_disablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
    

    // Configure time-base period
    EPWM_setTimeBasePeriod(EPWM1_BASE, EPWM_variables->TBPRD);
    EPWM_setTimeBasePeriod(EPWM2_BASE, EPWM_variables->TBPRD);
    EPWM_setTimeBasePeriod(EPWM3_BASE, EPWM_variables->TBPRD);
    EPWM_setTimeBasePeriod(EPWM4_BASE, EPWM_variables->TBPRD);

    // Initialize counters
    EPWM_setTimeBaseCounter(EPWM1_BASE, 0);
    EPWM_setTimeBaseCounter(EPWM2_BASE, 0);
    EPWM_setTimeBaseCounter(EPWM3_BASE, 0);
    EPWM_setTimeBaseCounter(EPWM4_BASE, 0);

    // Explicit zero phase for the initial alignment
    EPWM_setPhaseShift(EPWM2_BASE, 0);
    EPWM_setPhaseShift(EPWM3_BASE, 0);
    EPWM_setPhaseShift(EPWM4_BASE, 0);



    // Configure counter mode (ALL to UP_MODE)
    EPWM_setTimeBaseCounterMode(EPWM1_BASE, EPWM_COUNTER_MODE_UP_DOWN);
    EPWM_setTimeBaseCounterMode(EPWM2_BASE, EPWM_COUNTER_MODE_UP_DOWN);
    EPWM_setTimeBaseCounterMode(EPWM3_BASE, EPWM_COUNTER_MODE_UP_DOWN);
    EPWM_setTimeBaseCounterMode(EPWM4_BASE, EPWM_COUNTER_MODE_UP_DOWN);


    // Configure clock prescalers
    EPWM_setClockPrescaler(EPWM1_BASE, EPWM_CLOCK_DIVIDER_1, EPWM_HSCLOCK_DIVIDER_1);
    EPWM_setClockPrescaler(EPWM2_BASE, EPWM_CLOCK_DIVIDER_1, EPWM_HSCLOCK_DIVIDER_1);
    EPWM_setClockPrescaler(EPWM3_BASE, EPWM_CLOCK_DIVIDER_1, EPWM_HSCLOCK_DIVIDER_1);
    EPWM_setClockPrescaler(EPWM4_BASE, EPWM_CLOCK_DIVIDER_1, EPWM_HSCLOCK_DIVIDER_1);


    // Configure shadow load mode (When the new duty scycle get updated)
    EPWM_setCounterCompareShadowLoadMode(EPWM1_BASE, EPWM_COUNTER_COMPARE_A, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM2_BASE, EPWM_COUNTER_COMPARE_A, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM3_BASE, EPWM_COUNTER_COMPARE_A, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM4_BASE, EPWM_COUNTER_COMPARE_A, EPWM_COMP_LOAD_ON_CNTR_ZERO);

    EPWM_setCounterCompareShadowLoadMode(EPWM1_BASE, EPWM_COUNTER_COMPARE_B, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM2_BASE, EPWM_COUNTER_COMPARE_B, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM3_BASE, EPWM_COUNTER_COMPARE_B, EPWM_COMP_LOAD_ON_CNTR_ZERO);
    EPWM_setCounterCompareShadowLoadMode(EPWM4_BASE, EPWM_COUNTER_COMPARE_B, EPWM_COMP_LOAD_ON_CNTR_ZERO);



    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(EPWM1_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);


    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(EPWM2_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);


    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(EPWM3_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);

    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(EPWM4_BASE, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);


    // //     // this happens only on a sync event
    EPWM_setCountModeAfterSync(EPWM1_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);
    EPWM_setCountModeAfterSync(EPWM2_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);
    EPWM_setCountModeAfterSync(EPWM3_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);
    EPWM_setCountModeAfterSync(EPWM4_BASE, EPWM_COUNT_MODE_UP_AFTER_SYNC);
    



    // // EPWM1 as master: no phase load, emits SYNCOUT at ZERO
    
    EPWM_disablePhaseShiftLoad(EPWM1_BASE);
    EPWM_setSyncOutPulseMode(EPWM1_BASE, EPWM_SYNC_OUT_PULSE_ON_COUNTER_ZERO);
    EPWM_setSyncOutPulseMode(EPWM2_BASE, EPWM_SYNC_OUT_PULSE_ON_EPWMxSYNCIN);
    EPWM_setSyncOutPulseMode(EPWM3_BASE, EPWM_SYNC_OUT_PULSE_ON_EPWMxSYNCIN);
    EPWM_setSyncOutPulseMode(EPWM4_BASE, EPWM_SYNC_OUT_PULSE_ON_EPWMxSYNCIN);

    
    EPWM_enablePhaseShiftLoad(EPWM2_BASE);
    EPWM_enablePhaseShiftLoad(EPWM3_BASE);
    EPWM_enablePhaseShiftLoad(EPWM4_BASE); 
    EPWM_forceSyncPulse(EPWM1_BASE);

    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
 
    EDIS;
    

    DEVICE_DELAY_US(1000);


    // EALLOW;
    // EPWM_disablePhaseShiftLoad(EPWM2_BASE);
    // EPWM_disablePhaseShiftLoad(EPWM3_BASE);
    // EPWM_disablePhaseShiftLoad(EPWM4_BASE);
    // EDIS;


}

