#include "mcu_support_package/inc/stm32f10x.h"
#include "signal.h"
#include "stdbool.h"
#define dt 0.005
#define PI 3.1415926

int current_1 = 0;
int current_2 = 0;
int sample_time = 0;
int PI_velocity_period=0;
int PI_current_period=0;
static int angular_velocity = 0;
static int desired_current = 0;
static int desired_velocity = 500; //желаемая скорость
int prev_position = 0;
int position = 0;
int desired_voltage = 0;
int final_position = 0;

void TIM1_UP_IRQHandler(void) {
    TIM_ClearFlag(TIM1, TIM_FLAG_Update);
}

void TIM3_IRQHandler(void) {
    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
    sample_time++;
    PI_velocity_period++;
    PI_current_period++;
}

void init_all() {
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);

    //настройка TIM1 и TIM3
    GPIO_InitTypeDef GPIOA_pwn;
    GPIOA_pwn.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIOA_pwn.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;
    GPIOA_pwn.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIOA_pwn);

    TIM_TimeBaseInitTypeDef TIM1_init;
    TIM_TimeBaseStructInit( & TIM1_init);
    TIM1_init.TIM_Prescaler = 71;
    TIM1_init.TIM_Period = 50; //20 000 раз в секунду
    TIM1_init.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM1, &TIM1_init);
    TIM_CtrlPWMOutputs(TIM1,ENABLE);

    TIM_TimeBaseInitTypeDef TIM3_init;
    TIM_TimeBaseStructInit( & TIM3_init);
    TIM3_init.TIM_Prescaler = 71;
    TIM3_init.TIM_Period = 10; //100 000 раз в секунду
    TIM3_init.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, & TIM3_init);

    TIM_OCInitTypeDef PA8_channel_pwn;
    TIM_OCStructInit( & PA8_channel_pwn);
    PA8_channel_pwn.TIM_OCMode = TIM_OCMode_PWM1;
    PA8_channel_pwn.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OC1Init(TIM1, &PA8_channel_pwn);

    TIM_OCInitTypeDef PA9_channel_pwn;
    TIM_OCStructInit( & PA9_channel_pwn);
    PA9_channel_pwn.TIM_OCMode = TIM_OCMode_PWM1;
    PA9_channel_pwn.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OC2Init(TIM1, &PA9_channel_pwn);

    //настройка АЦП
    GPIO_InitTypeDef PortA_adc;
    PortA_adc.GPIO_Mode = GPIO_Mode_AIN;
    PortA_adc.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    GPIO_Init(GPIOA, &PortA_adc);

    ADC_InitTypeDef ADC_init;
    ADC_init.ADC_Mode = ADC_Mode_Independent;
    ADC_init.ADC_ScanConvMode = ENABLE;
    ADC_init.ADC_ContinuousConvMode = ENABLE;
    ADC_init.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
    ADC_init.ADC_DataAlign = ADC_DataAlign_Right;
    ADC_init.ADC_NbrOfChannel = 2;
    ADC_Init(ADC1,&ADC_init);
    ADC_InjectedSequencerLengthConfig(ADC1,2);
    ADC_InjectedChannelConfig(ADC1,ADC_Channel_0,1,ADC_SampleTime_55Cycles5);
    ADC_InjectedChannelConfig(ADC1,ADC_Channel_1,2,ADC_SampleTime_55Cycles5);

    ADC_ExternalTrigInjectedConvConfig( ADC1, ADC_ExternalTrigInjecConv_None );

    //калибровка АЦП
    ADC_ResetCalibration(ADC1);
    while(ADC_GetResetCalibrationStatus(ADC1)){;}
    ADC_StartCalibration(ADC1);
    while(ADC_GetCalibrationStatus(ADC1)){;}

    //включение прерываний
    ADC_ITConfig(ADC1,ADC_IT_EOC,ENABLE);
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
    NVIC_EnableIRQ(ADC1_2_IRQn);
    NVIC_EnableIRQ(TIM1_UP_IRQn);
    NVIC_EnableIRQ(TIM3_IRQn);

    TIM_Cmd(TIM1, ENABLE);
    TIM_Cmd(TIM3, ENABLE);
    ADC_Cmd(ADC1,ENABLE);

    ADC_AutoInjectedConvCmd(ADC1,ENABLE);
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}



float PI_velocity(const float error, const float Kp, const float Ki){//ПИ регулятор, возвращающий желаемый ток

    float ext_value = 0;
    static float integral = 0;
    integral += error * PI_velocity_period/100000;
    ext_value = Kp * error + Ki * integral;

    //30% от максимального значения на выходе
    if (integral > 400) integral = 400;
    if (integral < -400) integral = -400;
    return ext_value;
}

float PI_current(const float error, const float Kp, const float Ki ){ //ПИ регулятор, возвращающий желаемое напряжение

    float ext_value = 0;
    static float integral = 0;
    integral += error * PI_current_period/100000;
    ext_value= Kp * error + Ki * integral;

    //30% от максимального значения на выходе
    if (integral > 200) integral= 200;
    if (integral < -200) integral = -200;
    return ext_value;
}

void ADC1_2_IRQHandler(void) {//прерывания от АЦП
    current_1 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1) - 2048;
    current_2 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2) - 2048;
//    if(current_1<0) current_1=0;
//    if(current_2<0) current_2=0;
}

void setVoltage( int voltage ) {

    if(voltage > 9000) desired_voltage = 9000;
    if(voltage < -9000) desired_voltage = -9000;
    float PWN_Value=(int) voltage * 50 / 9000;
    if (PWN_Value>=0) {
        TIM_SetCompare1(TIM1, PWN_Value);
        TIM_SetCompare2(TIM1, 0);
    }
    else {
        TIM_SetCompare1(TIM1,0);
        TIM_SetCompare2(TIM1, -PWN_Value);
    }
}


int main(void) {

    init_all();
    TIM_SetCompare1(TIM1, 0);
    TIM_SetCompare2(TIM1, 0);

    while (1) {
        position =  TIM2->CNT ; //положение ротора
        int position_difference = (position - prev_position);
        if(position_difference > 512){
            final_position -= 1024;
            final_position += position_difference;
        }
        else {
            if (position_difference < -512){
                final_position += 1024;
                final_position += position_difference;
            }
            else final_position += position_difference;
        }
        if(sample_time >= dt * 100000){//срабатывает раз в 0.005 с
            angular_velocity = (int) 2 * PI * 100000 * final_position / (1024 * sample_time);//угловая скорость
            sample_time = 0;
            final_position = 0;
        }
        prev_position = position;

        desired_current = PI_velocity(desired_velocity - angular_velocity, 1.2, 1);//регулировка скорости 1.2 4.95
        desired_current = (int) 2048.0 * desired_current / 3000.0;//нормировка по максимальному значению на выходе
        if (desired_voltage > 0){//вращение вперёд
            if(current_1 > current_2) desired_voltage = PI_current(desired_current - current_1, 5.2, 1.1); // 18.2 10.1
            else desired_voltage = PI_current(desired_current - current_2, 5.2, 1.1);//регулировка тока
        }
        else{//вращение назад
            if(current_1 > current_2)
                desired_voltage = PI_current(desired_current + current_1, 5.2, 1.1);
            else desired_voltage = PI_current(desired_current + current_2, 5.2, 1.1);
        }

        setVoltage(desired_voltage);
    }
}

// классический ассерт для STM32
#ifdef USE_FULL_ASSERT
    void assert_failed(uint8_t * file, uint32_t line)
    {
        /* User can add his own implementation to report the file name and line number,
         ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

        (void)file;
        (void)line;

        while(1){};
    }
#endif