#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(RELAY1_GROUP, RELAY1_PIN, GPIO_PIN_SET);
			} else if (value == 0) {
				HAL_GPIO_WritePin(RELAY1_GROUP, RELAY1_PIN, GPIO_PIN_RESET);
			}
			break;

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

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

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

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

	}
}
} // namespace tasks