/*
 * 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] = 0xFC;
		colorBoundingBox[2] = 0;
	}else{
		colorBoundingBox[0] = 0xFC;
		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 += drawRobotLid();
	errors += drawRobotTeam();
	errors += drawBall();
	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::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];
			hBuffer[1][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];
			hBuffer[sizeY-2][i][j] = color[j];
		}
	}
	for(uint32_t i=0; i<sizeY; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[i][0][j] = color[j];
			hBuffer[i][1][j] = color[j];
		}
	}
	for(uint32_t i=0; i<sizeY; i++){
		for(uint32_t j=0; j<3; j++){
			hBuffer[i][sizeX-1][j] = color[j];
			hBuffer[i][sizeX-2][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;
	uint8_t colorPurple[3] = {0xFC, 0x00, 0xFC};
	uint8_t colorGreen[3] = {0x00, 0xFC, 0x00};
	uint8_t* colorCircle[4];
	switch(robotStatus.robotId){
	case 0:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorPurple;
		break;
	case 1:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorPurple;
		break;
	case 2:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorPurple;
		break;
	case 3:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorPurple;
		break;
	case 4:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorGreen;
		break;
	case 5:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorGreen;
		break;
	case 6:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorGreen;
		break;
	case 7:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorGreen;
		break;
	case 8:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorGreen;
		break;
	case 9:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorPurple;
		break;
	case 10:
		colorCircle[0] = colorPurple;
		colorCircle[1] = colorPurple;
		colorCircle[2] = colorGreen;
		colorCircle[3] = colorGreen;
		break;
	case 11:
		colorCircle[0] = colorGreen;
		colorCircle[1] = colorGreen;
		colorCircle[2] = colorPurple;
		colorCircle[3] = colorPurple;
		break;
	}
	errors += drawCircle(colorCircle[0], 28, 33, 4);
	errors += drawCircle(colorCircle[1], 52, 33, 4);
	errors += drawCircle(colorCircle[2], 32, 52, 4);
	errors += drawCircle(colorCircle[3], 48, 52, 4);
	return  errors;
}

int32_t SSL_GFX::drawRobotTeam(){
	uint8_t colorTobotTeam[3];
	if(robotStatus.team){
		colorTobotTeam[0] = 0b11111100;
		colorTobotTeam[1] = 0b11111100;
		colorTobotTeam[2] = 0;
	}else{
		colorTobotTeam[0] = 0;
		colorTobotTeam[1] = 0;
		colorTobotTeam[2] = 0b11111100;
	}
	return drawCircle(colorTobotTeam, 40, 40, 5);
}

int32_t SSL_GFX::drawBall() {
	int32_t errors = 0;
	uint8_t hasBallColor[3] = {0xFC, 0xA0, 0x00};
	uint8_t noBallColor[3] = {0x00, 0x00, 0x00};
	if(robotStatus.status&1){
		drawCircle(hasBallColor, 40, 21, 4);
	}else{
		drawCircle(noBallColor, 40, 21, 4);
	}
	return errors;
}

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

int32_t SSL_GFX::drawBatteryBar() {
	uint32_t errors = 0;
	for(uint32_t j = 3; j<7; j++){
		for(uint32_t k = 3; k<77; k++){
			if(robotStatus.batteryLevel*74 > k-3){
				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;
}

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::drawCircle(uint8_t color[3], uint16_t posX, uint16_t posY, uint16_t radius) {
	uint32_t errors = 0;
	uint8_t corners = 3;
	uint8_t delta = 0;
	int16_t x0 = posX;
	int16_t y0 = posY;
	int16_t r = radius;
	int16_t f = 1 - r;
	int16_t ddF_x = 1;
	int16_t ddF_y = -2 * r;
	int16_t x = 0;
	int16_t y = r;
	int16_t px = x;
	int16_t py = y;

	delta++; // Avoid some +1's in the loop

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

	while (x < y) {
		if (f >= 0) {
		  y--;
		  ddF_y += 2;
		  f += ddF_y;
		}
		x++;
		ddF_x += 2;
		f += ddF_x;
		// These checks avoid double-drawing certain lines, important
		// for the SSD1306 library which has an INVERT drawing mode.
		if (x < (y + 1)) {
		  if (corners & 1)
			for(uint32_t i=0; i<2*y + delta; i++){
				hBuffer[i + y0 - y][x0 + x][0] = color[0];
				hBuffer[i + y0 - y][x0 + x][1] = color[1];
				hBuffer[i + y0 - y][x0 + x][2] = color[2];
			}
		  if (corners & 2)
			for(uint32_t i=0; i<2*y + delta; i++){
				hBuffer[i + y0 - y][x0 - x][0] = color[0];
				hBuffer[i + y0 - y][x0 - x][1] = color[1];
				hBuffer[i + y0 - y][x0 - x][2] = color[2];
			}
		}
		if (y != py) {
		  if (corners & 1)
			for(uint32_t i=0; i<2*px + delta; i++){
				hBuffer[i + y0 - px][x0 + py][0] = color[0];
				hBuffer[i + y0 - px][x0 + py][1] = color[1];
				hBuffer[i + y0 - px][x0 + py][2] = color[2];
			}
		  if (corners & 2)
			for(uint32_t i=0; i<2*px + delta; i++){
				hBuffer[i + y0 - px][x0 - py][0] = color[0];
				hBuffer[i + y0 - px][x0 - py][1] = color[1];
				hBuffer[i + y0 - px][x0 - py][2] = color[2];
			}
		  py = y;
		}
		px = x;
	}
	return  errors;
}