// SPDX-License-Identifier: CC0-1.0
//
// SPDX-FileContributor: Antonio Niño Díaz, 2022

#include <stdint.h>
#include <stdlib.h>

#define GBA_SCREEN_W            240
#define GBA_SCREEN_H            160

#define REG_DISPCNT             *((volatile uint16_t *)0x04000000)

#define REG_KEYINPUT        *((volatile uint16_t *)0x04000130)
#define DEFAULT_REG_KEYINPUT    *((volatile uint16_t *)0x04000130)
#define KEY_DOWN_NOW(key)  (~(REG_KEYINPUT) & key)

#define KEY_A           1
#define KEY_B           2
#define KEY_SELECT      3
#define KEY_START       4
#define KEY_DPAD_RIGHT      5
#define KEY_DPAD_LEFT       6
#define KEY_DPAD_UP     7
#define KEY_DPAD_DOWN       8
#define KEY_TRIGGER_LEFT    9
#define KEY_TRIGGER_RIGHT   10

#define DISPCNT_BG_MODE_MASK    (0x7)
#define DISPCNT_BG_MODE(n)      ((n) & DISPCNT_BG_MODE_MASK) // 0 to 5

#define DISPCNT_BG2_ENABLE      (1 << 10)

#define MEM_VRAM_MODE4       ((uint8_t *)buffer)

#define SHOW_BACK 0x10;
#define FRONT_BUFFER (0x6000000)
#define BACK_BUFFER  (0x600A000)

#define PALETTE ((uint16_t *)0x5000000)
static volatile uint8_t lastPaletteIndex = 0;

#define FIXED_POINT int32_t
#define FP 12
#define SHIFT_THRESHOLD 0.05
#define SHIFT_THRESHOLD_FP ((1 << FP) * SHIFT_THRESHOLD)

#define FLOAT2FIXED(value)     (int)((value) * (1 << FP))
#define FIXED2FLOAT(value)     ((value) / (float)(1 << FP))

static uint8_t *buffer = (uint8_t *)FRONT_BUFFER;

static inline uint16_t
RGB15(uint16_t r, uint16_t g, uint16_t b)
{
    return (r & 0x1F) | ((g & 0x1F) << 5) | ((b & 0x1F) << 10);
}

void
flipBuffers()
{
    if(buffer == (uint8_t *)FRONT_BUFFER) {
        REG_DISPCNT &= ~SHOW_BACK;
        buffer = (uint8_t *)BACK_BUFFER;
    } else {
        REG_DISPCNT |= SHOW_BACK;
        buffer = (uint8_t *)FRONT_BUFFER;
    }
}

void
putPx(uint16_t x, uint16_t y, uint8_t c)
{
    uint16_t pos = (GBA_SCREEN_W * y + x);
    buffer[pos] = c;
}

///////////////////////////////////////////////////////////

#include <math.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <float.h>

#define VWIDTH 25
#define VHEIGHT 25
#define CUBE_WIDTH 10
#define CUBE_WIDTH_FP ((1 << FP) * CUBE_WIDTH)

enum faces {
    FACE_FRONT = 0,
    FACE_LEFT,
    FACE_RIGHT,
    FACE_BOTTOM,
    FACE_TOP,
    FACE_BACK,
    NUM_FACES,
};

#define STEP 20
#define STEP_FP ((1 << FP) * STEP)

#define ACTION_STEP 0.1
#define ACTION_STEP_FP ((1 << FP) * ACTION_STEP)

#define SCALE 3 // how much is our initial render scaled

volatile FIXED_POINT  K1 = 20;
volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 10;

#define MULT_FP(a,b) ((a * b) >> FP)

#define SQ(n) (n * n)
#define SQ_FP(n) (MULT_FP(n, n))

#define PLOT_COORD(x, y, c) \
    for (uint8_t i = 0 ; i < SCALE ; ++i) \
        for (uint8_t j = 0 ; j < SCALE ; ++j) \
            putPx(x * SCALE + i, j + (SCALE * y), chooseColor(c));

#define GET_ROTATE_X_Q(a) ({ float _a = (a) ; \
        struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
        q.x = FLOAT2FIXED(sin(_a * .5)); q; })
