#include "motors.h"

int16_t abs(int16_t x){
    if(x >=0) return x;
    else return -x;
}

float constrain(float x, float min, float max)
{
    if((x >= min) && (x <= max)) return x;
    else if(x < min) return min;
    else return max;
}

Motors :: Motors(MPU9250_ *mpu)
{
	m_mpu = mpu;
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); //Periph clocking
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

    GPIO_InitTypeDef param; //port A configuration
    param.GPIO_Mode = GPIO_Mode_AF;
    param.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOA, &param);
    
    param.GPIO_Mode = GPIO_Mode_AF; //port B configuration
    param.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOB, &param);

    TIM_TimeBaseInitTypeDef timer_param; //timer 1 configuration
	timer_param.TIM_Period = pwm_period;
	timer_param.TIM_Prescaler = pwm_perscaller;
	timer_param.TIM_ClockDivision = TIM_CKD_DIV1;
    timer_param.TIM_CounterMode = TIM_CounterMode_Up;
	timer_param.TIM_RepetitionCounter = 0;
    
    TIM_OCInitTypeDef cann; //timer 1 channels configuration
    cann.TIM_OCMode = TIM_OCMode_PWM1;
    cann.TIM_OutputState = TIM_OutputState_Enable;
    cann.TIM_OutputNState = TIM_OutputNState_Disable;
    cann.TIM_Pulse = 0;
    cann.TIM_OCPolarity = TIM_OCPolarity_High;
    cann.TIM_OCNPolarity = TIM_OCNPolarity_High;
    cann.TIM_OCIdleState = TIM_OCIdleState_Set;
    cann.TIM_OCNIdleState = TIM_OCNIdleState_Set;
    
    TIM_TimeBaseInit(TIM1, &timer_param); //timer 1 Init
    TIM_Cmd(TIM1, ENABLE);
    
    TIM_OC1Init(TIM1, &cann); //timer 1 channels Init
    TIM_OC2Init(TIM1, &cann);
    TIM_OC3Init(TIM1, &cann);
    TIM_OC4Init(TIM1, &cann);
    TIM_CtrlPWMOutputs(TIM1, ENABLE);
    
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF_TIM1); //GPIO alternative function config
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_TIM1);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_TIM1);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_TIM1);
    
    TIM_TimeBaseInit(TIM4, &timer_param); //timer 4 Init
    TIM_Cmd(TIM4, ENABLE);
    
    TIM_OC1Init(TIM4, &cann); //timer 4 channels Init
    TIM_OC2Init(TIM4, &cann);
    TIM_OC3Init(TIM4, &cann);
    TIM_OC4Init(TIM4, &cann);
    //TIM_CtrlPWMOutputs(TIM4, ENABLE);
    
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_TIM4); //GPIO alternative function config
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_TIM4);
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_TIM4);
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_TIM4);
    
    m1_pos = 0;
    m2_pos = 0;
    m3_pos = 0;
    m4_pos = 0;
    
    enc1::Init(); //encoders initialization
    enc2::Init();
    enc3::Init();
    enc4::Init();
}

void Motors :: m1(int16_t val)
{
    if(val*m1_inverse >= 0)
    {
        TIM_SetCompare1(TIM1, abs(val));
        TIM_SetCompare2(TIM1, 0);
    }
    else
    {
        TIM_SetCompare1(TIM1, 0);
        TIM_SetCompare2(TIM1, abs(val));
    }
}

void Motors :: m2(int16_t val)
{
    if(val*m2_inverse >= 0)
    {
        TIM_SetCompare3(TIM1, 0);
        TIM_SetCompare4(TIM1, abs(val));
    }
    else
    {
        TIM_SetCompare3(TIM1, abs(val));
        TIM_SetCompare4(TIM1, 0);
    }
}

void Motors :: m3(int16_t val)
{
    if(val*m3_inverse >= 0)
    {
        TIM_SetCompare1(TIM4, 0);
        TIM_SetCompare2(TIM4, abs(val));
    }
    else
    {
        TIM_SetCompare1(TIM4, abs(val));
        TIM_SetCompare2(TIM4, 0);
    }
}

void Motors :: m4(int16_t val)
{
    if(val*m4_inverse >= 0)
    {
        TIM_SetCompare3(TIM4, abs(val));
        TIM_SetCompare4(TIM4, 0);
    }
    else
    {
        TIM_SetCompare3(TIM4, 0);
        TIM_SetCompare4(TIM4, abs(val));
    }
}

void Motors :: m1_pos_add(double pos)
{
    m1_pos += pos;
}

