/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_conf.h"
#include "Robot.h"
#include "Coordinate.h"
#include "Interrupts.h"
#include "Servos.h"

#include <math.h>

/* Private define ------------------------------------------------------------*/
// robot constants
#define ROBOT_UpperArmLength    (119.0f) // length of upper arm from elbow to end actuator pivot
#define ROBOT_LowerArmLength    (90.0f)  // length of lower arm from elbow to center of servo
#define ROBOT_ServoOffset       (51.0f)  // distance from reference point (center of base) to center of servo
#define ROBOT_EndActuatorOffset (30.0f)  // distance from center of end actuator to end actuator pivot

// servo reference frame translation parameters
#define ROBOT_Servo1_SineTheta   (0.0f)
#define ROBOT_Servo1_CosineTheta (1.0f)
#define ROBOT_Servo2_SineTheta   (0.86602540378443864676372317075294f)
#define ROBOT_Servo2_CosineTheta (-0.5f)
#define ROBOT_Servo3_SineTheta   (-0.86602540378443864676372317075294f)
#define ROBOT_Servo3_CosineTheta (-0.5f)

// precalculated constants
#define ROBOT_ArmOffset             (ROBOT_ServoOffset - ROBOT_EndActuatorOffset)
#define ROBOT_UpperArmLengthSquared (ROBOT_UpperArmLength * ROBOT_UpperArmLength)
#define ROBOT_LowerArmLengthSquared (ROBOT_LowerArmLength * ROBOT_LowerArmLength)

// servo calibration angle
#define ROBOT_CalibrationAngle (1.5707963267948966192313216916398f)

/* Private variables ---------------------------------------------------------*/
volatile Coordinate_Absolute _absoluteCoordinate = { 0.0f, 0.0f, 150.0f };
volatile uint16_t _adcvalue[3];

/* Private function prototypes -----------------------------------------------*/
static void Robot_OnADCComplete();
static void Robot_TranslateToServoCoordinates(servo_TypeDef servo, const Coordinate_Absolute absoluteCoordinate, /* out */ Coordinate_Local* localCoordinatePtr);
static bool Robot_CalculateServoAngle(servo_TypeDef servo, const Coordinate_Absolute absoluteCoordinate, /* out */ float* anglePtr);

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

// Robot_Init()
// ------------
// Initialize the Robot module.
extern void Robot_Init()
{
	Servos_Init();

	/* Enable GPIOA, ADC1 and DMA clocks -------------------------------------*/
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_ADC1, ENABLE);

	/* GPIO configuration ----------------------------------------------------*/
	// Pin 1 (Port A) - Leg 8  ADC123_IN10
	// Pin 2 (Port A) - Leg 9  ADC123_IN11
	// Pin 3 (Port A) - Leg 10 ADC123_IN12
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	/* DMA1 channel1 configuration -------------------------------------------*/
	//DMA_DeInit(DMA1_Channel1);
	DMA_InitTypeDef   DMA_InitStructure;
	DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; ;
	DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&_adcvalue;
	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
	DMA_InitStructure.DMA_BufferSize = 3;
	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
	DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
	DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
	DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
	DMA_Init(DMA1_Channel1, &DMA_InitStructure);

	/* ADC1 configuration ----------------------------------------------------*/
	//ADC_DeInit(ADC1);
	ADC_InitTypeDef   ADC_InitStructure;
	ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
	ADC_InitStructure.ADC_ScanConvMode = ENABLE;
	ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
	ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
	ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
	ADC_InitStructure.ADC_NbrOfChannel = 3;
	ADC_Init(ADC1, &ADC_InitStructure);

	/* ADC1 regular channels configuration -----------------------------------*/
	ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_239Cycles5);
	ADC_RegularChannelConfig(ADC1, ADC_Channel_2, 2, ADC_SampleTime_239Cycles5);
	ADC_RegularChannelConfig(ADC1, ADC_Channel_3, 3, ADC_SampleTime_239Cycles5);

	/* Enable ADC1 DMA -------------------------------------------------------*/
	ADC_DMACmd(ADC1, ENABLE);

	/* Enable ADC1 -----------------------------------------------------------*/
	ADC_Cmd(ADC1, ENABLE);

	/* Enable ADC1 reset calibaration register -------------------------------*/
	ADC_ResetCalibration(ADC1);

	/* Check the end of ADC1 reset calibration register ----------------------*/
	while(ADC_GetResetCalibrationStatus(ADC1));

	/* Start ADC1 calibaration -----------------------------------------------*/
	ADC_StartCalibration(ADC1);

	/* Check the end of ADC1 calibration -------------------------------------*/
	while(ADC_GetCalibrationStatus(ADC1));

	DMA_Cmd(DMA1_Channel1, ENABLE);

	/* Configure and enable ADC interrupt ------------------------------------*/
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel = ADC1_2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

    /* Register interrupt handler --------------------------------------------*/
	Interrupts_RegisterCallback(INTERRUPTS_ADC, Robot_OnADCComplete);

	/* Enable interrupt on end of conversion ---------------------------------*/
	ADC_ITConfig(ADC1, ADC_IT_EOC, ENABLE);
}

