/*
 * RobotFunctions.c
 *
 *  Created on: Aug 28, 2024
 *      Author: DashyFox
 */
#include <math.h>
#include "RobotFunctions.h"
#include "pca9685.h"
#include "UART3_Handler.h"
#include "EEPROM.h"
#include "ShiftReg.h"
#include "Print.h"
#include "SimpleTimer.h"

#define ballReact_value 10 // время реакции на вылет мяча

CurrentInfo currentInfo;

extern int16_t Vz1;
extern int16_t Vz2;
unsigned char Shiftreg[3];
extern InfoBlock infoBlock;
uint8_t ballDetected = 0;
uint32_t ballReact_timer = 0;
uint8_t isDelayTimerRun = 0;
uint32_t startDelay_timer;
CurrentShot *postDelayShot = NULL;
uint32_t lastIndicationTime = 0;
uint32_t noBallTimer = 0;
uint32_t noBallTimeout = 0xFFFFFFFF;

void BallEXT_Handler();
void robotStateStart(uint32_t delayTime);
void robotStatePause();
void robotStateStop();
float calculatePeriod(int pwm_value);
uint8_t nextBallInShot(CurrentShot *shot);
uint8_t nextProgramInMacro(CurrentMacro *macro);
uint8_t nextShotInProgram(CurrentProgram *prog);
uint8_t loadNextShotInProgram(CurrentProgram *currentProg);
uint8_t loadShotFromProgram(CurrentProgram *currentProg);
uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro);
uint8_t loadProgramFromMacro(CurrentMacro *currentMacro);

void robotStateStart(uint32_t delayTime) {
	currentInfo.state = PRERUN_WAIT;
	currentInfo.startDelay = delayTime;
	startDelay_timer = millis();
	isDelayTimerRun = 1;
}
void robotStatePause() {
	currentInfo.state = PAUSE;
}

void robotStateStop() {
	currentInfo.state = STOP;
}

long map(long x, long in_min, long in_max, long out_min, long out_max) {
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
float calculatePeriod(int pwm_value) {// Функция для расчета периода вылета мяча на основе ШИМ
	// Коэффициенты из аппроксимации
	float a = 100382.255;
	float b = 0.21895;
	float c = 883.456;

	// Расчет периода на основе экспоненциальной формулы
	float period = a * expf(-b * pwm_value) + c;
	return period;
}

void Robot_INIT() {
	memset(&currentInfo, 0x00, sizeof(currentInfo));
	// NULL
	currentInfo.shot.indexGlobal = 0xFF;
	currentInfo.program.indexGlobal = 0xFF;
	currentInfo.macro.indexGlobal = 0xFF;
	initPCA9685();
	EEPROM_INIT();
	UART3_START();

	setPosDefault();
	Shiftreg[0] = 0x00;
	Shiftreg[1] = 0x44;
	Shiftreg[2] = 0x00;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x00;
	Shiftreg[1] = 0x66;
	Shiftreg[2] = 0x00;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x00;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x00;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x01;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x01;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x03;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x03;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x07;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x07;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x0F;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x0F;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x1F;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x1F;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x3F;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x3F;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x7F;
	Shiftreg[1] = 0x77;
	Shiftreg[2] = 0x7F;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);
	Shiftreg[0] = 0x00;
	Shiftreg[1] = 0x00;
	Shiftreg[2] = 0x00;
	SetShiftReg(Shiftreg);
	HAL_Delay(10);

	//testing

}

void BallEXT() {
	ballDetected = 1;
	ballReact_timer = millis();
}


void RobotTick() {
	BallEXT_Handler();

	// No Ball Handler
	if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) {
		robotStateStop();
		setScrewkSpeed(0);
		setRollersSpeed(100, 100);
		setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def,
				infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def,
				infoBlock.hwInfo.servos[SERVO_VERTICAL].min);
		print("NO BALL!!!");
	}

	// PreRun delay
	if (isDelayTimerRun) {
		uint32_t elapsedTime = millis() - startDelay_timer;

		if (elapsedTime > currentInfo.startDelay) {
			isDelayTimerRun = 0;
			if (currentInfo.state == PRERUN_WAIT) {
				currentInfo.state = RUN;
				shotApply(postDelayShot);
			}
		} else {
			uint32_t intervalStep = currentInfo.startDelay / NUMLEDS;
			if (elapsedTime - lastIndicationTime >= intervalStep) {
				lastIndicationTime = elapsedTime;
				uint8_t progress = (elapsedTime * 100) / currentInfo.startDelay;
//                indicate(progress);
				print("delay: ");
				printNumber(progress);
				print("\n");
			}
		}
	}
}

