/*
 * application.c
 *
 *  Created on: 22.09.2022
 *      Author: jonas
 */

#include "platform.h"
#include "McuRTOS.h"
#include "application.h"
#include "McuWait.h"
#include "McuLED.h"
#include "McuGPIO.h"
#include "McuULN2003.h"
#include "splitflap_pins.h"
#include "splitflap.h"
#include "multi-splitflap.h"
#include "McuLog.h"
#include "shell.h"

/* blue led pins */
#define LED_BLUE_GPIO GPIOC
#define LED_BLUE_PORT PORTC
#define LED_BLUE_PIN 2U


/* vars */
static McuLED_Handle_t LED_blue;
static SF_Handle_t splitflap0, splitflap1;
static uint32_t taskParameter1 = 5;
static TaskHandle_t appTaskHandle = NULL;

/* function declaration */
void configureSplitflaps(void);

/* Application initialization */
void App_Init(void){
	/* configure blue LED */
	McuLED_Config_t config;
	McuLED_GetDefaultConfig(&config);
	config.isLowActive = true;
	config.isOnInit = false;
	config.hw.gpio = LED_BLUE_GPIO;
	config.hw.port = LED_BLUE_PORT;
	config.hw.pin = LED_BLUE_PIN;
	LED_blue = McuLED_InitLed(&config);

	configureSplitflaps();
	MultiSplitFlap_Init(2);
	MultiSplitFlap_AddFlap(splitflap0);
	MultiSplitFlap_AddFlap(splitflap1);
}

/* App Task */
static void App_Task(void* pv){
	McuLog_info("Application Task starting");

	bool initSuccess = MultiSplitFlap_MoveAllToZeroPosition();

#ifndef APP_DEBUG
	if(initSuccess == false)
	{
		// end here
		for(;;) {}
	}
#else
	// fake set to ready
	((SF_t*)splitflap0)->state = SF_STATE_READY;
	((SF_t*)splitflap1)->state = SF_STATE_READY;
#endif

	QueueHandle_t cmdQueueHandle = SHELL_GetShellCmdQueueHandle();;
	Shell_cmd_s cmd;
	char* sentence;
	while(1){
		// if queue recieved something
		if(xQueueReceive(cmdQueueHandle, &cmd, pdMS_TO_TICKS(20)) == pdPASS){
			switch (cmd.shellCmd) {
				case Shell_Init_All_SF:
					MultiSplitFlap_MoveAllToZeroPosition();
				break;

				case Shell_Identify_SF:
					// params[0]=hwId, params[1]= setupId
					MultiSplitFlap_SetHardwareIdentifier(cmd.params[1], cmd.params[0]);
				break;
				case Shell_Display_String_SF:
					sentence = (char*)calloc(cmd.numberOfParams,sizeof(char));
					if(SHELL_Int32ArrayToStringForSF(cmd.params, cmd.numberOfParams, sentence) == ERR_OK){
						McuLog_info("Sentence parsed");
						MultiSplitFlap_Display(sentence);
						free(sentence);
					}else{
						McuLog_error("Invalid String to Display");
					}
					break;
				case Shell_Powerof_rpi:
				default:
					McuLog_error("Not implemented command recieved in App_Task. Command Id was <%i>", (int)cmd.shellCmd);
					break;
			}
		}

		vTaskDelay(pdMS_TO_TICKS(20));
	}


	// wait before moving
	vTaskDelay(pdMS_TO_TICKS(2000));

	/* MULTI SPLIT FLAP TESTING */
	char sentences[][2] = {"HI", "IT", "IS", "ME", "!!" }; // "COLLIN!";
	while(1){
		for(int i = 0; i < sizeof(sentences)/sizeof(sentences[0]); i++){
			MultiSplitFlap_Display(sentences[i]);
			vTaskDelay(pdMS_TO_TICKS(2000));
		}
	}



	// endless loop (no return! this would produce an bkpt0)
	for(;;) {}


	/* SINGLE SPLIT FLAP TESTING */
	// go through the following letters
	char* letters[] = {"J", "O", "N", "A", "S", "!"};
	//char* 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",
	//		"!", "?", ":"};
	McuLog_info("Amount of letters %i", sizeof(letters)/sizeof(letters[0]));

	while(1){
		for(int i = 0; i < sizeof(letters)/sizeof(letters[0]); i++){
			McuWait_Waitms(1000);
			McuLog_info("Moving to letter '%s', position is %i", letters[i], (int)SF_GetMotorPosition(splitflap0));
			SF_MoveToFlap(splitflap0, letters[i]);
			McuLog_info("Position after move is is %i", (int)SF_GetMotorPosition(splitflap0));
		}
	}
}