// Robot_BeginCalibration()
// ------------------------
// Start the continuous reading of the ADC values from the servo calibration trimpots.
// Values are read via DMA and stored in the global _adcvalues.
extern void Robot_BeginCalibration()
{
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}

// Robot_UpdateCalibration()
// -------------------------
// Sets all servos to a known angle so that the servo calibration parameters can
// be correctly adjusted.
extern void Robot_UpdateCalibration()
{
	Servos_SetAllAngles(ROBOT_CalibrationAngle, ROBOT_CalibrationAngle, ROBOT_CalibrationAngle);
}

// Robot_EndCalibration()
// ----------------------
// Stop the ADC conversions and leave the servo calibration parameters in a fixed state.
extern void Robot_EndCalibration()
{
	ADC_SoftwareStartConvCmd(ADC1, DISABLE);
}

// Robot_SetPosition()
// -------------------
// Set the required position of the robot end actuator.
// The actual calculation of the servo angles is handled in Robot_Calculate().
// Called from interrupt via MotionControl_OnTimer.
extern void Robot_SetPosition(const Coordinate_Absolute absoluteCoordinate)
{
    _absoluteCoordinate[0] = absoluteCoordinate[0];
    _absoluteCoordinate[1] = absoluteCoordinate[1];
    _absoluteCoordinate[2] = absoluteCoordinate[2];
}

// Robot_Calculate()
// -----------------
// Calculate the servo angles to place the end actuator of the delta robot
// in the required position.
//
// Returns false if the required position is outside the limits of the
// delta robot or the servos.
extern bool Robot_Calculate()
{
	float servo1angle;
    float servo2angle;
	float servo3angle;

	// shadow copy
	__disable_irq();
	Coordinate_Absolute shadow;
	shadow[0] = _absoluteCoordinate[0];
	shadow[1] = _absoluteCoordinate[1];
	shadow[2] = _absoluteCoordinate[2];
	__enable_irq();

	if (Robot_CalculateServoAngle(SERVO_1, shadow, &servo1angle) &&
	    Robot_CalculateServoAngle(SERVO_2, shadow, &servo2angle) &&
	    Robot_CalculateServoAngle(SERVO_3, shadow, &servo3angle))
	{
	    return Servos_SetAllAngles(servo1angle, servo2angle, servo3angle);
	}

	return FALSE;
}

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

// Robot_OnADCCompete()
// --------------------
// On the completion of each ADC conversion set the calibration parameters for
// each servo based on the value of the corresponding trimpot.
// Called from ADC complete interrupt.
static void Robot_OnADCComplete()
{
	// read the ADC pins and convert to servo offset value
	float servo1calibration = (128 - (_adcvalue[1] >> 4)) * 0.002f;
	float servo2calibration = (128 - (_adcvalue[2] >> 4)) * 0.002f;
	float servo3calibration = (128 - (_adcvalue[0] >> 4)) * 0.002f;

	Servos_SetAllCalibrations(servo1calibration, servo2calibration, servo3calibration);
}

