#include <string.h>
#include "bsp/bsp.h"
#include "foc/foc_api.h"
#include "foc/foc_core.h"

extern motor_foc_t mFOC;

FOCState foc_stm_state(void){
	return mFOC.state;
}

foc_fault_t foc_stm_nextstate(FOCState state) {
	bool changed = false;
	if (state == mFOC.state) {
		return foc_success;
	}
	if (state == START) {
		if (mFOC.state == IDLE) {
			changed = true;
		}
	}else if (state == IDLE) {
		if (mFOC.state == ANY_STOP) {
			changed = true;
		}
	}else if (state == ANY_STOP) {
		if (mFOC.state != IDLE) {
			changed = true;
		}
	}else if (state == CURRENT_CALIBRATE) {
		if (mFOC.state == START) {
			changed = true;
		}
	}else if (state == READY_TO_RUN) {
		if (mFOC.state == CURRENT_CALIBRATE) {
			changed = true;
		}
	}else if (state == RAMPING_START) {
		if (mFOC.state == READY_TO_RUN || mFOC.state == RUNNING) {
			changed = true;
		}
	}else if (state == CLOSED_LOOP) {
		if (mFOC.state == RAMPING_START) {
			changed = true;
		}		
	}else if (state == RUNNING) {
		if (mFOC.state == CLOSED_LOOP) {
			changed = true;
		}
	}
	if (changed) {
		mFOC.state = state;
		return foc_success;
	}
	return foc_not_allowed;
}

void FOC_Normal_Task(motor_foc_t *foc) {
	switch (foc->state) {
		case IDLE:
			foc->mode = FOC_MODE_OPEN_LOOP;
			break;
		case START:
			PWM_TurnOnLowSides();
			foc_stm_nextstate(CURRENT_CALIBRATE);
			break;
		case CURRENT_CALIBRATE:
			foc_current_calibrate();
			foc_stm_nextstate(READY_TO_RUN);
			break;
		case READY_TO_RUN:
			foc_pwm_start(true);
			foc_stm_nextstate(RAMPING_START);
			ramp_exc(&foc->voltage_ramp);
			foc_overide_vdq(true);
			break;
		case RAMPING_START:
			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->voltage_ramp));
			printf("target %f\n", ramp_get_target(&foc->voltage_ramp));
			if (foc_get_speed() >= RPM_FOR_CLOSE_LOOP){
				//foc_stm_nextstate(CLOSED_LOOP);
				//foc_overide_vdq(false);
			}
			break;
		case CLOSED_LOOP:
			Foc_Speed_Ramp(foc);
			foc->mode = FOC_MODE_PI_FULL;
			foc_stm_nextstate(RUNNING);
			ramp_clear(&foc->voltage_ramp);
			break;
		case RUNNING:
			Foc_Speed_Ramp(foc);
			Foc_Calc_Current_Ref(foc);
			if (foc->foc_fault == foc_brake_error){
				foc_stm_nextstate(ANY_STOP);
				break;
			}
			/*
			if (foc_get_speed() <= RPM_FOR_CLOSE_LOOP){
				foc_set_voltage_ramp(speed_to_voltage(foc_get_speed()));
				ramp_exc(&foc->voltage_ramp);
				foc_stm_nextstate(RAMPING_START);
				foc->mode = FOC_MODE_OPEN_LOOP;
				foc_overide_vdq(true);
			}*/
			break;
		case ANY_STOP:
			ramp_clear(&foc->current_ramp);
			foc_clear();
			foc_stm_nextstate(IDLE);
			break;
		default:
			break;
	}

}