/*
 * splitflap.c
 *
 *  Created on: 29.09.2022
 *      Author: jonas
 */

#include "splitflap.h"
#include "McuULN2003.h"
#include <stdbool.h>
#include "McuWait.h"

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;
}

void SF_MoveSteps(SF_Handle_t instance, uint8_t steps){
	McuULN2003_AccelerationStart(((SF_t*)instance)->motor);
	// maybe move callback?
	while(steps>0){
		if(McuULN2003_StepCallback(((SF_t*)instance)->motor, true) == true){
			steps--;
		}
		McuWait_Waitms(1);
	}
	McuULN2003_AccelerationEnd(((SF_t*)instance)->motor);
	// Power off necessary? -> what to do after power off? init again?
	McuULN2003_PowerOff(((SF_t*)instance)->motor);
}

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


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