#define GET_ROTATE_Y_Q(a) ({ float _a = (a) ; \
        struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
        q.y = FLOAT2FIXED(sin(_a * .5)); q; })
#define GET_ROTATE_Z_Q(a) ({ float _a = (a) ; \
        struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
        q.z = FLOAT2FIXED(sin(_a * .5)); q; })

//TODO Idle animations
#define IS_IDLE (Idle.x || Idle.y || Idle.z)
#define RESET_IDLE {Idle.x = 0; Idle.y = 0; Idle.z = 0;}

void
init_colors()
{
    const uint16_t color_order[NUM_FACES + 1] = {
        RGB15(0,  0,  0),
        RGB15(31, 0,  0),
        RGB15(0,  31, 0),
        RGB15(0,  0,  31),
        RGB15(0,  31, 31),
        RGB15(31, 31, 0),
        RGB15(31, 0,  31),
    };

    for ( ; lastPaletteIndex <= NUM_FACES ; ++lastPaletteIndex)
           PALETTE[lastPaletteIndex] = color_order[lastPaletteIndex];
}

struct {
    uint8_t x;
    uint8_t y;
    uint8_t z;
} Idle;


struct Quaternions {
    FIXED_POINT w;
    FIXED_POINT x;
    FIXED_POINT y;
    FIXED_POINT z;
} Target, Current;

static FIXED_POINT      interpolationStep = 0;
static volatile uint16_t            vertices[NUM_FACES * 2 * 4];
static volatile FIXED_POINT         zBuffer[NUM_FACES * 2];

static volatile uint8_t shouldBreak = 1;
static volatile uint8_t currentlyMoving = 0;
static volatile uint8_t currentCountR = 0;
static volatile uint8_t frontFacingFace = FACE_FRONT;

void
normalize(struct Quaternions *q)
{
    float n = sqrt(FIXED2FLOAT(SQ_FP(q->w) + SQ_FP(q->x) +
                SQ_FP(q->y) + SQ_FP(q->z)));
    if (n == 0 || n == 1)
        return;

    q->w = FLOAT2FIXED(FIXED2FLOAT(q->w) / n);
    q->x = FLOAT2FIXED(FIXED2FLOAT(q->x) / n);
    q->y = FLOAT2FIXED(FIXED2FLOAT(q->y) / n);
    q->z = FLOAT2FIXED(FIXED2FLOAT(q->z) / n);
}

struct Quaternions
mult(struct Quaternions *q, FIXED_POINT x, FIXED_POINT y, FIXED_POINT z)
{
    //p = q * p * qbar
    struct Quaternions res;

    res.w = 0;
    res.x = MULT_FP(x, (SQ_FP(q->w) + SQ_FP(q->x) - SQ_FP(q->y) - SQ_FP(q->z)))
        + (MULT_FP(y, (MULT_FP(q->x, q->y) - MULT_FP(q->w, q->z))) * 2)
        + (MULT_FP(z, (MULT_FP(q->x, q->z) + MULT_FP(q->w, q->y))) * 2);

    res.y = (MULT_FP(x, (MULT_FP(q->x, q->y) + MULT_FP(q->w,q->z))) * 2)
        + (MULT_FP(y, (SQ_FP(q->w) - SQ_FP(q->x) + SQ_FP(q->y) - SQ_FP(q->z))))
        + (MULT_FP(z, (MULT_FP(q->y, q->z) - MULT_FP(q->w, q->x))) << 2);

    res.z = (MULT_FP(x, (MULT_FP(q->x, q->z) - MULT_FP(q->w, q->y)))* 2)
        + (MULT_FP(y, (MULT_FP(q->y, q->z) + MULT_FP(q->w, q->x))) * 2)
        + MULT_FP(z, (SQ_FP(q->w) - SQ_FP(q->x) - SQ_FP(q->y) + SQ_FP(q->z)));

    return res;
}

