/*
 * mot.c
 *
 *  Created on: May 2, 2026
 *      Author: user
 */
#include <stdio.h>
#include <string.h>
#include "main.h"
#include "mot.h"
#include "pwm.h"

extern uint16_t apM1start;
extern uint16_t apM1stop;
extern uint16_t apM2start;
extern uint16_t apM2stop;
extern uint16_t apM3start;
extern uint16_t apM3stop;
extern uint16_t apM4start;
extern uint16_t apM4stop;
extern uint16_t chM1start;
extern uint16_t chM1stop;
extern uint16_t chM2start;
extern uint16_t chM2stop;
extern uint16_t chM3start;
extern uint16_t chM3stop;
extern uint16_t chM4start;
extern uint16_t chM4stop;
extern volatile uint16_t rtCiclo;
extern uint16_t m1pwmap;
extern uint16_t m1pwmch;
extern uint16_t m2pwmap;
extern uint16_t m2pwmch;
extern uint16_t m3pwmap;
extern uint16_t m3pwmch;
extern uint16_t m4pwmap;
extern uint16_t m4pwmch;
extern uint16_t mrampstart[4];
extern uint16_t tramp;

extern UART_HandleTypeDef huart1;

motMov_st m1ap(void){
	static motMov_st m1st=motWaiting;
	static motMov_st memm1st=motStop;
	char s[100];
	if(memm1st!=m1st){
		memm1st=m1st;
		sprintf(s,"\nm1ap=%d",m1st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}
	if(rtCiclo<apM1start)m1st=motWaiting;
	switch(m1st){
		case motWaiting:
			SetMotPerc(M1,FW,0);
			SetMotPerc(M1,BW,0);
			if(rtCiclo>=apM1start){
				(void)ramp(M1,BW,mrampstart[M1-1],m1pwmap,tramp,RAMPINIT);
				m1st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M1,BW,mrampstart[M1-1],m1pwmap,tramp,RAMPRUN)==DONE){
				m1st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=apM1stop){
				(void)ramp(M1,BW,m1pwmap,0,tramp,RAMPINIT);
				m1st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M1,BW,m1pwmap,0,tramp,RAMPRUN)==DONE){
				m1st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M1,FW,0);
			SetMotPerc(M1,BW,0);
		break;
	}
	return m1st;
}

motMov_st m2ap(void){
	static motMov_st m2st;
	static motMov_st memm2st=motStop;
	char s[100];
	if(memm2st!=m2st){
		memm2st=m2st;
		sprintf(s,"\nm2ap=%d",m2st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}
	if(rtCiclo<apM2start)m2st=motWaiting;
	switch(m2st){
		case motWaiting:
			SetMotPerc(M2,FW,0);
			SetMotPerc(M2,BW,0);
			if(rtCiclo>=apM2start){
				(void)ramp(M2,BW,mrampstart[M2-1],m2pwmap,tramp,RAMPINIT);
				m2st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M2,BW,mrampstart[M2-1],m2pwmap,tramp,RAMPRUN)==DONE){
				m2st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=apM2stop){
				(void)ramp(M2,BW,m2pwmap,0,tramp,RAMPINIT);
				m2st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M2,BW,m2pwmap,0,tramp,RAMPRUN)==DONE){
				m2st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M2,FW,0);
			SetMotPerc(M2,BW,0);
		break;
	}
	return m2st;
}

motMov_st m3ap(void){
	static motMov_st m3st;
	static motMov_st memm3st=motStop;
	char s[100];
	if(memm3st!=m3st){
		memm3st=m3st;
		sprintf(s,"\nm3ap=%d",m3st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}
	if(rtCiclo<apM3start)m3st=motWaiting;
	switch(m3st){
		case motWaiting:
			SetMotPerc(M3,FW,0);
			SetMotPerc(M3,BW,0);
			if(rtCiclo>=apM3start){
				(void)ramp(M3,BW,mrampstart[M3-1],m2pwmap,tramp,RAMPINIT);
				m3st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M3,BW,mrampstart[M3-1],m3pwmap,tramp,RAMPRUN)==DONE){
				m3st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=apM3stop){
				(void)ramp(M3,BW,m3pwmap,0,tramp,RAMPINIT);
				m3st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M3,BW,m3pwmap,0,tramp,RAMPRUN)==DONE){
				m3st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M3,FW,0);
			SetMotPerc(M3,BW,0);
		break;
	}
	return m3st;
}

motMov_st m4ap(void){
	static motMov_st m4st;
	static motMov_st memm4st=motStop;
	char s[100];
	if(memm4st!=m4st){
		memm4st=m4st;
		sprintf(s,"\nm4ap=%d",m4st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}
	if(rtCiclo<apM4start)m4st=motWaiting;
	switch(m4st){
		case motWaiting:
			SetMotPerc(M4,FW,0);
			SetMotPerc(M4,BW,0);
			if(rtCiclo>=apM4start){
				(void)ramp(M4,BW,mrampstart[M4-1],m2pwmap,tramp,RAMPINIT);
				m4st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M4,BW,mrampstart[M4-1],m4pwmap,tramp,RAMPRUN)==DONE){
				m4st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=apM4stop){
				(void)ramp(M4,BW,m4pwmap,0,tramp,RAMPINIT);
				m4st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M4,BW,m4pwmap,0,tramp,RAMPRUN)==DONE){
				m4st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M4,FW,0);
			SetMotPerc(M4,BW,0);
		break;
	}
	return m4st;
}

