/*
 * UART3_CMD_Handler.c
 *
 *  Created on: Sep 12, 2024
 *      Author: DashyFox
 */

#include "UART3_CMD_Handler.h"
#include "Print.h"
#include "RobotFunctions.h"
#include "EEPROM.h"

#define HIGHBIT(b) (((b)>>8)&0xff)
#define LOWBIT(b) ((b)&0xff)

extern CurrentInfo currentInfo;
extern InfoBlock infoBlock;

extern void SendResponse(uint8_t command, uint8_t result, uint8_t *data,
		uint8_t data_length);

uint8_t checkLen(uint8_t cmd, uint8_t current_length, uint8_t required_length) {
	if (current_length < required_length) {
		print("Invalid length for command ");
		printNumber(cmd);
		print("  len = ");
		printNumber(current_length);
		print("\n");
		return 0;
	}
	return 1;
}

// 10
void UART3_SaveShot(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 8;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t shotIndx = dataPtr[1];

	Shot shot;
	shot.isExist = 1;
	shot.countRepeatShot = dataPtr[2];
	shot.speedRollerTop = dataPtr[3] + 100;
	shot.speedRollerBottom = dataPtr[4] + 100;
	shot.speedScrew = map(dataPtr[5], 0, 120, 0, 100);
	shot.rotationAxial = map((int8_t) dataPtr[6], -99, 99, 0, 180);
	shot.rotationHorizontal = map((int8_t) dataPtr[7], -99, 99, 0, 180);
	shot.rotationVertical = 180 - (int8_t) dataPtr[8] - 90;

	saveShot(shotIndx, &shot);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

void UART3_SaveProgram(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 5;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	Program prog;
	uint8_t progIndx = dataPtr[1];
	prog.header.shotCount = (len - 3) / sizeof(ProgramShot);
	prog.header.countRepeat = dataPtr[2];
	prog.header.options = dataPtr[3];

	if (dataPtr[4] != 0xFF && dataPtr[5] != 0xFF) {
		for (uint8_t i = 0; i < prog.header.shotCount; i++) {
			uint8_t pos = 4 + i * sizeof(ProgramShot);
			prog.shots[i].id = dataPtr[pos + 0];
			prog.shots[i].speedScrew = dataPtr[pos + 1];
		}
	} else {
		delProg(progIndx);
	}

	saveProg(progIndx, &prog);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

void UART3_SaveMacro(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 5;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	Macro macro;
	uint8_t macroIndx = dataPtr[1];
	macro.header.programmCount = (len - 1) / sizeof(MacroProgram);

	if (/**/dataPtr[2] != 0xFF && //
			dataPtr[3] != 0xFF && //
			dataPtr[4] != 0xFF && //
			dataPtr[5] != 0xFF) {
		for (uint8_t i = 0; i < macro.header.programmCount; i++) {
			uint8_t pos = 2 + i * sizeof(MacroProgram);
			macro.programs[i].id = dataPtr[pos + 0];
			macro.programs[i].speedScrew = dataPtr[pos + 2];
			macro.programs[i].countRepeat = dataPtr[pos + 3];
			macro.programs[i].options = dataPtr[pos + 4];
		}
	} else {
		delMacro(macroIndx);
	}

	SendResponse(dataPtr[0], 0, NULL, 0);
}

void UART3_StartMacro(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	SendResponse(dataPtr[0], 0, NULL, 0);
}

void UART3_StartProgram(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t progIndx = dataPtr[1];
	prepareProgramm(progIndx);
	startShooting(infoBlock.hwInfo.timings.preRun);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 102
void UART3_StartShot(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t shotIndx = dataPtr[1];
	prepareShot(shotIndx);
	startShooting(infoBlock.hwInfo.timings.preRun);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 110
void UART3_Stop(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	stopShooting();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 13
void UART3_DeleteShot(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t shotIndex = dataPtr[1];
	delShot(shotIndex);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 14
void UART3_DeleteProgram(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t progIndex = dataPtr[1];
	delProg(progIndex);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 15
void UART3_DeleteMacro(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t macroIndex = dataPtr[1];
	delMacro(macroIndex);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 120
void UART3_DeleteAllData(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	getInfoBlock();
	EEPROM_EARSE();
	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 180
void UART3_GetDeviceStatus(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t res = currentInfo.state == RUN;

	SendResponse(dataPtr[0], 0, &res, sizeof(res));
}

// 200
void UART3_SetServoOffset(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 2;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
	uint8_t newDef = dataPtr[2];

	printNumber(newDef);
	print("\n");

	newDef += 90; // from center
	if (newDef > 180)
		newDef = 180;

	currentServo->def = newDef;

	printNumber(newDef);
	print("\n");

	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 205
void UART3_GetServoOffset(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];

	int16_t def = currentServo->def - 90; // offset from center

	uint8_t res[2];
	res[0] = HIGHBIT(def);
	res[1] = LOWBIT(def);
	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 201
void UART3_SetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 3;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	uint16_t maxAngl = (dataPtr[2] << 8) | dataPtr[3];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];

	if (maxAngl > 180)
		maxAngl = 180;
	if (maxAngl < 0)
		maxAngl = 0;
	currentServo->max = maxAngl;

	saveInfoBlock();
	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 202
void UART3_SetServoMinAngle(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 3;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	uint16_t minAngl = (dataPtr[2] << 8) | dataPtr[3];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];

	if (minAngl > 180)
		minAngl = 180;
	if (minAngl < 0)
		minAngl = 0;
	currentServo->min = minAngl;

	saveInfoBlock();
	SendResponse(dataPtr[0], 0, NULL, 0);
}

void UART3_GetServoMaxAngle(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];

	uint8_t maxAngl = currentServo->max;

	uint8_t res[2];
	res[0] = HIGHBIT(maxAngl);
	res[1] = LOWBIT(maxAngl);
	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

void UART3_GetServoMinAngle(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	ServoMap servo = dataPtr[1];
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];

	uint8_t minAngl = currentServo->min;

	uint8_t res[2];
	res[0] = HIGHBIT(minAngl);
	res[1] = LOWBIT(minAngl);
	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 203
void UART3_MoveServoToInitialPosition(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	setPosDefaultSingle(dataPtr[1]);

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 206
void UART3_SetStartupDelay(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 2;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	infoBlock.hwInfo.timings.preRun = (dataPtr[1] << 8) | dataPtr[2];
	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 207
void UART3_GetStartupDelay(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t res[2];

	res[0] = HIGHBIT(infoBlock.hwInfo.timings.preRun);
	res[1] = LOWBIT(infoBlock.hwInfo.timings.preRun);

	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 210 !!!!!!!!
void UART3_SetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 211 !!!!!!!!!!!
void UART3_GetMinRollerSpeed(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t res[1];
//		res[0] = ;

	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 212
void UART3_SetMinScrewSpeed(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	infoBlock.hwInfo.motors.speed_Screw_min = dataPtr[1];
	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 215
void UART3_GetMinScrewSpeed(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t res[1];
	res[0] = infoBlock.hwInfo.motors.speed_Screw_min;

	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 214
void UART3_SetServoInvertFlag(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 1;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t inv = dataPtr[1];

	infoBlock.hwInfo.servos[SERVO_AXIAL].invert = inv;
	infoBlock.hwInfo.servos[SERVO_HORIZONTAL].invert = inv;
	infoBlock.hwInfo.servos[SERVO_VERTICAL].invert = inv;

	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}

// 215
void UART3_GetServoInvertFlag(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	uint8_t res[1];

	res[0] = (infoBlock.hwInfo.servos[SERVO_AXIAL].invert
			|| infoBlock.hwInfo.servos[SERVO_HORIZONTAL].invert
			|| infoBlock.hwInfo.servos[SERVO_VERTICAL].invert);

	SendResponse(dataPtr[0], 0, res, sizeof(res));
}

// 181
void UART3_ReadStatistics(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	typedef struct __attribute__((packed)) {
		uint8_t status;
		uint8_t macro_number;
		uint8_t program_number;
		uint8_t shot_number;
		uint8_t total_macro_done_LOW;
		uint8_t total_macro_done_HIGH;
		uint8_t total_program_done_LOW;
		uint8_t total_program_done_HIGH;
		uint8_t total_shot_done_LOW;
		uint8_t total_shot_done_HIGH;
	} StatusStruct;

	StatusStruct res;
	uint8_t isRun = currentInfo.state == RUN;
	res.status = isRun;

	res.shot_number = 0xFF;
	res.program_number = 0xFF;
	res.macro_number = 0xFF;

	switch (currentInfo.mode) {
	case ShotMode:
		res.shot_number = isRun ? currentInfo.shot.index : 0xFF;
		break;
	case ProgramMode:
		res.program_number = isRun ? currentInfo.program.index : 0xFF;
		break;
	case MacroMode:
		res.macro_number = isRun ? currentInfo.macro.index : 0xFF;
		break;
	default:
		break;
	}

	res.total_shot_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInShot);
	res.total_shot_done_LOW = LOWBIT(infoBlock.statInfo.shotsInShot);

	res.total_program_done_HIGH = HIGHBIT(infoBlock.statInfo.shotsInProgram);
	res.total_program_done_LOW = LOWBIT(infoBlock.statInfo.shotsInProgram);

	res.total_macro_done_HIGH = HIGHBIT(infoBlock.statInfo.totalMacros);
	res.total_macro_done_LOW = LOWBIT(infoBlock.statInfo.totalMacros);

	SendResponse(dataPtr[0], 0, (uint8_t*) &res, sizeof(res));
}

// 121
void UART3_ResetStatistics(uint8_t *dataPtr, uint8_t len) {
	const uint8_t MIN_PARAM_LENGTH = 0;
	if (!checkLen(dataPtr[0], len, MIN_PARAM_LENGTH))
		return;

	memset(&infoBlock.statInfo, 0x00, sizeof(infoBlock.statInfo));
	saveInfoBlock();

	SendResponse(dataPtr[0], 0, NULL, 0);
}