uint8_t prepareShot(uint8_t number) {
	Shot shot;
	getShot(number, &shot);
	if (shot.isExist) {
		currentInfo.mode = ShotMode;
		currentInfo.shot.shot = shot;
		currentInfo.shot.currentBallCount = 0;
		return 1;
	} else {
		// TODO: sound_ERR(); ledFX_ERR();
		print("shot not exist\n\n");
		return 0;
	}
}
uint8_t prepareProgramm(uint8_t number) {
	Program program;
	getProg(number, &program);
	if (program.header.shotCount) { // isExist
		currentInfo.mode = ProgramMode;
		currentInfo.program.program = program;
		currentInfo.program.indexGlobal = number;
		currentInfo.program.currentShotIndexLocal = 0;
		currentInfo.program.currentBallCount = 0;
		return 1;
	} else {
		// TODO: sound_ERR(); ledFX_ERR();
		print("program not exist\n\n");
		return 0;
	}
}

uint8_t prepareMacro(uint8_t number) {
	Macro macro;
	getMacro(number, &macro);
	if (macro.header.programmCount) {
		currentInfo.mode = MacroMode;
		currentInfo.macro.macro = macro;
		currentInfo.macro.indexGlobal = number;
		currentInfo.macro.currentProgramIndexLocal = 0;
		currentInfo.macro.currentBallCount = 0;

		return 1;
	} else {
		print("Macro not exist ");
		printNumber(number);
		print("\n");

		return 0;
	}
}

void BallEXT_Handler() {
	if (ballDetected && millis() - ballReact_timer > ballReact_value) {
		ballDetected = 0;

		if (currentInfo.state != RUN) {
			print("BallDetected on idle\n");
			return;
		}

		print("BallDetected ");
		uint16_t period = ballReact_timer - noBallTimer;
		printNumber(period);
		print("ms\n");
		noBallTimer = ballReact_timer;

		switch (currentInfo.mode) {
		case NoneMode:

			break;
		case ShotMode:
			print("ShotMode\n");
			if (nextBallInShot(&currentInfo.shot)) {
				stopShooting();
			}
			infoBlock.statInfo.shotsInShot++;
			saveInfoBlock();
			break;
		case ProgramMode:
			print("ProgramMode\n");
			if (nextShotInProgram(&currentInfo.program)) {
				stopShooting();
			}
			infoBlock.statInfo.shotsInProgram++;
			saveInfoBlock();
			break;
		case MacroMode:
			print("MacroMode\n");
			if (nextProgramInMacro(&currentInfo.macro)) {
				stopShooting();
			}
			infoBlock.statInfo.shotInMacro++;
			saveInfoBlock();
			break;
		default:
			break;
		}

	}
}

uint8_t loadShotFromProgram(CurrentProgram *currentProg) {
	Shot shot_;
	uint8_t currentShotIndexLocal = currentProg->currentShotIndexLocal;
	if (currentShotIndexLocal >= currentProg->program.header.shotCount) {
		print("Shot indexGlobal out of range\n");
		// TODO: sound_ERR(); ledFX_ERR();
		return 0;
	}
	uint8_t shotId = currentProg->program.shots[currentShotIndexLocal].id;
	currentProg->currentShot.indexGlobal = shotId;
	getShot(shotId, &shot_);
	if (!shot_.isExist) {
		print("Shot in program is NULL\n");
		// TODO: sound_ERR(); ledFX_ERR();
		return 0;
	}

	uint8_t overridenSpeedScrew =
			currentProg->program.shots[currentShotIndexLocal].speedScrew;
	if (overridenSpeedScrew != 0) {
		shot_.speedScrew = overridenSpeedScrew;
	}

	uint8_t repeatCountFromShot = currentProg->program.header.options & 2U;
	if (!repeatCountFromShot) {
		print("Repeat Count Override\n");
		shot_.countRepeatShot = 1;
	}else if(shot_.countRepeatShot == 0 && currentInfo.mode != ShotMode){
		shot_.countRepeatShot = 1;
	}
	currentProg->currentShot.shot = shot_;
	currentProg->currentShot.currentBallCount = 0;
	return 1;
}

uint8_t loadNextShotInProgram(CurrentProgram *currentProg) {
	uint8_t random = currentProg->program.header.options & 1U;
	uint8_t *currentProgramShotId = &currentProg->currentShotIndexLocal;

	if (random) {
		*currentProgramShotId = rand() % currentProg->program.header.shotCount;
	} else {
		if ((*currentProgramShotId) + 1
				< currentProg->program.header.shotCount) {
			++(*currentProgramShotId);
		} else {
			*currentProgramShotId = 0;
		}
	}
	return loadShotFromProgram(currentProg);
}

