/*
#include <splitflap_positions.c>
#include <splitflap_flaps.h>
 * splitflap.c
 *
 *  Created on: 29.09.2022
 *      Author: jonas
 */

#include "platform.h"
#include <stdbool.h>
#include "splitflap.h"
#include "splitflap_positions.h"
#include "McuRTOS.h"
#include "McuULN2003.h"
#include <stdbool.h>
#include "McuWait.h"
#include "McuLog.h"
#include "lib/dict.h"
#include "McuUtility.h"
#include "McuLib.h"
#include "MotOffsetTable.h"
#include "nvmc.h"

#define FLAP_QUEUE_LENGTH (5)						/* number of elements in queue */
#define FLAP_QUEUE_SELEM_SIZE (sizeof(Flap_t))      /* size of element */

/* function declarations */
static bool OngoingMoveMutex_Lock(SF_Handle_t instance);
static void OngoingMoveMutex_Unlock(SF_Handle_t instance);
static void SF_Task(void *pv);

/**********************/
/* INIT / DEINIT      */
/**********************/
SF_Handle_t SF_Init(SF_Config_t* config){
	SF_t* splitflap;

#if SPLITFLAP_CONFIG_USE_FREERTOS_HEAP
	splitflap = (SF_t*)pvPortMalloc(sizeof(SF_t)); 						// get memory for device from FreeRtos heap
#else
	splitflap = (SF_t*)malloc(sizeof(SF_t)); 							// get memory for device from MCU heap
#endif

	splitflap->magSensor = McuGPIO_InitGPIO(&config->magSensorConfig);	// initialize magnet sensor GPIO
	splitflap->motor = McuULN2003_InitMotor(&config->motorConfig);		// initialize ULN driver for stepper motor
	splitflap->ongoingMoveMutex = xSemaphoreCreateRecursiveMutex(); 	// create mutex for ongoing move
	splitflap->flapQueue = xQueueCreate(FLAP_QUEUE_LENGTH, 				// create queue
										FLAP_QUEUE_SELEM_SIZE);
	splitflap->id = config->setupIdentifier;							// copy setup identifier
	splitflap->hwId = config->hardwareIdentifier;						// copy hardware identifier
	splitflap->state = SF_STATE_NOT_READY;								// set to "not initialized"
#if PL_CONFIG_USE_NVMC
	int16_t hwIdFromNvmc = -1;
	// if there exists a hw id in the NVMC and it is valid
	if(NVMC_GetStepperID(config->setupIdentifier, &hwIdFromNvmc) == ERR_OK && hwIdFromNvmc >= 0){
		splitflap->hwId = hwIdFromNvmc; // use this
	} else{
		McuLog_warn("SF_Init: No or invalid HwId found in NVMC for SplitFlap <%i>, storing default ID.", splitflap->id);
		// save default stepper id from code to NVMC
		if(NVMC_SetStepperID(config->setupIdentifier, config->hardwareIdentifier) != ERR_OK){
			McuLog_error("SF_Init: Failed to save hardware ID in NVMC for SplitFlap ID <%i>", config->setupIdentifier);
		}
	}
#endif
	// check if queue was created
	if(splitflap->flapQueue == NULL){
		McuLog_error("Flap queue creation failed for Splitflap <%i>", splitflap->id);
		for(;;){}
	}
	// if yes: add queue to registry
	vQueueAddToRegistry(splitflap->flapQueue, "FlapQueue");

	// check if queue was created
	if(splitflap->ongoingMoveMutex == NULL){
		McuLog_error("Ongoing Move mutex creation failed for Splitflap <%i>", splitflap->id);
		for(;;){}
	}
	// if yes: add ongoing move mutex to registry
	vQueueAddToRegistry(splitflap->ongoingMoveMutex, "OngoingMoveMutex");

	// create Task for splitflap
	BaseType_t res = xTaskCreate(	SF_Task,
									"SF",
									800/sizeof(StackType_t),
									splitflap,  // no &, since otherwise pointing on parameter address! we want handle address!
									tskIDLE_PRIORITY,
									NULL);
	if(res != pdPASS) // task creation not successful?
	{
		McuLog_error("Creation of task for Splitflap <%i> failed", splitflap->id);
		for(;;) {} // Endless loop
	}

	return splitflap;
}

