#include <string.h>
#include <stdint.h>
#include <stdbool.h>

#include <PontCoopScheduler.h>
#include <stdlib.h>
#include <main.h>
#include <usart.h>


void my_setup_1() {
	schInit();

}

void my_errorHandler() {
  
}



void blinkRed(void *handle) {
	HAL_GPIO_TogglePin(LED_Red_GPIO_Port, LED_Red_Pin);
}

void blinkGreen(void *handle) {
	HAL_GPIO_TogglePin(LED_Green_GPIO_Port, LED_Green_Pin);
}

void helloWorld(void *handle) {
	static char hello[] = "Hello World\n\r";
	HAL_UART_Transmit_IT(&debugUart, (uint8_t*) hello, strlen(hello));
}

void enableLoop(void *handle) {
	HAL_GPIO_WritePin(Loop_Enable_GPIO_Port, Loop_Enable_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(Loop_Enable_GPIO_Port, Loop_Enable_Pin, GPIO_PIN_RESET);
}

void disableLoop(void *handle) {
	HAL_GPIO_WritePin(Loop_Disable_GPIO_Port, Loop_Disable_Pin, GPIO_PIN_SET);
	HAL_GPIO_WritePin(Loop_Disable_GPIO_Port, Loop_Disable_Pin, GPIO_PIN_RESET);
}

void toggleLoop(void *handle) {
	static bool state = false;

	if (state) {
		disableLoop(NULL);
	} else {
		enableLoop(NULL);
	}
	state ^= true;
}

void my_setup_2() {
    schAdd(blinkRed, NULL, 0, 500);
	schAdd(blinkGreen, NULL, 0, 1000);
	schAdd(helloWorld, NULL, 0, 5000);
	// schAdd(enableLoop, NULL, 10000, 0);
	// schAdd(disableLoop, NULL, 20000, 0);
	schAdd(toggleLoop, NULL, 0, 10000);
}

void my_loop() {
	schExec();
}

void SYSTICK_Callback() {
	schUpdate();
}