#include "stm32f10x.h"
#include "servo.h"
#include "config.h"

#define STM32_MASTER_CLOCK                       72000000

/**
 * Sets a throttle output frequency
 * @param freq throttle frequency
 */
void throttle_freq (unsigned short freq)
{
	if (freq < 50)
		freq = 50;
	if (freq > 200)
		freq = 200;

	TIM_TimeBaseInitTypeDef TIM_struct;

	TIM_struct.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_struct.TIM_Prescaler = (STM32_MASTER_CLOCK / 1000000) - 1;
	TIM_struct.TIM_Period = ((1000000 / freq) - 1);
	TIM_struct.TIM_CounterMode = TIM_CounterMode_Up;

	TIM_TimeBaseInit (TIM2, &TIM_struct);
	TIM_TimeBaseInit (TIM1, &TIM_struct);
}

void throttle_set (unsigned short v)
{
	TIM_SetCompare4 (TIM2, (uint16_t) v);
	TIM_SetCompare1 (TIM1, (uint16_t) v);
}

/**
* @brief Timer Configuration
* @param None
* @retval None
*/
static void timer_conf (void)
{
	TIM_OCInitTypeDef TIM_oc;

	throttle_freq (50);

	TIM_oc.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_oc.TIM_OutputState = TIM_OutputState_Enable;
	TIM_oc.TIM_OutputNState = TIM_OutputNState_Disable;
	TIM_oc.TIM_Pulse = 1100;
	TIM_oc.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_oc.TIM_OCNPolarity = TIM_OCPolarity_High;
	TIM_oc.TIM_OCIdleState = TIM_OCIdleState_Reset;
	TIM_oc.TIM_OCNIdleState = TIM_OCNIdleState_Reset;

	/* throttle : TIM2 */
	TIM_OC4Init (TIM2, &TIM_oc);
	TIM_OC4PreloadConfig (TIM2, TIM_OCPreload_Enable);

	TIM_ARRPreloadConfig (TIM2, ENABLE);
	//TIM_CtrlPWMOutputs (TIM2, ENABLE);
	TIM_Cmd (TIM2, ENABLE);


	TIM_OC1Init (TIM1, &TIM_oc);
	TIM_OC1PreloadConfig (TIM1, TIM_OCPreload_Enable);

	TIM_ARRPreloadConfig (TIM1, ENABLE);
	TIM_CtrlPWMOutputs (TIM1, ENABLE);
	TIM_Cmd (TIM1, ENABLE);
}

/**
 * @brief Configure the used I/O ports pin
 * @param None
 * @retval None
 */
static void gpio_conf (void)
{
	/* Enable the low speed peripheral clock */
	RCC_APB1PeriphClockCmd (RCC_APB1Periph_TIM2, ENABLE);

	/* Enable the high speed peripheral clock */
	RCC_APB2PeriphClockCmd (RCC_APB2Periph_TIM1, ENABLE);

	/* throttle */
	GPIO_InitTypeDef GPIO_throttle;

	GPIO_throttle.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_8;
	GPIO_throttle.GPIO_Speed = GPIO_Speed_2MHz;
	GPIO_throttle.GPIO_Mode = GPIO_Mode_AF_PP;

	GPIO_Init (GPIOA, &GPIO_throttle);
}

int throttle_init ()
{
	gpio_conf ();
	timer_conf ();

	unsigned short i;
	while (1) {
		for (i=1300; i < 1800; i ++) {
			throttle_set (i);
			sleep (3);
		}

		for (i=1800; i > 1300; i --) {
			throttle_set (i);
			sleep (3);
		}
	}

	return 0;
}