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

#include <stdbool.h>
#include "splitflap.h"
#include "fsl_debug_console.h"
#include "McuULN2003.h"
#include <stdbool.h>
#include "McuWait.h"
#include "McuRTOS.h"
#include "lib/dict.h"
#include "McuUtility.h"

/* dynamic dictionary for the letters of the dictionary */
static dict_t **splitFlapDict;
/* all letters of the splitflap in the correct order */
static Flap_t SF_Letters[] = {	" ", "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K",
								"L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V", "W", "X", "Y", "Z",
								"0", "1", "2", "3", "4", "5", "6", "7", "8", "9",
								"!", "?", ":"};

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

/**********************/
/* INIT / DEINIT      */
/**********************/
void SF_InitConfig(void){
	splitFlapDict = dictAlloc();
	((dict_t*)splitFlapDict)->key=NULL;
	((dict_t*)splitFlapDict)->value=NULL;
	((dict_t*)splitFlapDict)->next=NULL;
	float stepsPerSegment = SPLITFLAP_STEPS_PER_SEGMENT; // do division once

	// add all splitflap flaps to the dictionary, calculating the position automatically
	// print out for debugging
	PRINTF("Splitflap positioning data is automatically calculated:\n");
	for(int i = 0; i < SPLITFLAP_AMOUNT_OF_SEGMENTS; i ++){
		// + 0.5 so the rounding is done correctly
		int32_t position = (stepsPerSegment * (float)i + 0.5);
		addItem(splitFlapDict, SF_Letters[i], (int32_t*)position);
		PRINTF("Letter '%s': Position %i\n", SF_Letters[i], (int)position);
	}
}

void SF_DeInitConfig(void){
	dictDealoc(splitFlapDict);
}

SF_Handle_t SF_Init(SF_Config_t* instance, int id){
	SF_t* splitflap;

#if SPLITFLAP_CONFIG_USE_FREERTOS_HEAP
	splitflap = (SF_t*)pvPortMalloc(sizeof(SF_t)); /* get a new device descriptor */
#else
	splitflap = (SF_t*)malloc(sizeof(SF_t)); /* get a new device descriptor */
#endif

	splitflap->magSensor = McuGPIO_InitGPIO(&instance->magSensorConfig);
	splitflap->motor = McuULN2003_InitMotor(&instance->motorConfig);
	splitflap->id = id;
	splitflap->ongoingMoveMutex = xSemaphoreCreateRecursiveMutex(); 	// create mutex for ongoing move
	splitflap->initialized = false;
	splitflap->nextFlap = SF_Letters[0];

	// add mutex to registry
	char text[50] = "Ongoing move SplitFlap ";
	McuUtility_strcatNum16s((uint8_t*)text, sizeof(text)+20, id);
	vQueueAddToRegistry(splitflap->ongoingMoveMutex, text);

	return splitflap;
}

void SF_Deinit(SF_Handle_t instance){
	vSemaphoreDelete(((SF_t*)instance)->ongoingMoveMutex);
	((SF_t*)instance)->initialized = false;
	McuULN2003_DeinitMotor(((SF_t*)instance)->motor);
	McuGPIO_DeinitGPIO(((SF_t*)instance)->magSensor);
}


/**********************/
/* PUBLIC FUNCTIONS   */
/**********************/
bool SF_MoveMotorToZeroPosition(SF_Handle_t instance, uint16_t offsetSteps){
	int numStepsMoved = 0;

	if(OngoingMoveMutex_Lock(instance)){
		// move out of sensor
		while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == true){
			McuULN2003_IncStep(((SF_t*)instance)->motor);
			McuWait_Waitms(20);
		}

		// 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);
			McuWait_Waitms(20);
			numStepsMoved++;
		}

		// offset after init
		if(numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND){
			for(int i=0; i < offsetSteps; i++){
				McuULN2003_IncStep(((SF_t*)instance)->motor);
				McuWait_Waitms(20);
			}
		}
		McuULN2003_SetPos(((SF_t*)instance)->motor, 0);
		McuULN2003_PowerOff(((SF_t*)instance)->motor);

		OngoingMoveMutex_Unlock(instance);
	}

	// success if less than one rotation
	((SF_t*)instance)->initialized = (numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND);

	return ((SF_t*)instance)->initialized;
}

void SF_MoveSteps(SF_Handle_t instance, uint32_t steps){
	if(((SF_t*)instance)->initialized){
		if(OngoingMoveMutex_Lock(instance)){
			// run move with acceleration & deceleration
			McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
			while(steps>0){
				if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true)){
					steps--;
				}
				McuWait_Waitms(1);
			}
			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);

			OngoingMoveMutex_Unlock(instance);
		}
	}
}

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
		int32_t flapPos = (int32_t)getItem(*splitFlapDict, flap);
		// 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(OngoingMoveMutex_Lock(instance)){
		BaseType_t res;

		char taskName[10] = "SF Mv ";
		McuUtility_strcatNum16s((uint8_t*)taskName, sizeof(taskName), ((SF_t*)instance)->id);
		// set next flap
		((SF_t*)instance)->nextFlap = flap;

		res = xTaskCreate(		SF_MoveToNextFlap,
								taskName,
								500/sizeof(StackType_t),
								instance,  // no &, since otherwise pointing on parameter address! we want handle address!
								tskIDLE_PRIORITY,
								NULL);

		if(res != pdPASS) // task creation not successful?
		{
			PRINTF("Creation of %s failed", taskName);
			for(;;) {} // Endless loop
		}

		OngoingMoveMutex_Unlock(instance);
	}
}

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

/**********************/
/* INTERNAL FUNCTIONS */
/**********************/
/* voidpointer to instance of splitflap (SF_Handle_t) */
static void SF_MoveToNextFlap(void *pv){
	// parse parameter
	SF_Handle_t instance = (SF_Handle_t)pv;
	PRINTF("Splitflap: Moving Flap nr. %i to letter '%c'.\n", ((SF_t*)instance)->id, (((SF_t*)instance)->nextFlap)[0]);
	// move to next flap
	SF_MoveToFlap(instance, ((SF_t*)instance)->nextFlap);
	// reset next flap
	((SF_t*)instance)->nextFlap = SF_Letters[0];
}


/**********************/
/* 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 */
		PRINTF("Could not give back ongoing move mutex for splitflap %i", ((SF_t*)instance)->id);
		for(;;);
	}
}