/*
 * Robot.cpp
 *
 *  Created on: Mar 17, 2023
 *      Author: Gabriel
 */

#include "Robot.hpp"
#include <cstdlib>
#include "SerialDebug.hpp"
#include "Definitions.hpp"

#define CONVERSION (reductionRatio/(ticksPerSecond*2*3.14159*wheelRadius))

extern UART_HandleTypeDef huart1;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
extern SerialDebug debug;

uint8_t buf[64];

Robot::Robot() {

}

Robot::~Robot() {

}

void Robot::uartCallback(){
	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
}

void Robot::controlCallback(){
	HAL_GPIO_TogglePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin);
	//float desiredSpeed0 = strtof((const char*)rxBuffer, NULL);
	float desiredSpeed0;
	float desiredSpeed1;
	sscanf((char*)rxBuffer, "%f,%f", &desiredSpeed0, &desiredSpeed1);	// Desired speed in mm/s
	float convertedSpeed0 = desiredSpeed0*CONVERSION;	// Converted speed in rev/tick
	float convertedSpeed1 = desiredSpeed1*CONVERSION;
#ifdef CONTROL_DISABLED
	motor0.setPower(desiredSpeed0);
	motor1.setPower(desiredSpeed1);
#else
	motor0.pid(convertedSpeed0);
	motor1.pid(convertedSpeed1);
#endif
	sprintf((char*)buf, ">Speed:%+0.6e,%+0.6e,%+0.6e,%+0.6e",
			motor0.getCurrentRevPerTick()/CONVERSION,
			motor1.getCurrentRevPerTick()/CONVERSION,
			desiredSpeed0,
			desiredSpeed1);
	HAL_UART_Transmit_DMA(&huart1, buf, 64);
}

void Robot::init(){
	debug.info("Init ESP8266 begin");
	HAL_Delay(10000);
	debug.info("Init ESP8266 end");
	motor0.setEncoder(&encoder0);
	motor1.setEncoder(&encoder1);
	motor0.kp = 0.0017198/CONVERSION;
	motor0.ki = 0.04662/(ticksPerSecond*CONVERSION);
	motor0.kd = 0*ticksPerSecond/CONVERSION;
	motor1.kp = 0.0017198/CONVERSION;
	motor1.ki = 0.04662/(ticksPerSecond*CONVERSION);
	motor1.kd = 0*ticksPerSecond/CONVERSION;
	debug.info("Init timers begin");
	//HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
	//HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
	HAL_TIM_Base_Start(&htim2);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
	HAL_TIM_Base_Start_IT(&htim4);
	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
	debug.info("Init timers end");
	motor0.setEnabled(true);
	motor1.setEnabled(true);
}