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


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


void setPos(){

}
void setPosDefault(){
	SetServo(0, 90);
	SetServo(1, 90);
	SetServo(2, 90);
}

// 0 .. 100
void setScrewkSpeed(uint8_t speed){
	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(){

}