#include "cube.h"

triPoint vertices[8];
triPoint lineVertices[8];

uint32_t angleX = 0;
uint32_t angleY = 0;
uint32_t angleZ = 0;

void cube_Init() {
    vertices[0].x = -1;
    vertices[0].y = 1;
    vertices[0].z = -1;
    vertices[1].x = 1;
    vertices[1].y = 1;
    vertices[1].z = -1;
    vertices[2].x = 1;
    vertices[2].y = -1;
    vertices[2].z = -1;
    vertices[3].x = -1;
    vertices[3].y = -1;
    vertices[3].z = -1;
    vertices[4].x = -1;
    vertices[4].y = 1;
    vertices[4].z = 1;
    vertices[5].x = 1;
    vertices[5].y = 1;
    vertices[5].z = 1;
    vertices[6].x = 1;
    vertices[6].y = -1;
    vertices[6].z = 1;
    vertices[7].x = -1;
    vertices[7].y = -1;
    vertices[7].z = 1;
}

triPoint rotateX(triPoint startingPoint, uint16_t angle) {
    triPoint computedPoint;
    float rad = angle * 3.141592 / 180;
    float cosa = cos(rad);
    float sina = sin(rad);

    computedPoint.x = startingPoint.x;
    computedPoint.y = (startingPoint.y * cosa) - (startingPoint.z * sina);
    computedPoint.z = (startingPoint.y * sina) + (startingPoint.z * cosa);
    return computedPoint;
}

triPoint rotateY(triPoint startingPoint, uint16_t angle) {
    triPoint computedPoint;
    float rad = angle * 3.141592 / 180;
    float cosa = cos(rad);
    float sina = sin(rad);

    computedPoint.y = startingPoint.y;
    computedPoint.z = (startingPoint.z * cosa) - (startingPoint.x * sina);
    computedPoint.x = (startingPoint.z * sina) + (startingPoint.x * cosa);
    return computedPoint;
}

triPoint rotateZ(triPoint startingPoint, uint16_t angle) {
    triPoint computedPoint;
    float rad = angle * 3.141592 / 180;
    float cosa = cos(rad);
    float sina = sin(rad);

    computedPoint.z = startingPoint.z;
    computedPoint.x = (startingPoint.x * cosa) - (startingPoint.y * sina);
    computedPoint.y = (startingPoint.x * sina) + (startingPoint.y * cosa);
    return computedPoint;
}

triPoint projectPoint(triPoint startingPoint, uint8_t win_width, uint8_t win_height, uint8_t fov, uint8_t viewer_distance) {
    triPoint returnPoint;
    float factor = fov/(viewer_distance + startingPoint.z);

    returnPoint.x = startingPoint.x * factor + win_width/2;
    returnPoint.y = -startingPoint.y * factor + win_height/2;
    returnPoint.z = 1;
    return returnPoint;
}

void cube_Update(uint8_t fov) {
    triPoint calcPoint, newPoint;

    for (uint8_t i = 0; i < 8; i++) {
        calcPoint = rotateZ(vertices[i],angleZ++);
        calcPoint = rotateY(calcPoint,angleY++);
        calcPoint = rotateX(calcPoint,angleX++);
        newPoint = projectPoint(calcPoint, SSD1306_WIDTH, SSD1306_HEIGHT, fov, 5);
        lineVertices[i] = newPoint;
    }

    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[1].x,lineVertices[1].y);
    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[2].x,lineVertices[2].y);
    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[3].x,lineVertices[3].y);
    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[0].x,lineVertices[0].y);
    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[4].x,lineVertices[4].y);
    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[5].x,lineVertices[5].y);
    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[6].x,lineVertices[6].y);
    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[7].x,lineVertices[7].y);
    drawLine(lineVertices[4].x,lineVertices[4].y,lineVertices[5].x,lineVertices[5].y);
    drawLine(lineVertices[5].x,lineVertices[5].y,lineVertices[6].x,lineVertices[6].y);
    drawLine(lineVertices[6].x,lineVertices[6].y,lineVertices[7].x,lineVertices[7].y);
    drawLine(lineVertices[7].x,lineVertices[7].y,lineVertices[4].x,lineVertices[4].y);
}