/** 
 * \file machine.h
 * \brief machine header file
 * \details File contains functions definitions for Main program
 *
 * \author Juraj Golej
 * \version 1.0
 */ 


#include <inttypes.h>
#include <app/keyboard/keyboard.h>
#include <app/lcd/lcd.h>
#include <app/path/path.h>
#include <app/tool/tool.h>
#include <app/fat32/fat32.h>
#include <drivers/adc/adc.h>
#include <drivers/pio/pio.h>
#include <drivers/sdcard/sdcard.h>
#include <image_line.h>
#include <image_info.h>
#include <image_warning.h>
#include "machine.h"

extern struct ADC_module joyX;
extern struct ADC_module joyY;
extern struct TIMER_module timer_ms;
extern struct TIMER_module timer_coord;
extern struct TIMER_module timer_joy;
extern struct PIO_irq endstop[6];

static volatile enum MACHINE_STATE gState = S_MAN0;

void action_AA_linear();
void action_AB_circleNegative();
void action_AC_circlePositive();
void action_AD_homeMachine();
void action_AE_homeWorkpiece();
void action_AF_press();
void action_AG_setMachineCoord();
void action_AH_setWorkpieceCoord();
void action_AI_coordAbs();
void action_AJ_coordInc();
void action_AK_pause();
void action_AL_changeTool();

struct MACHINE_data ProgramVars = {.actionFunc[0] = action_AA_linear,
								   .actionFunc[1] = action_AB_circleNegative,
								   .actionFunc[2] = action_AC_circlePositive,
								   .actionFunc[3] = action_AD_homeMachine,
								   .actionFunc[4] = action_AE_homeWorkpiece,
								   .actionFunc[5] = action_AF_press,
								   .actionFunc[6] = action_AG_setMachineCoord,
								   .actionFunc[7] = action_AH_setWorkpieceCoord,
								   .actionFunc[8] = action_AI_coordAbs,
								   .actionFunc[9] = action_AJ_coordInc,
								   .actionFunc[10] = action_AK_pause,
								   .actionFunc[11] = action_AL_changeTool
								   };

void keyboardClick(enum KB_CLICKTYPE clickType, uint8_t key)
{
	if(gState <= S_MAN1) {
		if(clickType == KB_PRESS) {
			switch(key) {
				case 1:
					if((!PIO_pin_level(PIN_SD_DETECT)) && (!(AXIS_isBusy(PATH_getAxis(PATH_AXIS_X)) || AXIS_isBusy(PATH_getAxis(PATH_AXIS_Y))) || AXIS_isBusy(TOOL_getAxis())))
					gState = S_FILEMENU0;
					break;
				case 6:
					if(PIO_pin_level(PIO(endstop[4].port, endstop[4].pin)))
						TOOL_moveFree(-10);
					break;
				case 7:
					if(PIO_pin_level(PIO(endstop[5].port, endstop[5].pin)))
						TOOL_moveFree(10);
					break;
				case 8:
					PATH_moveToInc(10, 0, 50, 0);
					break;
				case 9:
					PATH_moveToInc(0, -10, 50, 0);
					break;
				case 11:
					PATH_moveToInc(0, 10, 50, 0);
					break;
				case 12:
					PATH_moveToInc(-10, 0, 50, 0);
					break;
				case 10:
					gState = S_MAN2;
					break;
				default:
					break;
			}
		}
		else {
			switch(key) {
				case 6:
				case 7:
					TOOL_moveFree(0);
					break;
				default:
					break;
			}
		}
	}
	else if(gState >= S_FILEMENU1 && gState <= S_FILEMENU2) {
		if(clickType == KB_PRESS) {
			switch(key) {
				case 0:
					gState = S_MAN0;
					break;
				case 12:
					gState = S_FILEMENU1;
					break;
				case 13:
					gState = S_AUTO0;
					break;
				default:
					break;
			}
		}
	}
	else if(gState >= S_AUTO0 && gState <= S_AUTO4) {
		if(clickType == KB_PRESS) {
			switch(key) {
				case 15:
					gState = S_ERROR0;
					break;
				default:
					break;
			}
		}
	}
	else if(gState == S_ERROR1) {
		if(clickType == KB_PRESS && key == 13) {
			gState = S_MAN0;
		}
	}
}

void joystickButton(struct PIO_irq* const module)
{
	keyboardClick(KB_PRESS, 5);
}