void SF_Deinit(SF_Handle_t instance){
	McuLog_warn("Deinitializing Splitflap <%i>", ((SF_t*)instance)->id);
	vSemaphoreDelete(((SF_t*)instance)->ongoingMoveMutex);
	vQueueDelete(((SF_t*)instance)->flapQueue);
	((SF_t*)instance)->state = SF_STATE_NOT_READY;
	McuULN2003_DeinitMotor(((SF_t*)instance)->motor);
	McuGPIO_DeinitGPIO(((SF_t*)instance)->magSensor);
#if SPLITFLAP_CONFIG_USE_FREERTOS_HEAP
	vPortFree((SF_t*)instance);								// free FreeRTOS heap
#else
	free((SF_t*)instance); 									// free MCU heap
#endif
}


/**********************/
/* PUBLIC FUNCTIONS   */
/**********************/
bool SF_MoveMotorToZeroPosition(SF_Handle_t instance){
	int numStepsMoved = 0;
	int32_t offsetSteps = 0;
	// check if the hwId needs an offset init position
	if(MotOffset_Get(((SF_t*)instance)->hwId, &offsetSteps) == ERR_OK){
		McuLog_info("SF_MoveMotorToZeroPosition found offsetSteps=%i for Splitflap <%i> with hwId=%i", offsetSteps, ((SF_t*)instance)->id, ((SF_t*)instance)->hwId);
	}

	// check if positive offset!
	if(offsetSteps < 0){
		McuLog_info("SF_MoveMotorToZeroPosition found negative offset for Splitflap with hwId=%i. Offset set to zero.", ((SF_t*)instance)->hwId);
		offsetSteps = 0;
	}

	if(SF_IS_RDY_TO_MOVE((SF_t*)instance)){
		McuLog_warn("Tried to initialize motor of Splitflap <%i>, but already initialized => skipping", ((SF_t*)instance)->id);
		return true;
	}
	else if(SF_IS_MOVING((SF_t*)instance)){
		McuLog_warn("Tried to initialize motor of Splitflap <%i>, but it is currently moving => skipping", ((SF_t*)instance)->id);
		return false;
	}

	if(OngoingMoveMutex_Lock(instance)){
		// set state
		((SF_t*)instance)->state = SF_STATE_INITIALIZATION;

		// move out of sensor
		while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == true){
			McuULN2003_IncStep(((SF_t*)instance)->motor);
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
			vTaskDelay(pdMS_TO_TICKS(20));
#else
			McuWait_Waitms(20);
#endif
		}

		// turn until sensor is on and not reached one full rotation already (timeout)
//		while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == false && numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND ){
//			McuULN2003_IncStep(((SF_t*)instance)->motor);
//#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
//			vTaskDelay(pdMS_TO_TICKS(20));
//#else
//			McuWait_Waitms(20);
//#endif
//			numStepsMoved++;
//		}
		McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
		while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == false && numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND){
			if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true)){
				numStepsMoved++;
			}
			#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
						vTaskDelay(pdMS_TO_TICKS(20));
			#else
						McuWait_Waitms(20);
			#endif
		}
		McuULN2003_AccelerationEnd(((SF_t*)instance)->motor);
		// offset after init
		if(numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND){
			for(int i=0; i < offsetSteps; i++){
				McuULN2003_IncStep(((SF_t*)instance)->motor);
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
			vTaskDelay(pdMS_TO_TICKS(20));
#else
			McuWait_Waitms(20);
#endif
			}
		}
		McuULN2003_SetPos(((SF_t*)instance)->motor, 0);
		McuULN2003_PowerOff(((SF_t*)instance)->motor);

		OngoingMoveMutex_Unlock(instance);
	} else{
		// set state
		((SF_t*)instance)->state = SF_STATE_NOT_READY;
		McuLog_error("SF_MoveMotorToZeroPosition failed for Splitflap <%i>, could not aquire ongoing move mutex.", ((SF_t*)instance)->id);
		return false;
	}

	// success if less than one rotation
	((SF_t*)instance)->state = (numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND) ? SF_STATE_READY : SF_STATE_NOT_READY;
	McuLog_info("SF_MoveMotorToZeroPosition finished for Splitflap <%i>, success=%s", ((SF_t*)instance)->id, SF_IS_RDY_TO_MOVE(((SF_t*)instance)) ? "true" : "false");

	return SF_IS_RDY_TO_MOVE(((SF_t*)instance));
}