void Motors :: m2_pos_add(double pos)
{
    m2_pos += pos;
}

void Motors :: m3_pos_add(double pos)
{
    m3_pos += pos;
}

void Motors :: m4_pos_add(double pos)
{
    m4_pos += pos;
}


void Motors :: m1_pos_set(double pos)
{
    m1_pos = pos;
}

void Motors :: m2_pos_set(double pos)
{
    m2_pos = pos;
}

void Motors :: m3_pos_set(double pos)
{
    m3_pos = pos;
}

void Motors :: m4_pos_set(double pos)
{
    m4_pos = pos;
}

double Motors :: m1_pos_get()
{
    return m1_pos;
}

double Motors :: m2_pos_get()
{
    return m2_pos;
}

double Motors :: m3_pos_get()
{
    return m3_pos;
}

double Motors :: m4_pos_get()
{
    return m4_pos;
}

void Motors :: m1_pid()
{
    static double errold = 0;
    static double i = 0;
    static uint64_t old_time = 0;
    
    double err = m1_pos - enc1::deg();
    int p = err*k_p;
    i += k_i*err*(time_service::micros() - old_time);
    i = constrain(i, -5000.0, 5000.0);
    int d = (err - errold)*k_d/(time_service::micros() - old_time);
    int u = p + i + d;
    if(abs(err) < 2) u = 0;
    m1(constrain(u, -32767, 32767));
    errold = err;
    old_time = time_service::micros();
}

void Motors :: m2_pid()
{
    static double errold = 0;
    static double i = 0;
    static uint64_t old_time = 0;
    
    double err = m2_pos - enc2::deg();
    int p = err*k_p;
    i += k_i*err*(time_service::micros() - old_time);
    i = constrain(i, -5000.0, 5000.0);
    int d = (err - errold)*k_d/(time_service::micros() - old_time);
    int u = p + i + d;
    if(abs(err) < 2) u = 0;
    m2(constrain(u, -32767, 32767));
    errold = err;
    old_time = time_service::micros();
}

void Motors :: m3_pid()
{
    static double errold = 0;
    static double i = 0;
    static uint64_t old_time = 0;
    
    double err = m3_pos - enc3::deg();
    int p = err*k_p;
    i += k_i*err*(time_service::micros() - old_time);
    i = constrain(i, -5000.0, 5000.0);
    int d = (err - errold)*k_d/(time_service::micros() - old_time);
    int u = p + i + d;
    if(abs(err) < 2) u = 0;
    m3(constrain(u, -32767, 32767));
    errold = err;
    old_time = time_service::micros();
}

void Motors :: m4_pid()
{
    static double errold = 0;
    static double i = 0;
    static uint64_t old_time = 0;
    
    double err = m4_pos - enc4::deg();
    int p = err*k_p;
    i += k_i*err*(time_service::micros() - old_time);
    i = constrain(i, -5000.0, 5000.0);
    int d = (err - errold)*k_d/(time_service::micros() - old_time);
    int u = p + i + d;
    if(abs(err) < 2) u = 0;
    m4(constrain(u, -32767, 32767));
    errold = err;
    old_time = time_service::micros();
}

void Motors :: calculate_pid()
{
    m1_pid();
    m2_pid();
    m3_pid();
    m4_pid();
}

void Motors :: drive(double v, double u, double w)
{
    static uint64_t old_time = 0;
    uint32_t dt = time_service::micros() - old_time;
    if(dt > 30000) dt = 1000; //call period in microseconds 
	
    float q2 = constrain(LINEAR_TO_ANGULAR*(v + u + w*ang_k), -max_vel, max_vel); //linear velocity of wheels
    float q1 = constrain(LINEAR_TO_ANGULAR*(v - u - w*ang_k), -max_vel, max_vel);
    float q3 = constrain(LINEAR_TO_ANGULAR*(v - u + w*ang_k), -max_vel, max_vel);
    float q4 = constrain(LINEAR_TO_ANGULAR*(v + u - w*ang_k), -max_vel, max_vel);
    
    m1_pos += q1 * dt / 1000000.0; //add position
    m2_pos += q2 * dt / 1000000.0;
    m3_pos += q3 * dt / 1000000.0;
    m4_pos += q4 * dt / 1000000.0;
    old_time = time_service::micros();
    calculate_pid();
}

