#include <tasks/relay.hpp>
#include <sta/debug/debug.hpp>
#include <sta/rtos/debug/heap_stats.hpp>
#include "can.h"
#include <sta/tacos.hpp>

#include <cmsis_os2.h>

namespace tasks {
RelayTask::RelayTask(uint32_t canID) :
		TacosThread("RELAY", osPriorityNormal) {
	setCanID(canID);
}

void RelayTask::init() {
}

void RelayTask::func() {
	// Receiving polling message
	CanSysMsg msg;

	if (CAN_queue_.get(&msg, osWaitForever)) {

		uint8_t dev = msg.payload[0];
		uint8_t value = msg.payload[1];
		switch (dev) {
		case 1:
			if (value == 1) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, GPIO_PIN_RESET);
			}
			break;

		case 2:
			if (value == 1) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
			}
			break;

		case 3:
			if (value == 1) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);
			}
			break;

		case 4:
			if (value == 1) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);
			}
			break;

		case 5:
			if (value == 1) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
			}
			break;
		}

	}
}
} // namespace tasks