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

#include "splitflap.h"
#include "McuULN2003.h"
#include <stdbool.h>
#include "McuWait.h"
#include "lib/dict.h"
#include "splitflap_flaps.h"

static dict_t **splitFlapDict;

void SF_InitConfig(void){
	splitFlapDict = dictAlloc();
	((dict_t*)splitFlapDict)->key=NULL;
	((dict_t*)splitFlapDict)->value=NULL;
	((dict_t*)splitFlapDict)->next=NULL;
	addItem(splitFlapDict, SF_A, (int32_t*)13);
	addItem(splitFlapDict, SF_B, (int32_t*)26);
	addItem(splitFlapDict, SF_C, (int32_t*)39);
	addItem(splitFlapDict, SF_D, (int32_t*)52);
	addItem(splitFlapDict, SF_E, (int32_t*)65);
	addItem(splitFlapDict, SF_F, (int32_t*)78);
	addItem(splitFlapDict, SF_G, (int32_t*)91);
	addItem(splitFlapDict, SF_H, (int32_t*)104);
	addItem(splitFlapDict, SF_I, (int32_t*)117);
	addItem(splitFlapDict, SF_J, (int32_t*)130);
	addItem(splitFlapDict, SF_K, (int32_t*)143);
	addItem(splitFlapDict, SF_L, (int32_t*)156);
	addItem(splitFlapDict, SF_M, (int32_t*)169);
	addItem(splitFlapDict, SF_N, (int32_t*)182);
	addItem(splitFlapDict, SF_O, (int32_t*)195);
	addItem(splitFlapDict, SF_P, (int32_t*)208);
	addItem(splitFlapDict, SF_Q, (int32_t*)221);
	addItem(splitFlapDict, SF_R, (int32_t*)234);
	addItem(splitFlapDict, SF_S, (int32_t*)247);
	addItem(splitFlapDict, SF_T, (int32_t*)260);
	addItem(splitFlapDict, SF_U, (int32_t*)273);
	addItem(splitFlapDict, SF_V, (int32_t*)286);
	addItem(splitFlapDict, SF_W, (int32_t*)299);
	addItem(splitFlapDict, SF_X, (int32_t*)312);
	addItem(splitFlapDict, SF_Y, (int32_t*)325);
	addItem(splitFlapDict, SF_Z, (int32_t*)338);
	addItem(splitFlapDict, SF_0, (int32_t*)351);
	addItem(splitFlapDict, SF_1, (int32_t*)364);
	addItem(splitFlapDict, SF_2, (int32_t*)377);
	addItem(splitFlapDict, SF_3, (int32_t*)390);
	addItem(splitFlapDict, SF_4, (int32_t*)403);
	addItem(splitFlapDict, SF_5, (int32_t*)416);
	addItem(splitFlapDict, SF_6, (int32_t*)429);
	addItem(splitFlapDict, SF_7, (int32_t*)442);
	addItem(splitFlapDict, SF_8, (int32_t*)455);
	addItem(splitFlapDict, SF_9, (int32_t*)468);
	addItem(splitFlapDict, SF_EXCLAMATION, (int32_t*)481);
	addItem(splitFlapDict, SF_QUESTION, (int32_t*)494);
	addItem(splitFlapDict, SF_COLON, (int32_t*)507);
	addItem(splitFlapDict, SF_SPACE, (int32_t*)0);
}

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;

	/* TODO IMPLEMENT ACTUAL INITIALIZATION of Motor */

	return splitflap;
}

bool SF_MoveMotorToZeroPosition(SF_Handle_t instance){
	int numStepsMoved = 0;

	// 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
	while(SF_GetMagSensorAtZeroPosition((SF_t*)instance) == false && numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND ){
		McuULN2003_IncStep(((SF_t*)instance)->motor);
		McuWait_Waitms(20);
		numStepsMoved++;
	}

	// one more segment if not already one round
	if(numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND){
		for(int i=0; i<SPLITFLAP_STEPS_PER_SEGMENT; i++){
			McuULN2003_IncStep(((SF_t*)instance)->motor);
			McuWait_Waitms(20);
			numStepsMoved++;
		}
	}

	McuULN2003_SetPos(((SF_t*)instance)->motor, 0);
	McuULN2003_PowerOff(((SF_t*)instance)->motor);
	// success if less than one rotation
	return numStepsMoved < SPLITFLAP_STEPS_ONE_ROUND;
}

void SF_MoveSteps(SF_Handle_t instance, int32_t steps){

	// run move with acceleration & deceleration
	McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
	while(steps>0){
		if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true) == 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);
}

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

void SF_MoveToFlap(SF_Handle_t instance, char* flap){
	// get flap pos from dictonary
	int32_t flapPos = (int32_t)getItem(*splitFlapDict, flap);
	// get current motor pos
	int32_t currentPos = McuULN2003_GetPos(((SF_t*)instance)->motor);
	// 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(((SF_t*)instance)->motor, stepsToReachFlap);
	}

}

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

void SF_Deinit(SF_Handle_t instance){
	McuULN2003_DeinitMotor(((SF_t*)instance)->motor);
	McuGPIO_DeinitGPIO(((SF_t*)instance)->magSensor);
}