/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_conf.h"
#include "MotionControl.h"
#include "Interrupts.h"
#include "Robot.h"
#include <math.h>

/* Private definitions -------------------------------------------------------*/
#define MOTIONCONTROL_AnglePerSecond (-0.10471975511965977461542144610932f) // PI / 30
#define MOTIONCONTROL_ClockRadius (65.0f)
#define MOTIONCONTROL_ClockHeightRunning (175.0f)
#define MOTIONCONTROL_ClockHeightConfiguring (145.0f)

/* Private variables ---------------------------------------------------------*/
volatile MotionControl_State _motioncontrol_state = MOTIONCONTROL_Setup;
volatile int _motioncontrol_counter = 0;
volatile int _motioncontrol_seconds = 0;

/* Private function prototypes -----------------------------------------------*/
static void MotionControl_OnTimer();

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

// MotionControl_Init()
// --------------------
// Initialize the motion control module.
void MotionControl_Init()
{
	/* Initialise the robot --------------------------------------------------*/
	Robot_Init();

	/* Enable Timer2 clock ---------------------------------------------------*/
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

	/* Timer2 configuration --------------------------------------------------*/
	// Timer2 is clocked by PCLK2 = 72MHz
	// Timer2 must be configured in such way to generate update event every 250 ms */
	TIM_DeInit(TIM2);
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 719; //clocked at 72MHz / 720 = 0.1MHz
	TIM_TimeBaseInitStructure.TIM_Period = 24999; //period = (72MHz / 720) / 25000 = 4Hz (250ms period)
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;	
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);

    /* Enable Timer2 ---------------------------------------------------------*/
    TIM_Cmd(TIM2, ENABLE);

    /* Configure Timer2 interrupt --------------------------------------------*/
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    /* Enable Timer2 interrupt -----------------------------------------------*/
    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);

	/* Register callback for Timer2 interrupt --------------------------------*/
    Interrupts_RegisterCallback(INTERRUPTS_TIM2, MotionControl_OnTimer);
}

// MotionControl_SetState()
// ------------------------
// Set the current behaviour of the motion control module.
void MotionControl_SetState(MotionControl_State state)
{
	_motioncontrol_state = state;
}

// MotionControl_OnSecond()
// ------------------------
// One second interrupt handler.
// Keeps the motion control module in sync with the real time clock.
void MotionControl_OnSecond(uint16_t seconds)
{
    _motioncontrol_seconds = seconds % 60;
	_motioncontrol_counter = -1;
}

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

// MotionControl_OnTimer()
// -----------------------
// Interrupt handler for the 100ms timer.
static void MotionControl_OnTimer()
{
	/* Shadow copy -----------------------------------------------------------*/
	__disable_irq();
	_motioncontrol_counter++;

	MotionControl_State state = _motioncontrol_state;
	int seconds = _motioncontrol_seconds;
	int counter = _motioncontrol_counter;
	__enable_irq();

	Coordinate_Absolute absoluteCoordinate;
	float angle;

	switch(state)
	{
		default:
		case MOTIONCONTROL_Configuring:
			absoluteCoordinate[0] = 0.0f;
			absoluteCoordinate[1] = 0.0f;
			absoluteCoordinate[2] = MOTIONCONTROL_ClockHeightConfiguring;
			Robot_SetPosition(absoluteCoordinate);
			break;

		case MOTIONCONTROL_Alarm:
			if (counter == 0) // every second
			{
				// up on every odd second and down on every even
				absoluteCoordinate[0] = 0.0f;
				absoluteCoordinate[1] = 0.0f;
				absoluteCoordinate[2] = MOTIONCONTROL_ClockHeightConfiguring + (seconds % 2) * 35.0f;
				Robot_SetPosition(absoluteCoordinate);
			}
			break;

		case MOTIONCONTROL_Running:
			// this is called every 250ms
			angle = (seconds + (0.25f * counter)) * MOTIONCONTROL_AnglePerSecond;

			absoluteCoordinate[0] = MOTIONCONTROL_ClockRadius * sinf(angle);
			absoluteCoordinate[1] = MOTIONCONTROL_ClockRadius * cosf(angle);
			absoluteCoordinate[2] = MOTIONCONTROL_ClockHeightRunning;
			Robot_SetPosition(absoluteCoordinate);
			break;
	}
}
