/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_conf.h"
#include "Servos.h"

/* Private define ------------------------------------------------------------*/
#define SERVOS_NeutralPulseLength (1520)
#define SERVOS_NeutralAngle       (0.78539816339744830961566084581988f) // 45 degrees (1.85)
#define SERVOS_Scalar             (509.29581789406507446042804279205f)  // 400us / 45 degrees
#define SERVOS_Offset             (580) // SERVO_NeutralPulseLength - SERVO_NeutralAngle * SERVO_Scalar

/* Private variables ---------------------------------------------------------*/
float volatile _calibration[3];

/* Private function prototypes -----------------------------------------------*/
static bool Servos_CalculatePulseLength(float angle, /* out */ uint16_t* pulselength);

/* Exported functions ------------------------------------------------------- */

// Servo_Init()
// ------------
// Initialize the servo module.
// Called from the main thread.
extern void Servos_Init()
{
	_calibration[0] = 0;
	_calibration[1] = 0;
	_calibration[2] = 0;

    /* Enable GPIOA, AFIO, and Timer1 clocks ---------------------------------*/
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO | RCC_APB2Periph_TIM1, ENABLE);

	/* GPIO configuration ----------------------------------------------------*/
    // PWM output Timer1 - Channels 1, 2 & 3
    // Pin 8  (Port A) - Leg 41 PWM_OUT
    // Pin 9  (Port A) - Leg 42 PWM_OUT
    // Pin 10 (Port A) - Leg 43 PWM_OUT
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    /* Timer1 configuration --------------------------------------------------*/
    // Timer1 must be configured in such way to generate update event every 20 ms */
    // Timer1 is clocked by PCLK2 = 72MHz
    TIM_DeInit(TIM1);
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseInitStructure;
    TIM_TimeBaseInitStructure.TIM_Prescaler = 71; //clocked at 72MHz / 72 = 1MHz
    TIM_TimeBaseInitStructure.TIM_Period = 19999; //period = (72MHz / 72) / 20000 = 50Hz (20ms period)
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStructure);

    /* Channel 1, 2 & 3 Configuration in PWM mode ----------------------------*/
    TIM_OCInitTypeDef  TIM_OCInitStructure;
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
    TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;

    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);

    // Changes take effect on the next Update event
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);

    TIM_CtrlPWMOutputs(TIM1, ENABLE);

    //TIM1 enable counter
    TIM_Cmd(TIM1, ENABLE);
}

#ifdef SERVOS_SET_INDIVIDUAL

// Servo_SetCalibration()
// ----------------------
// Set the calibration parameter for an individual servo.
// Called from interrupt.
extern void Servos_SetCalibration(servo_TypeDef servo, float calibration)
{
	_calibration[servo] = calibration;
}

// Servo_SetAngle()
// ----------------
// Set the angle for an individual servo.
// Returns false if the requested angle is outside the range of the servo.
// Called from the main thread.
extern bool Servos_SetAngle(servo_TypeDef servo, float angle)
{
	uint16_t pulselength;

	if (!Servos_CalculatePulseLength(angle + _calibration[servo], &pulselength))
	{
		switch (servo)
		{
		case SERVO_1:
			TIM1->CCR1 = pulselength;
			break;

		case SERVO_2:
			TIM1->CCR2 = pulselength;
			break;

		case SERVO_3:
			TIM1->CCR3 = pulselength;
			break;
		}

		return TRUE;
	}
    
    return FALSE;
}

#endif

// Servo_SetAllCalibrations()
// --------------------------
// Set the calibration parameters for all servos simultaneously.
// Called from interrupt via Robot_OnADCCompete().
extern void Servos_SetAllCalibrations(float servo1calibration, float servo2calibration, float servo3calibration)
{
	__disable_irq();
	_calibration[SERVO_1] = servo1calibration;
	_calibration[SERVO_2] = servo2calibration;
	_calibration[SERVO_3] = servo3calibration;
	__enable_irq();
}

// Servo_AllAngles()
// -----------------
// Set the angles for all servos simultaneously.
// Returns false if the requested angle is outside the range of the servo.
// Called from the main thread via Robot_Calculate().
extern bool Servos_SetAllAngles(float servo1angle, float servo2angle, float servo3angle)
{
	uint16_t pulselength1;
	uint16_t pulselength2;
	uint16_t pulselength3;

	__disable_irq();
	float s1 = servo1angle + _calibration[SERVO_1];
	float s2 = servo2angle + _calibration[SERVO_2];
	float s3 = servo3angle + _calibration[SERVO_3];
	__enable_irq();

	if (Servos_CalculatePulseLength(s1, &pulselength1) &&
		Servos_CalculatePulseLength(s2, &pulselength2) &&
		Servos_CalculatePulseLength(s3, &pulselength3))
    {
	    TIM1->CCR1 = pulselength1;
		TIM1->CCR2 = pulselength2;
		TIM1->CCR3 = pulselength3;

		return TRUE;
    }

	return FALSE;
}

/* Private functions ---------------------------------------------------------*/

// Servos_CalculatePulseLength()
// -----------------------------
// Calculate the pulse length for the servo. Takes into account the calibration parameter.
// Returns false if the requested angle is outside the range of the servo.
// Called from the main thread via Servos_SetAllAngles.
static bool Servos_CalculatePulseLength(float angle, /* out */ uint16_t* pulselength)
{
	*pulselength = (uint16_t)(angle * SERVOS_Scalar + SERVOS_Offset);

	return *pulselength > 720 && *pulselength < 2320;
}