void SF_MoveMotorToZeroPositionAsync(SF_Handle_t instance){
	// set to state INITIALIZATION => is trigger for the task to start init
	((SF_t*)instance)->state = SF_STATE_INITIALIZATION;
}

void SF_MoveSteps(SF_Handle_t instance, uint32_t steps){
	if(SF_IS_RDY_TO_MOVE((SF_t*)instance)){
		if(OngoingMoveMutex_Lock(instance)){
			// set state
			((SF_t*)instance)->state = SF_STATE_MOVING;

			// run move with acceleration & deceleration
			McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
			while(steps>0){
				if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true)){
					steps--;
				}
#ifdef McuLib_CONFIG_SDK_USE_FREERTOS
				vTaskDelay(pdMS_TO_TICKS(1));
#else
				McuWait_Waitms(1);
#endif
			}
			McuULN2003_AccelerationEnd(((SF_t*)instance)->motor);

			// Power off disables all outputs of the ULN,
			// required since it is possible that one is still active, which would result in the motor getting hot
			// no re-init is required
			McuULN2003_PowerOff(((SF_t*)instance)->motor);

			// set new state
			((SF_t*)instance)->state = SF_STATE_READY;

			OngoingMoveMutex_Unlock(instance);
		} else{
			McuLog_error("SF_MoveSteps failed for Splitflap <%i>, could not aquire ongoing move mutex.", ((SF_t*)instance)->id);
		}
	} else{
		McuLog_error("SF_MoveSteps could not be called for Splitflap <%i> because it is not initialized.", ((SF_t*)instance)->id);
	}
}

bool SF_GetMagSensorAtZeroPosition(SF_Handle_t instance){
	return McuGPIO_IsLow(((SF_t*)instance)->magSensor);
}

void SF_MoveToFlap(SF_Handle_t instance, Flap_t flap){
	if(OngoingMoveMutex_Lock(instance)){

		// get flap pos from dictonary
		Position_t flapPos;
		if(SF_Position_Get(flap, &flapPos) == ERR_FAILED){
			McuLog_error("SF_MoveToFlap did not find the position of Flap %c", flap);
			OngoingMoveMutex_Unlock(instance);
			return;
		}
		// get current motor pos
		int32_t currentPos = SF_GetMotorPosition(instance) % SPLITFLAP_STEPS_ONE_ROUND;
		// calc steps to move
		int32_t stepsToReachFlap = 0;
		// not already there
		if(flapPos != currentPos){
			if(flapPos < currentPos){
					stepsToReachFlap = SPLITFLAP_STEPS_ONE_ROUND-currentPos+flapPos;
			}else if(flapPos > currentPos){
					stepsToReachFlap = flapPos - currentPos;
			}
			SF_MoveSteps(instance, stepsToReachFlap);
		}

		OngoingMoveMutex_Unlock(instance);
	}
}

void SF_MoveToFlapAsync(SF_Handle_t instance, Flap_t flap){
	if(xQueueSendToBack(((SF_t*)instance)->flapQueue, &flap, pdMS_TO_TICKS(100)) != pdPASS){
		McuLog_error("SF_MoveToFlapAsync: Failed to add flap to queue for Splitflap <%i>", ((SF_t*)instance)->id);
	}
}