void endstopActive(struct PIO_irq* const module)
{
	if(ProgramVars.actionIndex == MACHINE_ACT_HOMEMACH) {
		if(module == &endstop[0]) {
			AXIS_stop(PATH_getAxis(PATH_AXIS_X));
			return;
		}
		else if(module == &endstop[2]) {
			AXIS_stop(PATH_getAxis(PATH_AXIS_Y));
			return;
		}
		else if(module == &endstop[4]) {
			AXIS_stop(TOOL_getAxis());
			return;
		}
	}

	gState = S_ERROR0;
}

void SDCard_disconnect(struct PIO_irq* const module)
{
	if(gState >= S_FILEMENU1 && gState <= S_FILEMENU2) {
		gState = S_MAN0;
	}
}

void screen_showBackground(const uint8_t* image, const char* text1, const char* text2, const char* text3, const char* const text4)
{
	LCD_clear();
	LCD_setGraphicMode();
	LCD_show_image(image);
	LCD_setTextMode();
	LCD_home();
	if(text1) {
		LCD_goto(0, 0);
		LCD_puts((uint8_t*)text1);
	}
	if(text2) {
		LCD_goto(1, 0);
		LCD_puts((uint8_t*)text2);
	}
	if(text3) {
		LCD_goto(2, 0);
		LCD_puts((uint8_t*)text3);
	}
	if(text4) {
		LCD_goto(3, 0);
		LCD_puts((uint8_t*)text4);
	}
}

void screen_showCoord(struct TIMER_module* timer)
{
	char coordX[7] = "      ";
	char coordY[7] = "      ";
	char coordZ[7] = "      ";

	itoa((int32_t)AXIS_getAbsPos(PATH_getAxis(PATH_AXIS_X)), coordX, 10);
	itoa((int32_t)AXIS_getAbsPos(PATH_getAxis(PATH_AXIS_Y)), coordY, 10);
	itoa((int32_t)AXIS_getAbsPos(TOOL_getAxis()), coordZ, 10);

	coordX[strlen(coordX)] = ' ';
	coordY[strlen(coordY)] = ' ';
	coordZ[strlen(coordZ)] = ' ';

	LCD_goto(1,1);
	LCD_puts((uint8_t*)coordX);
	LCD_goto(2,1);
	LCD_puts((uint8_t*)coordY);
	LCD_goto(3,1);
	LCD_puts((uint8_t*)coordZ);
}

void joystick_readValues(struct TIMER_module* timer)
{
	ADC_start(&joyX);
	ADC_start(&joyY);

	if((ADC_read(&joyX, ADC_CHANNEL_6) > 1500) && PIO_pin_level(PIO(endstop[1].port, endstop[1].pin))) {
		PATH_moveFree(PATH_AXIS_X, 50);
	}
	else if((ADC_read(&joyX, ADC_CHANNEL_6) < -1500) && PIO_pin_level(PIO(endstop[0].port, endstop[0].pin))) {
		PATH_moveFree(PATH_AXIS_X, -50);
	}
	else if(PATH_getAxis(PATH_AXIS_X)->mode == AXIS_MODE_MOVEFREE) {
		PATH_moveFree(PATH_AXIS_X, 0);
	}

	if((ADC_read(&joyY, ADC_CHANNEL_8) > 1500) && PIO_pin_level(PIO(endstop[3].port, endstop[3].pin))) {
		PATH_moveFree(PATH_AXIS_Y, 50);
	}
	else if((ADC_read(&joyY, ADC_CHANNEL_8) < -1500) && PIO_pin_level(PIO(endstop[2].port, endstop[2].pin))) {
		PATH_moveFree(PATH_AXIS_Y, -50);
	}
	else if(PATH_getAxis(PATH_AXIS_Y)->mode == AXIS_MODE_MOVEFREE) {
		PATH_moveFree(PATH_AXIS_Y, 0);
	}
}

inline void set_variable()
{	
	if(ProgramVars.command[1] >= 'A' && ProgramVars.command[1] <= 'I') {
		ProgramVars.var.groupInt[ProgramVars.command[1] - 'A'] = strtol(ProgramVars.command+2, NULL, 10);
	}
	else if(ProgramVars.command[1] >= 'a' && ProgramVars.command[1] <= 'z') {
		//ProgramVars.var.groupDec[ProgramVars.command[1] - 'a'] = strtod(ProgramVars.command+2, NULL, 10);
	}
}

