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

#include "SSL_GFX.hpp"
#include "BMplain_font.h"

SSL_GFX::SSL_GFX(ILI9341* hDisplay, uint16_t posX, uint16_t posY, uint8_t hBuffer[][sizeX][3], uint32_t size) :
hDisplay(hDisplay), posX(posX), posY(posY), hBuffer(hBuffer), size(size){

}

SSL_GFX::~SSL_GFX() {

}

int32_t SSL_GFX::init() {
	int32_t errors = 0;
	return errors;
}

int32_t SSL_GFX::draw() {
	if(size != sizeY*sizeX*3){
		// Invalid parameter
		return -1;
	}
	int32_t errors = 0;
	uint8_t colorBoundingBox[3];
	if(robotStatus.connected){
		colorBoundingBox[0] = 0;
		colorBoundingBox[1] = 0b11111100;
		colorBoundingBox[2] = 0;
	}else{
		colorBoundingBox[0] = 0b11111100;
		colorBoundingBox[1] = 0;
		colorBoundingBox[2] = 0;
	}
	errors += drawBoundingBox(colorBoundingBox);
	uint8_t colorRobot[3] = {0b11111100, 0b11111100, 0b11111100};
	errors += drawRobotSilhouette(colorRobot);
	errors += drawId(colorRobot);
	errors += drawBatteryBar();
	errors += hDisplay->setPosition(posX, posY, sizeX, sizeY);
	errors += hDisplay->write((uint8_t*)hBuffer, 19200);
	return errors;
}

int32_t SSL_GFX::setRobotStatus(RobotStatus status) {
	robotStatus = status;
	return 0;
}

// Private methods

int32_t SSL_GFX::drawText(uint8_t color[3], uint16_t posX, uint16_t posY, uint8_t* string, uint32_t size) {
	if(posX+6*size > sizeX){
		// Out of bounds
		return -1;
	}
	if(posY+6 > sizeY){
		// Out of bounds
		return -1;
	}
	for(uint32_t i = 0; i<size; i++){
		for(uint32_t j = 0; j<6; j++){
			for(uint32_t k = 0; k<6; k++){
				if((font[string[i]-32][k]>>(j))&1){
					hBuffer[j+posY][k+posX][0] = color[0];
					hBuffer[j+posY][k+posX][1] = color[1];
					hBuffer[j+posY][k+posX][2] = color[2];
				}else{
					hBuffer[j+posY][k+posX][0] = 0;
					hBuffer[j+posY][k+posX][1] = 0;
					hBuffer[j+posY][k+posX][2] = 0;
				}
			}
		}
		posX += 6;
	}
	return 0;
}

int32_t SSL_GFX::drawBoundingBox(uint8_t color[3]) {
	for(uint32_t i=0; i<sizeX; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[0][i][j] = color[j];
		}
	}
	for(uint32_t i=0; i<sizeX; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[sizeY-1][i][j] = color[j];
		}
	}
	for(uint32_t i=0; i<sizeX; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[i][0][j] = color[j];
		}
	}
	for(uint32_t i=0; i<sizeX; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[i][sizeX-1][j] = color[j];
		}
	}
	return 0;
}

int32_t SSL_GFX::drawRobotSilhouette(uint8_t color[3]) {
	uint16_t x0 = 40;
	uint16_t y0 = 40;
	uint16_t r = 20;
	int16_t f = 1 - r;
	int16_t ddF_x = 1;
	int16_t ddF_y = -2 * r;
	int16_t x = 0;
	int16_t y = r;

	for(uint32_t i=0; i<3; i++){
		hBuffer[y0 + r][x0][i] = color[i];
		//hBuffer[y0 - r][x0][i] = color[i];
		hBuffer[y0][x0 + r][i] = color[i];
		hBuffer[y0][x0 - r][i] = color[i];
	}

	while (x < y) {
		if (f >= 0) {
			y--;
			ddF_y += 2;
			f += ddF_y;
		}
		x++;
		ddF_x += 2;
		f += ddF_x;

		for(uint32_t i=0; i<3; i++){
			hBuffer[y0 + y][x0 + x][i] = color[i];
			hBuffer[y0 + y][x0 - x][i] = color[i];
			//hBuffer[y0 - y][x0 + x][i] = color[i];
			//hBuffer[y0 - y][x0 - x][i] = color[i];
			hBuffer[y0 + x][x0 + y][i] = color[i];
			hBuffer[y0 + x][x0 - y][i] = color[i];
			hBuffer[y0 - x][x0 + y][i] = color[i];
			hBuffer[y0 - x][x0 - y][i] = color[i];
		}
	}
	while(x>=0){
		for(uint32_t i=0; i<3; i++){
			hBuffer[y0 - y][x0 + x][i] = color[i];
			hBuffer[y0 - y][x0 - x][i] = color[i];
		}
		x--;
	}
	return 0;
}

int32_t SSL_GFX::drawRobotLid() {
	uint32_t errors = 0;
	return  errors;
}

int32_t SSL_GFX::drawId(uint8_t color[3]) {
	uint32_t errors = 0;
	errors += drawText(color, 2, 72, (uint8_t*)"ID = ", 5);
	if(robotStatus.robotId<10){
		uint8_t robotIdChar = robotStatus.robotId+48;
		errors += drawText(color, 32, 72, &robotIdChar, 1);
	}else{
		uint8_t robotIdChar[2] = {49, robotStatus.robotId+(uint8_t)38};
		errors += drawText(color, 32, 72, robotIdChar, 2);
	}
	return errors;
}

int32_t SSL_GFX::drawBatteryBar() {
	uint32_t errors = 0;
	for(uint32_t j = 2; j<6; j++){
		for(uint32_t k = 2; k<78; k++){
			if(robotStatus.batteryLevel*76 > k){
				if(robotStatus.batteryLevel>0.5){
					hBuffer[j][k][0] = ((uint8_t)(512-robotStatus.batteryLevel*512))&0xFC;
					hBuffer[j][k][1] = 255&0xFC;
					hBuffer[j][k][2] = 0;
				}else{
					hBuffer[j][k][0] = 255&0xFC;
					hBuffer[j][k][1] = ((uint8_t)(robotStatus.batteryLevel*511))&0xFC;
					hBuffer[j][k][2] = 0;
				}
			}else{
				hBuffer[j][k][0] = 0;
				hBuffer[j][k][1] = 0;
				hBuffer[j][k][2] = 0;
			}
		}
	}
	return  errors;
}