// res in quat p
void
multQ(struct Quaternions *p, struct Quaternions *q)
{
    if (p->x <= SHIFT_THRESHOLD_FP && p->x >= -SHIFT_THRESHOLD_FP
            && p->y <= SHIFT_THRESHOLD_FP && p->y >= -SHIFT_THRESHOLD_FP
            && p->z <= SHIFT_THRESHOLD_FP && p->z >= -SHIFT_THRESHOLD_FP) {
        p = q;
        return;
    }

    if (q->x <= SHIFT_THRESHOLD_FP && q->x >= -SHIFT_THRESHOLD_FP
            && q->y <= SHIFT_THRESHOLD_FP && q->y >= -SHIFT_THRESHOLD_FP
            && q->z <= SHIFT_THRESHOLD_FP && q->z >= -SHIFT_THRESHOLD_FP)
        return;

    FIXED_POINT w = MULT_FP(p->w, q->w) - MULT_FP(p->x, q->x) -
                    MULT_FP(p->y, q->y) - MULT_FP(p->z, q->z);
    FIXED_POINT x = MULT_FP(p->w, q->x) + MULT_FP(p->x, q->w) +
                    MULT_FP(p->y, q->z) - MULT_FP(p->z, q->y);
    FIXED_POINT y = MULT_FP(p->w, q->y) - MULT_FP(p->x, q->z) +
                    MULT_FP(p->y, q->w) + MULT_FP(p->z, q->x);
    FIXED_POINT z = MULT_FP(p->w, q->z) + MULT_FP(p->x, q->y) -
                    MULT_FP(p->y, q->x) + MULT_FP(p->z, q->w);
    p->w = w;
    p->x = x;
    p->y = y;
    p->z = z;
}


uint16_t
chooseColor(uint8_t c)
{
    if (c >= 0 && c < NUM_FACES)
        return c + 1; // palette 0 is bg
    else
        return 0;
}

uint8_t
chooseMainFace()
{
    int total = 0;
    int faces[NUM_FACES] = {0};

    for (int k = 0; k < VWIDTH * VHEIGHT; ++k)
        if (vertices[k] >= 0 && vertices[k] < NUM_FACES) {
            faces[vertices[k]]++;
            ++total;
        }

    int max = 0, idx = 0;
    for (int k = 0; k < NUM_FACES; ++k)
        if (faces[k] > max) {
            max = faces[k];
            idx = k;
        }

    frontFacingFace = max > total * 0.9 ? idx : -1;
    return frontFacingFace;
}

// ISSUE the 4 vertices rendered do not make a convex quad
// we have to switch 2 vertices for that
#pragma GCC
uint8_t
isInQuad(const uint16_t x, const uint16_t y, uint8_t current_face)
{
    uint16_t *points = (uint16_t *)&vertices[current_face * 8];
    // little hack

    uint16_t tmpX = points[6], tmpY = points[7];
    points[6] = points[4];
    points[7] = points[5];
    points[4] = tmpX;
    points[5] = tmpY;

    uint8_t pos = 0, neg = 0;
    int32_t d;

    for (uint8_t i = 0; i < 4; ++i) {
        if (points[2 * i] == x && points[2 * i + 1] == y)
            return 1;

        //Form a segment between the i'th point
        int32_t x1 = points[2 * i];
        int32_t y1 = points[2 * i + 1];

        //And the i+1'th, or if i is the last, with the first point
        uint8_t i2 = (i + 1) % 4;

        int32_t x2 = points[2 * i2];
        int32_t y2 = points[2 * i2 + 1];


        //Compute the cross product
        d = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);

        if (d > 0) ++pos;
        if (d < 0) ++neg;

        //If the sign changes, then point is outside
        if (pos > 0 && neg > 0)
            return 0;
    }

    return 1;
}

void
fillQuads(uint8_t current_face)
{
    uint16_t top = UINT16_MAX, bot = 0, left = UINT16_MAX, right = 0;
    for (uint8_t k = 0 ; k < 8 ; ++k) {
        const uint16_t item = vertices[current_face * 8 + k];

        if (k & 1) {
            if (item > bot)
                bot = item;
            if (item < top)
                top = item;
        } else {
            if (item > right)
                right = item;
            if (item < left)
                left = item;
        }
    }

    for (uint16_t y = top ; y <= bot ; ++y) {
        for (uint16_t x = left ; x <= right ; ++x) {
            if (isInQuad(x, y, current_face))
                PLOT_COORD(x, y, current_face);
        }
    }
}

