/*
 * 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 "Print.h"
#include "SimpleTimer.h"
#include "Indicator.h"
#include "SoundMelody.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;
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 pauseShooting();
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();
}
void pauseShooting() {
	currentInfo.state = PAUSE;
	led_clear();
	setScrewkSpeed(0);
	setRollersSpeed(100, 100);

}

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() {
	led_init();
//	led_PingPong_start(7, 12);
	memset(&currentInfo, 0x00, sizeof(currentInfo));
	// NULL
	currentInfo.shot.indexGlobal = 0xFF;
	currentInfo.program.indexGlobal = 0xFF;
	currentInfo.macro.indexGlobal = 0xFF;
	initPCA9685();
	EEPROM_INIT();
	setPosDefault();
	UART3_START();
//	sound_init();

	for (int i = 0; i < 10; ++i) {
//		if ((i&1U)!=1 || i > 4) for test
		{
			led_writeMirror(i, 1);
			HAL_Delay(10);
		}
	}
	led_clear();
	melody(melody_start);
}

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

void RobotTick() {
	BallEXT_Handler();
	led_tick();
	sound_tick();

	// No Ball Handler
	if (currentInfo.state == RUN && millis() - noBallTimer > noBallTimeout) {
		if(currentInfo.mode == DebugShot){
			currentInfo.mode = ShotMode;
		}
		robotStateStop();

		melody(melody_NoBall);
		led_clear();
		for (int i = 7; i <= 12; ++i) {
			led_blink_num(i, 250, 0xFFFF - 1);
		}
		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 (currentInfo.state == PRERUN_WAIT) {
		uint32_t elapsedTime = millis() - startDelay_timer;

		if (elapsedTime > currentInfo.startDelay) {
				currentInfo.state = RUN;
//				melody(melody_start);
				shotApply(postDelayShot);

		} else {
			uint32_t intervalStep = currentInfo.startDelay / NUMLEDS;
			if (elapsedTime - lastIndicationTime >= intervalStep) {
				lastIndicationTime = elapsedTime;
				uint8_t progress = 100
						- (elapsedTime * 100) / currentInfo.startDelay;

				uint8_t ledProgress = map(progress, 0, 70, 0, 100);
				if (ledProgress > 30) {
					led_progressbar(9, 0, progress);
					led_progressbar(10, 19, progress);
					melody(melody_timerClick);
				} else {
					led_progressbar(9, 0, 100);
					led_progressbar(10, 19, 100);
					melody(melody_start);
				}
				print("delay: ");
				printNumber(progress);
				print("\n");

			}
		}
	}
}

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

uint8_t prepareMacro(uint8_t number) {
	stopShooting();
	Macro macro;
	memset(&macro, 0x00, sizeof(macro));
	getMacro(number, &macro);
	if (macro.header.programmCount) {
		currentInfo.mode = MacroMode;
		currentInfo.macro.macro = macro;
		currentInfo.macro.indexGlobal = number;
		melody(melody_OK);
		return 1;
	} else {
		melody(melody_Error);
		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;
		}

		melody(melody_Ball_1);

		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;
		case DebugShot:
			currentInfo.mode = ShotMode;
			led_clear();
			robotStateStop();
			setScrewkSpeed(0);
			setRollersSpeed(100, 100);
			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 {
			currentProg->doneCount++;
			*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;
	}
	MacroProgram *macroProgram =
			&currentMacro->macro.programs[currentProgIndexLocal];

	uint8_t programId = macroProgram->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 = macroProgram->countRepeat != 0
			&& macroProgram->speedScrew != 0;

	if (needOverride) {
		print("Override programs params\n");
		program_.header.countRepeat = macroProgram->countRepeat;

		// options override(???)
		uint8_t options = macroProgram->options;
		program_.header.options = options;

		for (uint8_t i = 0; i < program_.header.shotCount; i++) {
			program_.shots[i].speedScrew = macroProgram->speedScrew;
		}
	}
	currentMacro->currentProgram.program = program_;
	currentMacro->currentProgram.currentShotIndexLocal = 0;
	currentMacro->currentProgram.currentBallCount = 0;

	loadShotFromProgram(&currentMacro->currentProgram);
	return 1;
}

uint8_t loadNextProgramInMacro(CurrentMacro *currentMacro) {
	if (currentMacro->currentProgramIndexLocal + 1
			< currentMacro->macro.header.programmCount) {
		currentMacro->currentProgramIndexLocal++;
	} else {
		currentMacro->currentProgramIndexLocal = 0;
		currentMacro->doneCount++;
	}
	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");
		shot->doneCount++;
		return 1;
	}
}

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

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

uint8_t nextProgramInMacro(CurrentMacro *currentMacro) {
	uint8_t shotApply_f = 0;
	if (nextShotInProgram(&currentMacro->currentProgram)) {
		print("\nSwitch program\n");
		if (!loadNextProgramInMacro(currentMacro)) {
			print("loadNextProgramInMacro ERR\n");
			return 0;
		}
		shotApply_f = 1;
	}

	// Проверка на завершение макро
	if (currentMacro->doneCount) {
		print("Macro DONE\n");
		return 1;
	}

	if (shotApply_f)
		shotApply(&currentMacro->currentProgram.currentShot);
	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() {
	if(currentInfo.mode == DebugShot){
		currentInfo.mode = ShotMode;
	}

	if(currentInfo.state == RUN){
		melody(melody_Done);
	}

	robotStateStop();

	led_clear();

	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();

////////////////////////// LED ////////////////////////////////////////
	led_clear();
	led_showSpeed(shot->speedRollerTop, shot->speedRollerBottom);
/////////////////////////////////////////////////////////////////////////////

	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) {
	uint16_t min_speed_rpm = map(infoBlock.hwInfo.motors.speed_Rollers_min, 0, 200, 400, 800);
	uint16_t min_speed_pwm_positive = map(min_speed_rpm, 0, 8000, 100, 200);
	uint16_t min_speed_pwm_negative = 100 - map(min_speed_rpm, 0, 8000, 0, 100);

    if (up == 100) {
        up = 100;
    } else if (up > 100) {

        if (up < min_speed_pwm_positive) {
            up = min_speed_pwm_positive;
        }
    } else {
        if (up > min_speed_pwm_negative) {
            up = min_speed_pwm_negative;
        }
    }

    if (down == 100) {
        down = 100;
    } else if (down > 100) {
        if (down < min_speed_pwm_positive) {
            down = min_speed_pwm_positive;
        }
    } else {
        if (down > min_speed_pwm_negative) {
            down = min_speed_pwm_negative;
        }
    }

    Vz1 = 200 - up; // Инвертируем значение скорости для одного из моторов
    Vz2 = down;
}