void Motors :: driveGyro(double v, double u, double w)
{
	m_mpu->update_accel_gyro();
	static double dw_old = 0;
	
    static uint64_t old_time = 0;
    uint32_t dt = time_service::micros() - old_time;
    if(dt > 30000) dt = 1000; //call period in microseconds 
	
	if(m_mpu->isConnected())
	{
		if(abs(m_mpu->getGyroZ() - w) > 3) w -= (m_mpu->getGyroZ() - w) * 2.6 + (m_mpu->getGyroZ() - w - dw_old)*1500/dt;
	}
	drive(v, u, w);
    old_time = time_service::micros();
}

void Motors :: stop()
{
	m1_pos = enc1::deg(); //set target positions to current
	m2_pos = enc2::deg();
	m3_pos = enc3::deg();
	m4_pos = enc4::deg();
	m1(0); //stop motors
	m2(0);
	m3(0);
	m4(0);
}

Servos :: Servos()
{
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); //Periph clocking
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
    
    GPIO_InitTypeDef param; //port B configuration
    param.GPIO_Mode = GPIO_Mode_AF;
    param.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_14 | GPIO_Pin_15;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOB, &param);
    
    TIM_TimeBaseInitTypeDef timer_param; //timer 3 configuration
	timer_param.TIM_Period = 10000;
	timer_param.TIM_Prescaler = 336;
	timer_param.TIM_ClockDivision = TIM_CKD_DIV1;
    timer_param.TIM_CounterMode = TIM_CounterMode_Up;
	timer_param.TIM_RepetitionCounter = 0;
    
    TIM_OCInitTypeDef cann; //timer 3 channels configuration
    cann.TIM_OCMode = TIM_OCMode_PWM1;
    cann.TIM_OutputState = TIM_OutputState_Enable;
    cann.TIM_OutputNState = TIM_OutputNState_Enable;
    cann.TIM_Pulse = 0;
    cann.TIM_OCPolarity = TIM_OCPolarity_High;
    cann.TIM_OCNPolarity = TIM_OCNPolarity_Low;
    cann.TIM_OCIdleState = TIM_OCIdleState_Set;
    cann.TIM_OCNIdleState = TIM_OCNIdleState_Set;
    
    TIM_TimeBaseInit(TIM3, &timer_param); //timer 3 Init
    TIM_Cmd(TIM3, ENABLE);
    
    TIM_OC1Init(TIM3, &cann); //timer 3 channels Init
    TIM_OC2Init(TIM3, &cann);
    TIM_CtrlPWMOutputs(TIM3, ENABLE);
    
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3); //GPIO alternative function config
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
    
    TIM_TimeBaseInit(TIM12, &timer_param); //timer 12 Init
    TIM_Cmd(TIM12, ENABLE);
    
    TIM_OC1Init(TIM12, &cann); //timer 12 channels Init
    TIM_OC2Init(TIM12, &cann);
    TIM_CtrlPWMOutputs(TIM12, ENABLE);
    
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource15, GPIO_AF_TIM12); //GPIO alternative function config
	
	timer_param.TIM_Period = 10000; //timer 8 connfiguration
	timer_param.TIM_Prescaler = 55;
	
	TIM_TimeBaseInit(TIM8, &timer_param); //timer 3 Init
    TIM_Cmd(TIM8, ENABLE);
	
	TIM_OC2Init(TIM8, &cann);
	
	TIM_CtrlPWMOutputs(TIM8, ENABLE);
	
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource14, GPIO_AF_TIM8);
}

void Servos :: s1(float pos)
{
    if(pos > 180) pos=180;
    pos*=5.16;
    pos+=272;
    
    TIM_SetCompare1(TIM3, pos);
}

void Servos :: s2(float pos)
{
    if(pos > 180) pos=180;
    pos*=4.1666;
    pos+=350;
    
    TIM_SetCompare2(TIM3, pos);
}

void Servos :: s3(float pos)
{
    if(pos > 180) pos=180;
    //pos*=34.65;
    //pos+=1828; 8065
	//pos = pos * (7630 - 2100) / 180 + 2100;
	pos = pos * (7590 - 2100) / 180 + 2100;
    
    TIM_SetCompare2(TIM8, (uint32_t)pos);
}

void Servos :: s4(float pos)
{
    if(pos > 180) pos=180;
    pos*=5.16;
    pos+=272;
    
    TIM_SetCompare2(TIM12, pos);
}

void Servos :: s1_stop()
{
	TIM_SetCompare1(TIM3, 0);
}

void Servos :: s2_stop()
{
	TIM_SetCompare2(TIM3, 0);
}

void Servos :: s3_stop()
{
	TIM_SetCompare2(TIM8, 0);
}

void Servos :: s4_stop()
{
	TIM_SetCompare2(TIM12, 0);
}
