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

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

typedef enum Mode {
	NoneMode, ShotMode, ProgramMode, MacroMode,
} Mode;

uint8_t isPause = 0;
uint8_t isShooting = 0;

typedef struct CurrentProgram {
	Program program;
	uint8_t shot_index;
} CurrentProgram;

typedef struct CurrentMacro {
	Macro macro;
	uint8_t program_index;
} CurrentMacro;

typedef struct Current {
	Mode mode;
	Shot shot;
	CurrentProgram program;
	CurrentMacro macro;
} Current;

Current current;

HardwareInit_t hwSettings = {
/*DelayTimes*/{
/*preRun*/0 },
/*ServoSetting*/{ { 0, 0, 90, 180 }, { 0, 0, 90, 180 }, { 0, 0, 90, 180 } },
/*Motors*/{ 0, 0 } };

extern int16_t Vz1;
extern int16_t Vz2;

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 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 (current.mode) {
	case ShotMode:
		print("StartShooting\n");
		if (current.shot.isExist) {
			print("Fire!\n");

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

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

		break;
	case MacroMode:

		break;
	default:
		break;
	}
}

void stopShooting() {
	isShooting = 0;
	isPause = 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) {
		current.mode = ShotMode;
		current.shot = shot;
		return 1;
	} else {
		// TODO: sound_ERR(); ledFX_ERR();
		return 0;
	}
}

void setPosSingle(ServoMap servo, uint8_t value) {
	if (hwSettings.servos[servo].invert)
		value = 180 - value;
	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(
			hwSettings.servos[SERVO_AXIAL].def,
			hwSettings.servos[SERVO_HORIZONTAL].def,
			hwSettings.servos[SERVO_VERTICAL].def
	);
}

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

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

	speed = map(speed, 0, 100, hwSettings.motors.speed_Screw_min, 100);

	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 - hwSettings.motors.speed_Screw_min);
	} else {
		up = map(up, 0, 100, 0, 100 + hwSettings.motors.speed_Screw_min);
	}

	if (down < 100) {
		map(down, 0, 100, 0, 100 - hwSettings.motors.speed_Screw_min);
	} else {
		map(down, 0, 100, 0, 100 + hwSettings.motors.speed_Screw_min);
	}

	Vz1 = 200 - up; // invert
	Vz2 = down;
}
// shot sequence
void startProgram() {

}
// shot sequence
void startMacro() {

}