/*
 * RobotFunctions.c
 *
 *  Created on: Aug 28, 2024
 *      Author: DashyFox
 */

#include "RobotFunctions.h"
#include "pca9685.h"
#include "UART3_Handler.h"
#include "EEPROM.h"
#include "ShiftReg.h"
#include "Print.h"

uint8_t isPause = 0;
uint8_t isShooting = 0;

CurrentInfo currentInfo;

extern int16_t Vz1;
extern int16_t Vz2;
unsigned char Shiftreg[3];

int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min,
		int16_t 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 doShot(Shot *shot) {
	SetServo(0, shot->rotationHorizontal);
	SetServo(1, shot->rotationVertical);
	SetServo(2, shot->rotationAxial);
	Vz1 = shot->speedRollerTop;
	Vz2 = shot->speedRollerBottom;
	setScrewkSpeed(shot->speedScrew);
}

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

			print("isExist             ");
			printNumber(currentInfo.shot.shot.isExist);
			print("countRepeatShot;    ");
			printNumber(currentInfo.shot.shot.countRepeatShot);
			print("speedRollerTop;     ");
			printNumber(currentInfo.shot.shot.speedRollerTop);
			print("speedRollerBottom;  ");
			printNumber(currentInfo.shot.shot.speedRollerBottom);
			print("speedScrew;         ");
			printNumber(currentInfo.shot.shot.speedScrew);
			print("rotationAxial;      ");
			printNumber(currentInfo.shot.shot.rotationAxial);
			print("rotationHorizontal; ");
			printNumber(currentInfo.shot.shot.rotationHorizontal);
			print("rotationVertical;   ");
			printNumber(currentInfo.shot.shot.rotationVertical);

			currentInfo.state.isShooting = 1;
			doShot(&currentInfo.shot.shot);
		} else {
			print("Current Shot is NULL\n");
			// TODO: sound_ERR(); ledFX_ERR();
		}
		break;
	case ProgramMode:

		break;
	case MacroMode:

		break;
	default:
		break;
	}
}

void stopShooting() {
	currentInfo.state.isPause = 0;
	currentInfo.state.isShooting = 0;

	setScrewkSpeed(0);
	setRollersSpeed(100, 100);
	setPosDefault();
}

void doShotForever(uint8_t number) {

}

uint8_t prepareShot(uint8_t number) {
	Shot shot;
	getShot(number, &shot);
	if (shot.isExist) {
		currentInfo.mode = ShotMode;
		currentInfo.shot.shot = shot;
		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 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;
}