/*
 * 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 I2C_HandleTypeDef hi2c1;
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::print_roll_pitch_yaw() {
	static char printbuf[128];
	sprintf(printbuf, "X %f, Y %f, Z %f", imu0.getAccX(), imu0.getAccY(), imu0.getAccZ());
	debug.debug(printbuf);
}

void Robot::init(){
	debug.setLevel(SerialDebug::DEBUG_LEVEL_DEBUG);
	debug.info("Init DWT begin");
	CoreDebug->DEMCR |= 0x01000000; // TRCENA
	DWT->CYCCNT = 0; // reset the counter
	DWT->CTRL |= 1 ; // enable the counter
	debug.info("Init DWT end");
	debug.info("Init IMU begin");
	imu0.setup(0x68, MPU9250Setting(), &hi2c1);
	imu0.calibrateAccelGyro();
	debug.info("Init IMU end");
	debug.info("Init ESP8266 begin");
	HAL_Delay(10000);
	debug.info("Init ESP8266 end");
	motor0.setEncoder(&encoder0);
	motor1.setEncoder(&encoder1);
	motor0.kp = 1.3016/CONVERSION;
	motor0.ki = 22.3758/(ticksPerSecond*CONVERSION);
	motor0.kd = 0*ticksPerSecond/CONVERSION;
	motor1.kp = 1.3016/CONVERSION;
	motor1.ki = 22.3758/(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);
	//Begin gambiarrra
	/*while(1){
		if (imu0.update()) {
			static uint32_t prev_ms = HAL_GetTick();
			if (HAL_GetTick() > prev_ms + 100) {
				print_roll_pitch_yaw();
				prev_ms = HAL_GetTick();
			}
		}
	}*/
	//End gambiarra
}