#include "mbed.h"
#include "rtos.h"
//******following code is the frequency calculation code******
#define PI 3.141592654
Timer timer_1;
DigitalIn CS_5490(p14);
int periodic_time;//periodic time
float omega, omegaT;



int Vdc_ref;
float dt;//time taken to run the code
#define Kp_1 100
#define Ki_1 100
float Ki_parameter_old_1 = 0;
float Ki_parameter_new_1;
float V_error;
#define Kp_2 100
#define Ki_2 100
float Ki_parameter_old_2 = 0;
float Ki_parameter_new_2;
float Id_error;
#define Kp_3 100
#define Ki_3 100
float Ki_parameter_old_3 = 0;
float Ki_parameter_new_3;
float Iq_error;
DigitalIn Vdc_sensed(p13);
float Im, Ide, Ide_ref;
float Ia, Ib, Ic, Id, Iq, Iqe_ref, omegaL, Vqe, Vqe_ref, Vde, Vm, Vde_ref, Va, Vb, Vc;
float I_alpha, I_beta, V_alpha, V_beta;


PwmOut PWM1(p21);
PwmOut PWM2(p22);
PwmOut PWM3(p23);
AnalogIn phase_a(p15);
AnalogIn phase_b(p16);
AnalogIn phase_c(p17);
//AnalogOut Aout(p18);
//Serial pc(USBTX,USBRX);
//int r = 1;
//Timer timer;
//float begin,end;
int Ts = 100;
float Vdc = 1.75262;//1.75262 value is if the waveform's maximum value is 1.If the waveform's maximum value is 3, then Vdc = 1.75262*3 

float Va_init, Vb_init, Vc_init;
float Tas,Tbs,Tcs,Tmax,Tmin,Teff,Tzero,Toffset,Tga,Tgb,Tgc,duty_1,duty_2,duty_3;


void Omega_calculation_thread(void const *argument) {
        while(CS_5490 == 1){
                
            } 
        timer_1.start();
        wait_us(500);
        
        while(1){
                while(CS_5490 == 1){
                        
                    }
                wait_us(500);
        
                while(CS_5490 == 1){
                        
                    }
                periodic_time = timer_1.read_us();
                timer_1.reset();
                omega = (2*(3.141592654))/((periodic_time)*(0.000001));
                wait_us(500);  //??????need to get the omegaT value.for that, I need to write the code??????? 
        }  
}    

//******following code is the sample reference phase amplitude calculation code******


void PI_Controller1(){
        V_error = Vdc_sensed - Vdc_ref;
        Ki_parameter_new_1 = V_error*dt;
        Ki_parameter_old_1 = Ki_parameter_old_1 + Ki_parameter_new_1;
        Ide = Kp_1*V_error + Ki_1*Ki_parameter_old_1;
        Ide_ref = Ide + Im;
    }
 
void PI_Controller2(){
        Id_error = Ide_ref - Id;
        Ki_parameter_new_2 = Id_error*dt;
        Ki_parameter_old_2 = Ki_parameter_old_2 + Ki_parameter_new_2;
        Vde = (Kp_2)*(Id_error) + (Ki_2)*(Ki_parameter_old_2);
        Vde_ref = Vde + Vm - omegaL*Iq;
    }    
    
void PI_Controller3(){
        Iq_error = Iqe_ref - Iq;
        Ki_parameter_new_3 = Iq_error*dt;
        Ki_parameter_old_3 = Ki_parameter_old_3 + Ki_parameter_new_3;
        Vqe = (Kp_3)*(Iq_error) + (Ki_3)*(Ki_parameter_old_3);
        Vqe_ref = Vqe + omegaL*Id;
    } 
       
void abc_to_dq(){
        I_alpha = (2/3)*Ia - (1/3)*Ib -(1/3)*Ic;
        I_beta = (1/1.732050808)*Ib - (1/1.732050808)*Ic;
        omegaT = omega*timer_1.read_us()*0.000001;
        Id = cos(omegaT)*I_alpha + sin(omegaT)*I_beta;
        Iq = -sin(omegaT)*I_alpha + cos(omegaT)*I_beta;
    }    
    
void dq_to_abc(){
        omegaT = omega*timer_1.read_us()*0.000001;
        V_alpha = cos(omegaT)*Vde_ref - sin(omegaT)*Vqe_ref;
        V_beta = sin(omegaT)*Vde_ref - cos(omegaT)*Vqe_ref;
        Va = V_alpha;
        Vb = -(1/2)*V_alpha + (1.732050808/2)*V_beta;
        Vc = -(1/2)*V_alpha - (1.732050808/2)*V_beta;
    }    
    
    
  //  void Omega_calculation_thread(void const *args) {

void Voltage_Current_Controller_thread(void const *argument) {
       while(1){
        PI_Controller1();
        abc_to_dq();
        PI_Controller2();
        PI_Controller3();
        dq_to_abc(); 
       
    }   
}

int main() {
    Thread thread1(Omega_calculation_thread, NULL, osPriorityHigh, (DEFAULT_STACK_SIZE * 2.25));
    Thread thread2(Voltage_Current_Controller_thread, NULL, osPriorityRealtime, (DEFAULT_STACK_SIZE * 2.25));
    
    PWM1.period_us(100);
    PWM2.period_us(100);
    PWM3.period_us(100);
    //timer.start();
    //begin = timer.read_us();
    while(1) {
        Va_init = phase_a;//ADC output is a positive integer. But we need to split this value into plus and minus.ADC value is a float and varies from 0 to 1.       
        Vb_init = phase_b;
        Vc_init = phase_c;
        //Aout = phase_a;
        //end = timer.read_us();
        //pc.printf("%f\n\r",(end-begin));
        if(Va_init<0.4924){
            Va = (Va_init-0.4924)/0.41666;
        }
        else{
            Va = (Va_init-0.4924)/0.41666;
        }
        
        if(Vb_init<0.4924){
            Vb = (Vb_init-0.4924)/0.41666;
        }
        else{
            Vb = (Vb_init-0.4924)/0.41666;
        }
        if(Vc_init<0.4924){
            Vc = (Vc_init-0.4924)/0.41666;
        }
        else{
            Vc = (Vc_init-0.4924)/0.41666;
        }
        
        
        Tas = Ts*Va/Vdc;
        Tbs = Ts*Vb/Vdc;
        Tcs = Ts*Vc/Vdc;
        Tmax = Tas;
        Tmin = Tas;
        if(Tbs>Tmax){
            Tmax = Tbs;
        }
        if(Tbs<Tmin){
            Tmin = Tbs;
        }
        if(Tcs>Tmax){
            Tmax = Tcs;
        }
        if(Tcs<Tmin){
            Tmin = Tcs;
        }
        Teff = Tmax-Tmin;
        Tzero = Ts-Teff;
        Toffset = (Tzero/2)-Tmin;
        Tga = Tas+Toffset;
        Tgb = Tbs+Toffset;
        Tgc = Tcs+Toffset;
        
        duty_1 = Tga/100;
        duty_2 = Tgb/100;
        duty_3 = Tgc/100;
        
        PWM1 = duty_1;
        PWM2 = duty_2;
        PWM3 = duty_3;
        
        //r=r+1;
    }
    
}