// ==========================================================================
// file.c
// (c) 2020, Aurel Dumitru
//
// Description:
// File operations
// =========================================================================

#include "motor_test.h"
#include "main.h"
#include "rtc.h"
#include "devstatus.h"
#include "utils.h"
#include "calibrations.h"

#define MOTORTEST_IDLE_STATE				0
#define MOTORTEST_START_BOTH_MOTORS_STATE	1
#define MOTORTEST_FORWARD_STATE				2
#define MOTORTEST_FORWARD_COAST_STATE		3
#define MOTORTEST_BACKWARDS_1_STATE			4
#define MOTORTEST_BACKWARDS_STATE			5

uint8_t  MotorTest_State = MOTORTEST_IDLE_STATE;
uint32_t MotorTest_TimeStamp;


void MotorTest_Start(void)
{
	MotorTest_State = MOTORTEST_START_BOTH_MOTORS_STATE;
}

void MotorTest_Server()
{
uint32_t Time = HAL_GetTick() - MotorTest_TimeStamp;

switch (MotorTest_State)
{
case MOTORTEST_START_BOTH_MOTORS_STATE:
	// Start both motors forward
	HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port, MOTOR2_PH_Pin,  GPIO_PIN_SET);
	//HAL_GPIO_WritePin(MotorEnable_GPIO_Port, MotorEnable_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_SET);
	MotorTest_TimeStamp = HAL_GetTick();
	MotorTest_State = MOTORTEST_FORWARD_STATE;
	break;

case MOTORTEST_FORWARD_STATE:
	if (Time > 300)
	{
		// Set motors to coast
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_RESET);
		MotorTest_State = MOTORTEST_FORWARD_COAST_STATE;
	}
	break;

case MOTORTEST_FORWARD_COAST_STATE:
	if (Time > 400)
	{
		// Start motor1 backward
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port,  MOTOR1_PH_Pin,  GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_SET);
		MotorTest_State = MOTORTEST_BACKWARDS_1_STATE;
	}
	break;

case MOTORTEST_BACKWARDS_1_STATE:
	if (Time > 500)
	{
		// Start motor2 backward
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port,  MOTOR2_PH_Pin,   GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin,  GPIO_PIN_SET);
		MotorTest_State = MOTORTEST_BACKWARDS_STATE;
	}
	break;

case MOTORTEST_BACKWARDS_STATE:
	if (Time > 750)
	{
		// Disable motors
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port,  MOTOR2_PH_Pin,  GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port,  MOTOR1_PH_Pin,  GPIO_PIN_RESET);
		MotorTest_State = MOTORTEST_IDLE_STATE;
	}
	break;
}
}


void MotorTest_Health(void)
{
	uint32_t I;
	uint32_t AdcMRef, AdcMAct;
	ADC_ChannelConfTypeDef AdcChConfig;

	if ((DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] != DEVSTATUS_MOTOR_OK) ||
		(DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] != DEVSTATUS_MOTOR_OK) ||
		(Rtc_GetCurrentHour() == NC_MOTOR_TEST_HOUR))
	{
		AdcChConfig.Channel = ADC_CHANNEL_6;
		AdcChConfig.Rank = ADC_REGULAR_RANK_1;
		AdcChConfig.SamplingTime = ADC_SAMPLETIME_64CYCLES_5;
		AdcChConfig.SingleDiff = ADC_SINGLE_ENDED;
		AdcChConfig.OffsetNumber = ADC_OFFSET_NONE;
		AdcChConfig.Offset = 0;
		AdcChConfig.OffsetRightShift = DISABLE;
		AdcChConfig.OffsetSignedSaturation = DISABLE;
		HAL_ADC_ConfigChannel(&hadc2, &AdcChConfig);

		DevStatus[DEVSTATUS_MOTORS_COUNTER_IDX]++;
		DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] = DEVSTATUS_MOTOR_JAMMED;
		DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] = DEVSTATUS_MOTOR_JAMMED;

		// Start motor1 forward
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_SET);
		HAL_Delay(10);
		// Get ADC value
		HAL_ADC_Start(&hadc2);
		while ((ADC2->ISR & ADC_FLAG_EOC) == 0);	// wait end of conversion
		AdcMRef = HAL_ADC_GetValue(&hadc2) & 0xFFFF;
		if (AdcMRef < NC_MOTOR_OPENLOAD_TH)
			DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] = DEVSTATUS_MOTOR_OPENLOAD;
		else
			for (I=0; I<14; I++)
			{
				HAL_Delay(10);
				// Get ADC value
				HAL_ADC_Start(&hadc2);
				while ((ADC2->ISR & ADC_FLAG_EOC) == 0);	// wait end of conversion
				AdcMAct = HAL_ADC_GetValue(&hadc2) & 0xFFFF;
				if (((AdcMAct*100)/AdcMRef) < NC_MOTOR_JAMMED_PERCENTAGE)
				{
					DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] = DEVSTATUS_MOTOR_OK;
					break;
				}
			}
		// Start motor1 backward
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_RESET);
		HAL_Delay(200);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_RESET);

		// Start motor2 forward
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port, MOTOR2_PH_Pin,  GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_SET);
		HAL_Delay(10);
		// Get ADC value
		HAL_ADC_Start(&hadc2);
		while ((ADC2->ISR & ADC_FLAG_EOC) == 0);	// wait end of conversion
		AdcMRef = HAL_ADC_GetValue(&hadc2) & 0xFFFF;
		if (AdcMRef < NC_MOTOR_OPENLOAD_TH)
			DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] = DEVSTATUS_MOTOR_OPENLOAD;
		else
			for (I=0; I<14; I++)
			{
				HAL_Delay(10);
				// Get ADC value
				HAL_ADC_Start(&hadc2);
				while ((ADC2->ISR & ADC_FLAG_EOC) == 0);	// wait end of conversion
				AdcMAct = HAL_ADC_GetValue(&hadc2) & 0xFFFF;
				if (((AdcMAct*100)/AdcMRef) < NC_MOTOR_JAMMED_PERCENTAGE)
				{
					DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] = DEVSTATUS_MOTOR_OK;
					break;
				}
			}
		// Start motor2 backward
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port, MOTOR2_PH_Pin,  GPIO_PIN_RESET);
		HAL_Delay(200);
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_RESET);
	}
}


void MotorTest_PheromoneMix(void)
{
		// Start motor1 forward
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_SET);
		HAL_Delay(100);
		// Start motor1 backward
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_RESET);
		HAL_Delay(200);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_RESET);
}