motMov_st m1ch(void){
	static motMov_st m1st;
	static motMov_st memm1st=motStop;
	char s[100];
	if(memm1st!=m1st){
		memm1st=m1st;
		sprintf(s,"\nm1ch=%d",m1st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}
	if(rtCiclo<chM1start)m1st=motWaiting;
	switch(m1st){
		case motWaiting:
			SetMotPerc(M1,FW,0);
			SetMotPerc(M1,BW,0);
			if(rtCiclo>=chM1start){
				(void)ramp(M1,FW,mrampstart[M1-1],m1pwmch,tramp,RAMPINIT);
				m1st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M1,FW,mrampstart[M1-1],m1pwmch,tramp,RAMPRUN)==DONE){
				m1st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=chM1stop){
				(void)ramp(M1,FW,m1pwmch,0,tramp,RAMPINIT);
				m1st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M1,FW,m1pwmch,0,tramp,RAMPRUN)==DONE){
				m1st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M1,FW,0);
			SetMotPerc(M1,BW,0);
		break;
	}
	return m1st;
}

motMov_st m2ch(void){
	static motMov_st m2st;
	static motMov_st memm2st=motStop;
	char s[100];
	if(memm2st!=m2st){
		memm2st=m2st;
		sprintf(s,"\nm2ch=%d",m2st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}

	if(rtCiclo<chM2start)m2st=motWaiting;
	switch(m2st){
		case motWaiting:
			SetMotPerc(M2,FW,0);
			SetMotPerc(M2,BW,0);
			if(rtCiclo>=chM2start){
				(void)ramp(M2,FW,mrampstart[M2-1],m2pwmch,tramp,RAMPINIT);
				m2st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M2,FW,mrampstart[M2-1],m2pwmch,tramp,RAMPRUN)==DONE){
				m2st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=chM2stop){
				(void)ramp(M2,FW,m2pwmch,0,tramp,RAMPINIT);
				m2st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M2,FW,m2pwmch,0,tramp,RAMPRUN)==DONE){
				m2st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M2,FW,0);
			SetMotPerc(M2,BW,0);
		break;
	}
	return m2st;
}

motMov_st m3ch(void){
	static motMov_st m3st;
	static motMov_st memm3st=motStop;
	char s[100];
	if(memm3st!=m3st){
		memm3st=m3st;
		sprintf(s,"\nm3ch=%d",m3st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}

	if(rtCiclo<chM3start)m3st=motWaiting;
	switch(m3st){
		case motWaiting:
			SetMotPerc(M3,FW,0);
			SetMotPerc(M3,BW,0);
			if(rtCiclo>=chM3start){
				(void)ramp(M3,FW,mrampstart[M3-1],m2pwmch,tramp,RAMPINIT);
				m3st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M3,FW,mrampstart[M3-1],m3pwmch,tramp,RAMPRUN)==DONE){
				m3st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=chM3stop){
				(void)ramp(M3,FW,m3pwmch,0,tramp,RAMPINIT);
				m3st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M3,FW,m3pwmch,0,tramp,RAMPRUN)==DONE){
				m3st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M3,FW,0);
			SetMotPerc(M3,BW,0);
		break;
	}
	return m3st;
}

motMov_st m4ch(void){
	static motMov_st m4st;
	static motMov_st memm4st=motStop;
	char s[100];
	if(memm4st!=m4st){
		memm4st=m4st;
		sprintf(s,"\nm4ch=%d",m4st);
		HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
	}

	if(rtCiclo<chM4start)m4st=motWaiting;
	switch(m4st){
		case motWaiting:
			SetMotPerc(M4,FW,0);
			SetMotPerc(M4,BW,0);
			if(rtCiclo>=chM4start){
				(void)ramp(M4,FW,mrampstart[M4-1],m4pwmch,tramp,RAMPINIT);
				m4st=motStartRamp;
			}
			break;
		case motStartRamp:
			if(ramp(M4,FW,mrampstart[M4-1],m4pwmch,tramp,RAMPRUN)==DONE){
				m4st=motWaitingTime;
			}
			break;
		case motWaitingTime:
			if(rtCiclo>=chM4stop){
				(void)ramp(M4,FW,m4pwmch,0,tramp,RAMPINIT);
				m4st=motStopRamp;
			}
			break;
		case motStopRamp:
			if(ramp(M4,FW,m4pwmch,0,tramp,RAMPRUN)==DONE){
				m4st=motStop;
			}
			break;
		case motStop:
		default:
			SetMotPerc(M4,FW,0);
			SetMotPerc(M4,BW,0);
		break;
	}
	return m4st;
}