inline bool set_action()
{
	Try((ProgramVars.command[1] < 'A') || (ProgramVars.command[1] > ('A' + MACHINE_ACT)), false);

	ProgramVars.actionIndex = ProgramVars.command[1] - 'A';
	return true;
}

void Machine()
{
	struct FAT32_file file = {0};

	PIO_setpin_high(PIN_DRV_ENA);

	for(;;) {
		switch(gState) {
			case S_MAN0:
				screen_showBackground(image_line, "Manual control", "X:", "Y:", "Z:");
				TIMER_start(&timer_coord, TIMER_MODE_REPEAT);
				TIMER_start(&timer_joy, TIMER_MODE_REPEAT);
				gState = S_MAN1;
			case S_MAN1:
				break;
			case S_MAN2:
				TIMER_stop(&timer_joy);
				gState = S_MAN0;
				ProgramVars.actionIndex = MACHINE_ACT_HOMEMACH;
				action_AD_homeMachine();
				ProgramVars.actionIndex = 0;
				break;


			case S_FILEMENU0:
				TIMER_stop(&timer_coord);
				TIMER_stop(&timer_joy);
				screen_showBackground(image_line, "Choose file", NULL, NULL, NULL);
				if(!SDCARD_isReady()) {
					SDCARD_identify();
				}
				FAT32_init();
				FAT32_goToRoot();
				gState = S_FILEMENU1;
			case S_FILEMENU1:
				if(!FAT32_nextFile("TXT", &file)) {
					LCD_goto(1,0);
					LCD_puts((uint8_t*)file.name);
				}
				gState = S_FILEMENU2;
				break;
			case S_FILEMENU2:
				break;


			case S_AUTO0:
				FAT32_file_open(&file);
				screen_showBackground(image_line, "Automatic mode", "X:", "Y:", "Z:");
				TIMER_start(&timer_coord, TIMER_MODE_REPEAT);
				gState = S_AUTO1;
			case S_AUTO1:
				ProgramVars.cmdLength = FAT32_read((uint8_t*)ProgramVars.command, MACHINE_CMD_LEN);
				if(ProgramVars.cmdLength < 0) {
					gState = S_MAN0;
					break;
				}
				if(ProgramVars.command[0] == 'S' && ProgramVars.cmdLength > 3) gState = S_AUTO2;
				else if(ProgramVars.command[0] == 'A' && ProgramVars.cmdLength > 2) gState = S_AUTO3;
				break;
			case S_AUTO2:
				set_variable();
				gState = S_AUTO1;
				break;
			case S_AUTO3:
				gState = S_AUTO1;
				if(set_action()) ProgramVars.actionFunc[ProgramVars.actionIndex]();
				if(gState == S_AUTO1) break;
				gState = S_AUTO4;
			case S_AUTO4:
				break;
			case S_ERROR0:
				AXIS_stop(PATH_getAxis(PATH_AXIS_X));
				AXIS_stop(PATH_getAxis(PATH_AXIS_Y));
				AXIS_stop(TOOL_getAxis());
				TIMER_stop(&timer_coord);
				TIMER_stop(&timer_joy);
				screen_showBackground(image_warning, NULL, NULL, "Endstop", "Activated");
				gState = S_ERROR1;
				break;
			case S_ERROR1:
				break;
			default:
				break;
		}
	}
}

void action_AA_linear()
{
	const enum MACHINE_STATE inState = gState;
	uint8_t tmpState = 0;

	if(ProgramVars.coord.relative) {
		ProgramVars.var.groupInt[MACHINE_VAR_INT_X] += ProgramVars.workpiece.X;
		ProgramVars.var.groupInt[MACHINE_VAR_INT_Y] += ProgramVars.workpiece.Y;
	}

	while(inState == gState) {
		switch(tmpState) {
			case 0:
				if(ProgramVars.coord.incremental)
					TOOL_moveToInc(ProgramVars.var.groupInt[MACHINE_VAR_INT_Z], ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEZ]);
				else
					TOOL_moveToAbs(ProgramVars.var.groupInt[MACHINE_VAR_INT_Z], ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEZ]);
				tmpState = 1;
			case 1:
				if(!AXIS_isBusy(TOOL_getAxis())) tmpState = 2;
				break;
			case 2:
				if(ProgramVars.coord.incremental)
					PATH_moveToInc(ProgramVars.var.groupInt[MACHINE_VAR_INT_X], ProgramVars.var.groupInt[MACHINE_VAR_INT_Y], ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEXY], 0);
				else
					PATH_moveToAbs(ProgramVars.var.groupInt[MACHINE_VAR_INT_X], ProgramVars.var.groupInt[MACHINE_VAR_INT_Y], ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEXY], 0);
				tmpState = 3;
			case 3:
				if(!(AXIS_isBusy(PATH_getAxis(PATH_AXIS_X)) || AXIS_isBusy(PATH_getAxis(PATH_AXIS_Y)))) return;
				break;
			default:
				break;
		}
	}
}