uint8_t
detect(uint8_t current_face)
{
    for (uint8_t k = current_face * 8; k < current_face * 8 + 8; ++k)
        if (vertices[k] == UINT16_MAX)
            return 0;

    return 1;
}

int
comp(const void *p1, const void *p2) {
     FIXED_POINT left  = *(const FIXED_POINT *)p1;
     FIXED_POINT right = *(const FIXED_POINT *)p2;

     return ((left > right) - (left < right));
}

void
detectAndFillQuads()
{
    qsort((FIXED_POINT *)zBuffer, NUM_FACES, 2 * sizeof(FIXED_POINT), comp);
    for (uint8_t idx = 0 ; idx < NUM_FACES ; ++idx) {
        char ch = zBuffer[2 * idx + 1];
        if (detect(ch))
            fillQuads(ch);
    }
}

void
printAscii()
{
    detectAndFillQuads();
    flipBuffers();
}

void
rotateCube(FIXED_POINT cubeX, FIXED_POINT cubeY, FIXED_POINT cubeZ, uint8_t ch)
{
    struct Quaternions q = mult(&Current, cubeX, cubeY, cubeZ);

    uint32_t x = q.x >> FP;
    uint32_t y = q.y >> FP;

    // not fixed point yet!!
    float invZ = (1 << FP) / (float)(q.z + K2 * (1 << FP));

    int32_t screenX = CUBE_WIDTH * 2 + (int32_t)((x) * K1) * invZ;
    int32_t screenY = CUBE_WIDTH * 2 + (int32_t)((y) * K1) * invZ;
    //TODO luminescence

    if (screenX > GBA_SCREEN_W || screenX < 0
            || screenY > GBA_SCREEN_H || screenY < 0) return;

    FIXED_POINT invZFixed = FLOAT2FIXED(invZ);
    uint8_t firstEmptyVertex = ch * 8;
    for ( ; vertices[firstEmptyVertex] != UINT16_MAX
            && firstEmptyVertex - ch * 8 < 8 ; firstEmptyVertex+=2);

    vertices[firstEmptyVertex] = screenX;
    vertices[firstEmptyVertex + 1] = screenY;

    if (zBuffer[2 * ch] < invZFixed) {
        zBuffer[2 * ch] = invZFixed;
        zBuffer[2 * ch + 1] = ch;
    }
}

void
interpolate(struct Quaternions *qa, struct Quaternions *qb)
{
    frontFacingFace = -1;
    float cosHalfTheta =
        FIXED2FLOAT(MULT_FP(qa->w, qb->w) +
        MULT_FP(qa->x, qb->x) +
        MULT_FP(qa->y, qb->y) +
        MULT_FP(qa->z, qb->z));
    //if qa = qb or qa = -qb then theta = 0 and we can return qa
    if (cosHalfTheta >= 1.0 || cosHalfTheta <= -1.0) {
        goto exit;
    }
    if (cosHalfTheta < 0) {
        qb->w = -qb->w;
        qb->x = -qb->x;
        qb->y = -qb->y;
        qb->z = qb->z;
        cosHalfTheta = -cosHalfTheta;
    }

    float       halfTheta = acos(cosHalfTheta);
    float       sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
    //if theta = 180 degrees then result is not fully defined
    // we could rotate around any axis normal to qa or qb
    if (sinHalfTheta < 0.01 && sinHalfTheta > -0.01) {
        qa->w = ((qa->w >> 1) + (qb->w >> 1));
        qa->x = ((qa->x >> 1) + (qb->x >> 1));
        qa->y = ((qa->y >> 1) + (qb->y >> 1));
        qa->z = ((qa->z >> 1) + (qb->z >> 1));
        goto exit;
    }


    FIXED_POINT ratioA = FLOAT2FIXED(sin((1 - FIXED2FLOAT(interpolationStep)) * halfTheta) / sinHalfTheta);
    FIXED_POINT ratioB = FLOAT2FIXED(sin(FIXED2FLOAT(interpolationStep) * halfTheta) / sinHalfTheta);

    qa->w = (MULT_FP(qa->w, ratioA) + MULT_FP(qb->w, ratioB));
    qa->x = (MULT_FP(qa->x, ratioA) + MULT_FP(qb->x, ratioB));
    qa->y = (MULT_FP(qa->y, ratioA) + MULT_FP(qb->y, ratioB));
    qa->z = (MULT_FP(qa->z, ratioA) + MULT_FP(qb->z, ratioB));

exit:
    interpolationStep += ACTION_STEP_FP;
}

