/*
 * 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;
Shot* postDelayShot = NULL;
uint32_t lastIndicationTime = 0;
uint32_t noBallTimer = 0;
uint32_t noBallTimeout = 0xFFFFFFFF;

void BallEXT_Handler();

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;
}

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


// Функция для расчета периода вылета мяча на основе ШИМ
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 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");
            }
        }
    }
}

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:
			if (currentInfo.shot.currentRepeatCount + 1
					< currentInfo.shot.shot.countRepeatShot
					|| !currentInfo.shot.shot.countRepeatShot) {
				// nextRepeatCount
				print("Shot ");
				printNumber(currentInfo.shot.currentRepeatCount);
				print("\n\n");
				saveInfoBlock();
				currentInfo.shot.currentRepeatCount++;
			} else {
				// shotDone
				stopShooting();
				print("Shot DONE\n");
			}
			infoBlock.statInfo.totalShots++;
			break;
		case ProgramMode:

			break;
		case MacroMode:

			break;
		default:
			break;
		}

	}
}

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;
}
void Robot_INIT() {
	memset(&currentInfo, 0x00, sizeof(currentInfo));
	// NULL
	currentInfo.shot.index = 0xFF;
	currentInfo.program.index = 0xFF;
	currentInfo.macro.index = 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 shotApply(Shot *shot) {
	setPos(shot->rotationAxial, shot->rotationHorizontal,
			shot->rotationVertical);
	setRollersSpeed(shot->speedRollerTop, shot->speedRollerBottom);
	setScrewkSpeed(shot->speedScrew);
	noBallTimer = millis();
}

void startShooting(uint32_t delayTime) {
	switch (currentInfo.mode) {
	case ShotMode:
		print("StartShooting\n");
		if (currentInfo.shot.shot.isExist) {
			print("Fire!\n");

			print("isExist             ");
			printNumber(currentInfo.shot.shot.isExist);
			print("\ncountRepeatShot;    ");
			printNumber(currentInfo.shot.shot.countRepeatShot);
			print("\nspeedRollerTop;     ");
			printNumber(currentInfo.shot.shot.speedRollerTop);
			print("\nspeedRollerBottom;  ");
			printNumber(currentInfo.shot.shot.speedRollerBottom);
			print("\nspeedScrew;         ");
			printNumber(currentInfo.shot.shot.speedScrew);
			print("\nrotationAxial;      ");
			printNumber(currentInfo.shot.shot.rotationAxial);
			print("\nrotationHorizontal; ");
			printNumber(currentInfo.shot.shot.rotationHorizontal);
			print("\nrotationVertical;   ");
			printNumber(currentInfo.shot.shot.rotationVertical);
			print("\n\n");
			postDelayShot = &currentInfo.shot.shot;
#if PRE_RUN_DELAY_MODE == 0
#elif PRE_RUN_DELAY_MODE == 1
			setPosFromShot(postDelayShot);
#endif
			robotStateStart(delayTime);
		} else {
			print("Current Shot is NULL\n");
			// TODO: sound_ERR(); ledFX_ERR();
		}
		break;
	case ProgramMode:

		break;
	case MacroMode:

		break;
	default:
		break;
	}
}

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

uint8_t prepareShot(uint8_t number) {
	Shot shot;
	getShot(number, &shot);
	if (shot.isExist) {
		currentInfo.mode = ShotMode;
		currentInfo.shot.shot = shot;
		currentInfo.shot.currentRepeatCount = 0;
		noBallTimeout = calculatePeriod(shot.speedScrew)*NOBALL_TIMEOUT_MULTIPLIER;
		print("noBallTimeout: ");
		printNumber(noBallTimeout);
		print("\n");
		return 1;
	} else {
		// TODO: sound_ERR(); ledFX_ERR();
		print("shot not exist\n\n");
		return 0;
	}
}

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;
}