uint8_t loadProgramFromMacro(CurrentMacro *currentMacro) {
	Program program_;
	uint8_t currentProgIndexLocal = currentMacro->currentProgramIndexLocal;
	if (currentMacro->currentProgramIndexLocal
			>= currentMacro->macro.header.programmCount) {
		print("Program indexGlobal out of range in currentMacro\n");
		return 0;
	}


	uint8_t programId = currentMacro->macro.programs[currentProgIndexLocal].id;
	currentMacro->currentProgram.indexGlobal = programId;
	getProg(programId, &program_);

	if (program_.header.shotCount == 0) {
		print("Program in macro is NULL\n");
		// TODO: sound_ERR(); ledFX_ERR();
		return 0;
	}

	uint8_t needOverride =
			currentMacro->macro.programs[currentProgIndexLocal].countRepeat != 0
					&& currentMacro->macro.programs[currentProgIndexLocal].speedScrew
							!= 0;

	if (needOverride)
	{
		print("Override programs params\n");
		program_.header.countRepeat = currentMacro->macro.programs[currentProgIndexLocal].countRepeat;
		program_.header.options = currentMacro->macro.programs[currentProgIndexLocal].options;
		for (uint8_t i = 0; i < program_.header.shotCount; i++) {
			program_.shots[i].speedScrew =
					currentMacro->macro.programs[currentProgIndexLocal].speedScrew;
		}
	}
	currentMacro->currentProgram.program = program_;
	currentMacro->currentProgram.currentShotIndexLocal = 0;
	currentMacro->currentProgram.currentBallCount = 0;

	return 1;
}

uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro) {
	if (currentMacro->currentProgramIndexLocal + 1
			< currentMacro->macro.header.programmCount) {
		currentMacro->currentProgramIndexLocal++;
	} else {
		currentMacro->currentProgramIndexLocal = 0;
	}
	return loadProgramFromMacro(currentMacro);
}

uint8_t nextBallInShot(CurrentShot *shot) {

	print("shot->shot.countRepeatShot ");
	printNumber(shot->shot.countRepeatShot);
	print("\n");

	if (shot->currentBallCount + 1 < shot->shot.countRepeatShot
			|| !shot->shot.countRepeatShot) {
		// nextBall
		print("Shot ");
		printNumber(shot->currentBallCount);
		print("\n\n");

		shot->currentBallCount++;
		return 0;
	} else {
		print("Shot DONE\n");
		return 1;
	}
}


uint8_t nextShotInProgram(CurrentProgram *prog) {
	if (nextBallInShot(&prog->currentShot)) {
		print("\nSwitch shot\n");
		if (!loadNextShotInProgram(prog)) {
			print("loadNextShotInProgram ERR\n");
			return 0;
		}
		shotApply(&prog->currentShot);
	}

	// Проверка на завершение программы
	if (prog->currentBallCount + 1 < prog->program.header.countRepeat
			|| prog->program.header.countRepeat == 0) {
		prog->currentBallCount++;
	} else {
		print("Program DONE\n");
		return 1;
	}

	return 0;
}

uint8_t nextProgramInMacro(CurrentMacro *currentMacro) {
	if (nextShotInProgram(&currentMacro->currentProgram)) {
		print("\nSwitch program\n");
		if (!loadNextProgramInMacro(currentMacro)) {
			print("loadNextProgramInMacro ERR\n");
			return 0;
		}
//		shotApply(&currentMacro->currentProgram.currentShot);
	}

	// Проверка на завершение макро
//	if (currentMacro->currentProgramIndexLocal < currentMacro->macro.header.programmCount) {
//	} else {
//		print("Macro DONE\n");
//		return 1;
//	}
	return 0;
}

void startShooting(uint32_t delayTime) {
	print("StartShooting\n");
	switch (currentInfo.mode) {
	case ShotMode:
		if (currentInfo.shot.shot.isExist) {
			postDelayShot = &currentInfo.shot;
			if (PRE_RUN_DELAY_MODE) {
				setPosFromShot(&postDelayShot->shot);
			}
			robotStateStart(delayTime);
		} else {
			print("Current Shot is NULL\n");
			// TODO: sound_ERR(); ledFX_ERR();
		}
		break;
	case ProgramMode:
		if (currentInfo.program.program.header.shotCount) {
			if (!loadShotFromProgram(&currentInfo.program)) {
				print("loadShotFromProgram ERR\n");
				break;
			}
			postDelayShot = &currentInfo.program.currentShot;
			if (PRE_RUN_DELAY_MODE) {
				setPosFromShot(&postDelayShot->shot);
			}
			robotStateStart(delayTime);

		} else {
			print("Current Program is NULL\n");
			// TODO: sound_ERR(); ledFX_ERR();
		}
		break;
	case MacroMode:
		if (currentInfo.macro.macro.header.programmCount) {
			if (!loadProgramFromMacro(&currentInfo.macro)) {
				print("loadProgramFromMacro ERR\n");
				break;
			}
			if (!loadShotFromProgram(&currentInfo.macro.currentProgram)) {
				print("loadShotFromProgram in macro ERR\n");
				break;
			}

			postDelayShot = &currentInfo.macro.currentProgram.currentShot;
			if (PRE_RUN_DELAY_MODE) {
				setPosFromShot(&postDelayShot->shot);
			}
			robotStateStart(delayTime);

		} else {
			print("Current Macro is NULL\n");
			// TODO: sound_ERR(); ledFX_ERR();
		}
		break;
	default:
		break;
	}
}