void
handleAngle(uint8_t input)
{
    // TODO
    if (currentlyMoving == 0) {
        currentlyMoving = input;
        struct Quaternions tmp;
        switch (input) {
        case 'w':
        case 'W':
            tmp = GET_ROTATE_X_Q(M_PI_2);
            break;
        case 'a':
        case 'A':
            tmp = GET_ROTATE_Y_Q(-M_PI_2);
            break;
        case 's':
        case 'S':
            tmp = GET_ROTATE_X_Q(-M_PI_2);
            break;
        case 'd':
        case 'D':
            tmp = GET_ROTATE_Y_Q(M_PI_2);
            break;
        case 'q':
        case 'Q':
            tmp = GET_ROTATE_Z_Q(-M_PI_2);
            break;
        case 'e':
        case 'E':
            tmp = GET_ROTATE_Z_Q(M_PI_2);
            break;
        default:
            currentlyMoving = 0;
            return;
            //TODO idle movement
        }
        multQ(&tmp, &Target);
        Target = tmp;
        normalize(&Target);
    } else {
        if (interpolationStep < (1 << FP) ) {
            interpolate(&Current, &Target);
            normalize(&Current);
        }
        else {
            Current = Target;
            interpolationStep = 0;
            currentlyMoving = 0;
        }
    }
}

uint8_t
getInput()
{
    // TODO
    uint8_t c = 'd';
    handleAngle(c);
    return c;
}

int
main()
{
    REG_DISPCNT = DISPCNT_BG_MODE(4) | DISPCNT_BG2_ENABLE;
    Current = GET_ROTATE_Z_Q(0);
    init_colors();

    while (1) {
        memset(MEM_VRAM_MODE4, 0, GBA_SCREEN_H * GBA_SCREEN_W);
        memset((uint16_t *)vertices, UINT16_MAX, sizeof(uint16_t) * NUM_FACES * 4 * 2);
        //zBuff is not correct data struct, need a map or smth
        memset((uint16_t *)zBuffer, 0, NUM_FACES * 2 * sizeof(FIXED_POINT));

        for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + 1 * (1 << FP);
                cubeX <= CUBE_WIDTH_FP - 1 * (1 << FP); cubeX += STEP_FP - 2 * (1 << FP)) {
            for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + 1 * (1 << FP);
                    cubeY <= CUBE_WIDTH_FP - 1 * (1 << FP); cubeY += STEP_FP - 2 * (1 << FP)) {
                switch (frontFacingFace) {
                case FACE_FRONT:
                    rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT);
                    break;
                case FACE_LEFT:
                    rotateCube(-CUBE_WIDTH_FP, cubeX, cubeY, FACE_LEFT);
                    break;
                case FACE_RIGHT:
                    rotateCube(CUBE_WIDTH_FP, cubeX, cubeY, FACE_RIGHT);
                    break;
                case FACE_BOTTOM:
                    rotateCube(cubeX, -CUBE_WIDTH_FP, cubeY, FACE_BOTTOM);
                    break;
                case FACE_TOP:
                    rotateCube(cubeX, CUBE_WIDTH_FP, cubeY, FACE_TOP);
                    break;
                case FACE_BACK:
                    rotateCube(cubeX, cubeY, CUBE_WIDTH_FP, FACE_BACK);
                    break;
                default: // idk render all
                    rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT);
                    rotateCube(-CUBE_WIDTH_FP, cubeX, cubeY, FACE_LEFT);
                    rotateCube(CUBE_WIDTH_FP, cubeX, cubeY, FACE_RIGHT);
                    rotateCube(cubeX, -CUBE_WIDTH_FP, cubeY, FACE_TOP);
                    rotateCube(cubeX, CUBE_WIDTH_FP, cubeY, FACE_BOTTOM);
                    rotateCube(cubeX, cubeY, CUBE_WIDTH_FP, FACE_BACK);
                }
            }
        }

        printAscii();
        getInput();
    }
}