/*
 * SSL_Display.cpp
 *
 *  Created on: Feb 11, 2024
 *      Author: Gabriel
 */

#include "SSL_Display.hpp"
#include "SSL_GFX.hpp"

SSL_Display::SSL_Display(ILI9341* hDisplay) :
	hDisplay(hDisplay){

}

SSL_Display::~SSL_Display() {

}

// Public methods

int32_t SSL_Display::init() {
	int32_t errors = 0;
	errors += hDisplay->init();
	errors += hDisplay->setBacklight(1);
	RobotStatus status0;
	for(uint32_t i = 0; i<12; i++){
		status0.batteryLevel = 0;
		status0.connected = false;
		status0.robotId = i;
		status0.status = 0;
		status0.team = 0;
		robots[i].setRobotStatus(status0);
	}
	for(uint32_t i = 0; i<12; i++){
		robots[i].draw();
	}
	hQueueDisplay = xQueueCreateStatic(queueDisplayLength, sizeof(RobotStatus), queueDisplayStorage, &queueDisplayStructure);
	hTaskDisplay = xTaskCreateStatic((void(*)(void*))taskDisplayStatic, "taskDisplay", taskDisplayStackSize, (void*)this, 24, taskDisplayStack, &taskDisplayTCB);
	return errors;
}

int32_t SSL_Display::update(RobotStatus status){
	if(xQueueSend(hQueueDisplay, &status, 0)){
		return 0;
	}else{
		// Queue full
		return -1;
	}
}

// Private methods

void SSL_Display::taskDisplayStatic(SSL_Display* obj){
	obj->taskDisplay();
}

void SSL_Display::taskDisplay(){
	RobotStatus status;
	while(true){
		xQueueReceive(hQueueDisplay, &status, portMAX_DELAY);
		robots[status.robotId].setRobotStatus(status);
		robots[status.robotId].draw();
	}
}