void stopShooting() {
	robotStateStop();
	setScrewkSpeed(0);
	setRollersSpeed(100, 100);
	setPosDefault();
}

void shotApply(CurrentShot *currentShot) {
	Shot* shot = &currentShot->shot;
	setPos(shot->rotationAxial, shot->rotationHorizontal,
			shot->rotationVertical);
	setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
	setScrewkSpeed(shot->speedScrew);
	noBallTimeout = MIN(
			calculatePeriod( shot->speedScrew) * NOBALL_TIMEOUT_MULTIPLIER,
			30000);
	noBallTimer = millis();
	print("Global indx           ");
	printNumber(currentShot->indexGlobal);
	print("\nisExist               ");
	printNumber(shot->isExist);
	print("\ncountRepeatShot;    ");
	printNumber(shot->countRepeatShot);
	print("\nspeedRollerTop;     ");
	printNumber(shot->speedRollerTop);
	print("\nspeedRollerBottom;  ");
	printNumber(shot->speedRollerBottom);
	print("\nspeedScrew;         ");
	printNumber(shot->speedScrew);
	print("\nrotationAxial;      ");
	printNumber(shot->rotationAxial);
	print("\nrotationHorizontal; ");
	printNumber(shot->rotationHorizontal);
	print("\nrotationVertical;   ");
	printNumber(shot->rotationVertical);
	print("\nnoBallTimeout: ");
	printNumber(noBallTimeout);
	print("\n");
	print("\n\n");
}

void setPosSingle(ServoMap servo, uint8_t value) {
	ServoSetting *currentServo = &infoBlock.hwInfo.servos[servo];
	uint8_t inv = currentServo->invert;
	if (servo == SERVO_AXIAL)
		inv = !inv;

	if (inv)
		value = 180 - value;

	if (value > 90) {
		value = map(value, 91, 180, currentServo->def, currentServo->max);
	} else if (value < 90) {
		value = map(value, 89, 0, currentServo->def, currentServo->min);
	} else {
		value = currentServo->def;
	}

	SetServo(servo, value);
}

void setPos(uint8_t axial, uint8_t horizontal, uint8_t vertical) {
	setPosSingle(SERVO_AXIAL, axial);
	setPosSingle(SERVO_HORIZONTAL, horizontal);
	setPosSingle(SERVO_VERTICAL, vertical);
}

void setPosFromShot(Shot *shot) {
	setPos(shot->rotationAxial, shot->rotationHorizontal,
			shot->rotationVertical);
}

void setPosDefault() {
	setPos(infoBlock.hwInfo.servos[SERVO_AXIAL].def,
			infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def,
			infoBlock.hwInfo.servos[SERVO_VERTICAL].def);
}
void setPosDefaultSingle(ServoMap servo) {
	setPosSingle(servo, infoBlock.hwInfo.servos[servo].def);
}

// 0 .. 100
void setScrewkSpeed(uint8_t speed) {

//	if(speed < 0) speed = 0;
	if (speed > 100)
		speed = 100;

//	speed = map(speed, 0, 100, infoBlock.hwInfo.motors.speed_Screw_min, 100);
	if (speed && speed < infoBlock.hwInfo.motors.speed_Screw_min)
		speed = infoBlock.hwInfo.motors.speed_Screw_min;

	TIM1->CCR1 = 0;
	TIM1->CCR2 = (uint16_t) (40 * speed);
}

//(-v) 0 .. 100(stop) .. 200(+v)
void setRollersSpeed(uint8_t up, uint8_t down) {

	if (up < 100) {
//		up = map(up, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);
//		if (100 - up < min_speed) {
//			up = 100 - min_speed;
//		}
	} else {
//		up = map(up, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);
//		if (up - 100 < min_speed) { // Ограничиваем положительную скорость минимальной
//			up = 100 + min_speed;
//		}
	}

	if (down < 100) {
//		map(down, 0, 100, 0, 100 - infoBlock.hwInfo.motors.speed_Screw_min);

	} else {
//		map(down, 0, 100, 0, 100 + infoBlock.hwInfo.motors.speed_Screw_min);

	}

	Vz1 = 200 - up; // invert
	Vz2 = down;
}