// Robot_TranslateToServoCoordinates()
// -----------------------------------
// Translate the absolute coordinates of the delta robot end actuator into a coordinate system
// that simplifies the calculations for the given servo.
// Returns the translated coordinates in the localCoordinatePtr parameter.
static void Robot_TranslateToServoCoordinates(servo_TypeDef servo, const Coordinate_Absolute absoluteCoordinate, /* out */ Coordinate_Local* localCoordinatePtr)
{
  switch (servo)
	{
	case SERVO_1:
		(*localCoordinatePtr)[0] = absoluteCoordinate[0] * ROBOT_Servo1_SineTheta +
		                           absoluteCoordinate[1] * ROBOT_Servo1_CosineTheta -
		                           ROBOT_ArmOffset;
		(*localCoordinatePtr)[1] = absoluteCoordinate[2];
		(*localCoordinatePtr)[2] = absoluteCoordinate[0] * ROBOT_Servo1_CosineTheta -
				                   absoluteCoordinate[1] * ROBOT_Servo1_SineTheta;
		break;

	case SERVO_2:
		(*localCoordinatePtr)[0] = absoluteCoordinate[0] * ROBOT_Servo2_SineTheta +
		                           absoluteCoordinate[1] * ROBOT_Servo2_CosineTheta -
		                           ROBOT_ArmOffset;
		(*localCoordinatePtr)[1] = absoluteCoordinate[2];
		(*localCoordinatePtr)[2] = absoluteCoordinate[0] * ROBOT_Servo2_CosineTheta -
				                   absoluteCoordinate[1] * ROBOT_Servo2_SineTheta;
		break;

	case SERVO_3:
		(*localCoordinatePtr)[0] = absoluteCoordinate[0] * ROBOT_Servo3_SineTheta +
		                           absoluteCoordinate[1] * ROBOT_Servo3_CosineTheta -
		                           ROBOT_ArmOffset;
		(*localCoordinatePtr)[1] = absoluteCoordinate[2];
		(*localCoordinatePtr)[2] = absoluteCoordinate[0] * ROBOT_Servo3_CosineTheta -
					               absoluteCoordinate[1] * ROBOT_Servo3_SineTheta;
	  break;
	}
}

// Robot_CalculateServoAngle()
// ---------------------------
// Calculate an individual servo angle given the required end actuator position.
// Returns false if the required position is outside the limits of the delta robot.
// Called from main thread via Robot_Calculate.
static bool Robot_CalculateServoAngle(servo_TypeDef servo, const Coordinate_Absolute absoluteCoordinate, /* out */ float* anglePtr)
{
	Coordinate_Local localCoordinate;
    Robot_TranslateToServoCoordinates(servo, absoluteCoordinate, &localCoordinate);

	// determine the projection length of the upper arm onto the plane
	float r2 = ROBOT_UpperArmLengthSquared - localCoordinate[2] * localCoordinate[2];
	float r = sqrtf(r2);

    // determine distance to point in servo plane
	float d = hypotf(localCoordinate[0], localCoordinate[1]);

    if (d > (r + ROBOT_LowerArmLength))
    {
    	// no solution
    	return FALSE;
    }
    if (d < fabs(r - ROBOT_LowerArmLength))
    {
    	// no solution
    	return FALSE;
    }

    float a = (ROBOT_LowerArmLengthSquared - r2 + (d * d)) / (2.0 * d);
    float scalar = a / d;
    float cx = localCoordinate[0] * scalar;
    float cy = localCoordinate[1] * scalar;

    float h = sqrtf(ROBOT_LowerArmLengthSquared - (a * a));

    scalar = h / d;
    float dx = -localCoordinate[1] * scalar;
    float dy = localCoordinate[0] * scalar;

    float x = cx - dx;
    float y = cy - dy;

    *anglePtr = atan2f(y, x);
    return TRUE;
}