void action_AB_circleNegative()
{
	// not working yet
}

void action_AC_circlePositive()
{
	// not working yet
}

void action_AD_homeMachine()
{
	const enum MACHINE_STATE inState = gState;
	uint8_t tmpState = 0;

	while(inState == gState) {
		switch(tmpState) {
			case 0:
				if(PIO_pin_level(PIN_ESTOP_X1)) PATH_moveFree(PATH_AXIS_X, -50);
				if(PIO_pin_level(PIN_ESTOP_Y1)) PATH_moveFree(PATH_AXIS_Y, -50);
				if(PIO_pin_level(PIN_ESTOP_Z1)) TOOL_moveFree(-5);
				tmpState = 1;
			case 1:
				if(!(AXIS_isBusy(PATH_getAxis(PATH_AXIS_X)) || AXIS_isBusy(PATH_getAxis(PATH_AXIS_Y)) || AXIS_isBusy(TOOL_getAxis()))) {
					AXIS_resetAbsolutePos(TOOL_getAxis());
					AXIS_resetAbsolutePos(PATH_getAxis(PATH_AXIS_X));
					AXIS_resetAbsolutePos(PATH_getAxis(PATH_AXIS_Y));
					PATH_moveToInc(10, 10, 50, 0);
					TOOL_moveToInc(5, 5);
					tmpState = 2;
				}
				break;
			case 2:
				if(!(AXIS_isBusy(PATH_getAxis(PATH_AXIS_X)) || AXIS_isBusy(PATH_getAxis(PATH_AXIS_Y)) || AXIS_isBusy(TOOL_getAxis()))) {
					AXIS_resetAbsolutePos(PATH_getAxis(PATH_AXIS_X));
					AXIS_resetAbsolutePos(PATH_getAxis(PATH_AXIS_Y));
					AXIS_resetAbsolutePos(TOOL_getAxis());
					return;
				}
				break;
			default:
				break;
		}
	}
}

void action_AE_homeWorkpiece()
{
	const enum MACHINE_STATE inState = gState;
	uint8_t tmpState = 0;

	while(inState == gState) {
		switch(tmpState) {
			case 0:
				TOOL_moveToAbs(0, ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEZ]);
				tmpState = 1;
			case 1:
				if(!AXIS_isBusy(TOOL_getAxis())) tmpState = 2;
				break;
			case 2:
				PATH_moveToAbs(ProgramVars.workpiece.X, ProgramVars.workpiece.Y, ProgramVars.var.groupInt[MACHINE_VAR_INT_FRATEXY], 0);
				tmpState = 3;
			case 3:
				if(!(AXIS_isBusy(PATH_getAxis(PATH_AXIS_X)) || AXIS_isBusy(PATH_getAxis(PATH_AXIS_Y)))) return;
				break;
			default:
				break;
		}
	}
}

void action_AF_press()
{
	// not working yet
}

void action_AG_setMachineCoord()
{
	ProgramVars.coord.relative = false;
}

void action_AH_setWorkpieceCoord()
{
	ProgramVars.coord.relative = true;
}

void action_AI_coordAbs()
{
	ProgramVars.coord.incremental = false;
}

void action_AJ_coordInc()
{
	ProgramVars.coord.incremental = true;
}

void action_AK_pause()
{
	const enum MACHINE_STATE inState = gState;

	TIMER_setInterval(&timer_ms, ProgramVars.var.groupInt[8] * 1000);
	TIMER_start(&timer_ms, TIMER_MODE_SINGLE);
	while(TIMER_isBusy(&timer_ms)) {
		if(inState != gState) {
			TIMER_stop(&timer_ms);
			return;
		}
	}
}

void action_AL_changeTool()
{
	// not working yet
}