/*
 * 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;

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();
	Vz1 = 100;
	Vz2 = 100;
	TIM1->CCR1 = 0;
	TIM1->CCR2 = 0;
}

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 setPos(uint8_t axial, uint8_t horizontal, uint8_t vertical) {

	//todo:
//	hwSettings.servos[SERVO_AXIAL].invert

	SetServo(SERVO_AXIAL, axial);			// Axial
	SetServo(SERVO_HORIZONTAL, horizontal);	// Horizontal
	SetServo(SERVO_VERTICAL, vertical);		// Vertical
}
void setPosDefault() {
	SetServo(0, 90);	// Axial
	SetServo(1, 90);	// Horizontal
	SetServo(2, 90);	// Vertical
}

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

	if(speed && speed < hwSettings.motors.speed_Screw_min)
		speed = hwSettings.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) {
	Vz1 = up;
	Vz2 = down;
}
// shot sequence
void startProgram() {

}
// shot sequence
void startMacro() {

}