void SF_SetHardwareIdentifier(SF_Handle_t instance, HardwareIdentifier_t identifier){
	((SF_t*)instance)->hwId = identifier;
#if PL_CONFIG_USE_NVMC	// update hardwareid in NVMC if used
	if(NVMC_SetStepperID(((SF_t*)instance)->id, identifier) != ERR_OK){
		McuLog_error("SF_SetHardwareIdentifier: Failed to update hardware ID in NVMC for SplitFlap ID <%i>", ((SF_t*)instance)->id);
	}
#endif
}

int32_t SF_GetMotorPosition(SF_Handle_t instance){
	return McuULN2003_GetPos(((SF_t*)instance)->motor);
}

SF_State SF_GetCurrentState(SF_Handle_t instance){
	return ((SF_t*)instance)->state;
}

/**********************/
/* INTERNAL FUNCTIONS */
/**********************/
/* voidpointer to instance of splitflap (SF_Handle_t) */
static void SF_Task(void *pv){
	// parse parameter
	SF_Handle_t instance = (SF_Handle_t)pv;
	SF_t* splitflap = (SF_t*)instance;
	Flap_t nextFlap = ' ';
	bool initSuccess = false;
	McuLog_info("Splitflap: Task for Splitflap <%i> started.", splitflap->id);

	for(;;){
		// delay task
		vTaskDelay(pdMS_TO_TICKS(50));

		// check state
		switch (splitflap->state) {
			// no action to be made
			case SF_STATE_NOT_READY:
				break;   // = restart for loop

			// action: start initialization
			case SF_STATE_INITIALIZATION:
				McuLog_info("Splitflap <%i> recognized to be in state initialization => starting zero move.", splitflap->id);
				initSuccess = SF_MoveMotorToZeroPosition(instance);
				McuLog_info("Splitflap <%i> finished initialization. success=%s", splitflap->id, initSuccess ? "true" : "false");

				break;  	// = go on in the current loop

			// action: check if any moves need to be made
			case SF_STATE_READY:
				// check if anything is in queue once (poll once, don't get item)
				if(xQueuePeek(splitflap->flapQueue, &nextFlap, 0) != pdPASS){
					/* failed to receive => queue empty */
					continue;
				}
				McuLog_info("Splitflap <%i> recognized new flap '%c' in queue to move to.", splitflap->id, nextFlap);

				// new flap to move to is available
				// check if ongoing move
				if(OngoingMoveMutex_Lock(instance)){
					// now get the queue item
					if(xQueueReceive(splitflap->flapQueue, &nextFlap, 0) != pdPASS){
						/* failed to receive => queue empty but previously an item was there? error */
						McuLog_error("Failed to receive Queue item of Splitflap <%i>.", splitflap->id);
						OngoingMoveMutex_Unlock(instance);
						continue; // ignore & proceed
					}

					//=> execute move
					McuLog_info("Splitflap <%i> moving to flap '%c'.", splitflap->id, nextFlap);
					SF_MoveToFlap(instance, nextFlap);
					McuLog_info("Splitflap <%i> move done to flap '%c'.", splitflap->id, nextFlap);
					vTaskDelay(pdMS_TO_TICKS(100)); // wait for logger to finish

					OngoingMoveMutex_Unlock(instance);
				}
				break;

			// wrong state
			default:
				McuLog_warn("Not implemented state in SF_Task!");
				break; 	// = restart for loop
		}
	}
}


/**********************/
/* HELPERS            */
/**********************/
static bool OngoingMoveMutex_Lock(SF_Handle_t instance){
	/* aquire mutex */
	if(xSemaphoreTakeRecursive(((SF_t*)instance)->ongoingMoveMutex, pdMS_TO_TICKS(20)) != pdTRUE){
		return false; /* timeout? */
	}
	return true;
}

static void OngoingMoveMutex_Unlock(SF_Handle_t instance){
	/* give back mutex */
	if(xSemaphoreGiveRecursive(((SF_t*)instance)->ongoingMoveMutex) != pdTRUE){
		/* issue */
		McuLog_error("Could not give back ongoing move mutex for splitflap %i", ((SF_t*)instance)->id);
		for(;;);
	}
}