/* Application run */
void App_Run(void){
	BaseType_t res;

	res = xTaskCreate(		App_Task,
							"appTask",
							900/sizeof(StackType_t),
							&taskParameter1,
							tskIDLE_PRIORITY,
							&appTaskHandle);

	if(res != pdPASS) // task creation not successful?
	{
		McuLog_info("Task creation of app task failed");
		for(;;) {} // Endless loop
	}

    vTaskStartScheduler();
}


/* Application de-initialization */
void App_Deinit(void){

}


void configureSplitflaps(void){
	/* SPLITFLAP 0 */
	/* configure the motor */
	McuULN2003_Config_t motor0Config;
	McuULN2003_GetDefaultConfig(&motor0Config);
	motor0Config.hw[0].gpio = STEPPER_MOTOR0_IN1_GPIO;
	motor0Config.hw[0].port = STEPPER_MOTOR0_IN1_PORT;
	motor0Config.hw[0].pin 	= STEPPER_MOTOR0_IN1_PIN;
	motor0Config.hw[1].gpio = STEPPER_MOTOR0_IN2_GPIO;
	motor0Config.hw[1].port = STEPPER_MOTOR0_IN2_PORT;
	motor0Config.hw[1].pin 	= STEPPER_MOTOR0_IN2_PIN;
	motor0Config.hw[2].gpio = STEPPER_MOTOR0_IN3_GPIO;
	motor0Config.hw[2].port = STEPPER_MOTOR0_IN3_PORT;
	motor0Config.hw[2].pin	= STEPPER_MOTOR0_IN3_PIN;
	motor0Config.hw[3].gpio = STEPPER_MOTOR0_IN4_GPIO;
	motor0Config.hw[3].port = STEPPER_MOTOR0_IN4_PORT;
	motor0Config.hw[3].pin 	= STEPPER_MOTOR0_IN4_PIN;
	motor0Config.inverted 	= true;

	/* configure magnetic sensor for motor 0 */
	McuGPIO_Config_t magSensor0Config;
	McuGPIO_GetDefaultConfig(&magSensor0Config);
	magSensor0Config.hw.gpio 	= MAG_MOTOR0_GPIO;
	magSensor0Config.hw.port 	= MAG_MOTOR0_PORT;
	magSensor0Config.hw.pin 	= MAG_MOTOR0_PIN;
	magSensor0Config.isInput = true;
	magSensor0Config.hw.pull = McuGPIO_PULL_UP;

	/* create config instance for splitflap 0 */
	SF_Config_t sf0Config;
	sf0Config.setupIdentifier = 0;
	sf0Config.hardwareIdentifier = 4;
	sf0Config.magSensorConfig = magSensor0Config;
	sf0Config.motorConfig = motor0Config;

	/* instanciate splitflap 0 */
	splitflap0 = SF_Init(&sf0Config);

    /* SPLITFLAP 1 */
    /* configure the motor */
    McuULN2003_Config_t motor1Config;
    McuULN2003_GetDefaultConfig(&motor1Config);
    motor1Config.hw[0].gpio = STEPPER_MOTOR1_IN1_GPIO;
    motor1Config.hw[0].port = STEPPER_MOTOR1_IN1_PORT;
    motor1Config.hw[0].pin  = STEPPER_MOTOR1_IN1_PIN;
    motor1Config.hw[1].gpio = STEPPER_MOTOR1_IN2_GPIO;
    motor1Config.hw[1].port = STEPPER_MOTOR1_IN2_PORT;
    motor1Config.hw[1].pin  = STEPPER_MOTOR1_IN2_PIN;
    motor1Config.hw[2].gpio = STEPPER_MOTOR1_IN3_GPIO;
    motor1Config.hw[2].port = STEPPER_MOTOR1_IN3_PORT;
    motor1Config.hw[2].pin  = STEPPER_MOTOR1_IN3_PIN;
    motor1Config.hw[3].gpio = STEPPER_MOTOR1_IN4_GPIO;
    motor1Config.hw[3].port = STEPPER_MOTOR1_IN4_PORT;
    motor1Config.hw[3].pin  = STEPPER_MOTOR1_IN4_PIN;
    motor1Config.inverted   = true;

    /* configure magnetic sensor for motor 0 */
    McuGPIO_Config_t magSensor1Config;
    McuGPIO_GetDefaultConfig(&magSensor1Config);
    magSensor1Config.hw.gpio     = MAG_MOTOR1_GPIO;
    magSensor1Config.hw.port     = MAG_MOTOR1_PORT;
    magSensor1Config.hw.pin     = MAG_MOTOR1_PIN;
    magSensor1Config.isInput = true;
    magSensor1Config.hw.pull = McuGPIO_PULL_UP;

    /* create config instance for splitflap 0 */
    SF_Config_t sf1Config;
    sf1Config.setupIdentifier = 1;
	sf1Config.hardwareIdentifier = 22;
    sf1Config.magSensorConfig = magSensor1Config;
    sf1Config.motorConfig = motor1Config;

    /* instanciate splitflap 1 */
    splitflap1 = SF_Init(&sf1Config);
}
