#include <PSX/GTE/gte.hpp>

static int32_t hisin(int32_t value) {
    static constexpr int32_t qN = 13;
    static constexpr int32_t qA = 12;
    static constexpr int32_t B  = 19900;
    static constexpr int32_t C  = 3516;

    const auto c = value << (30 - qN);          // Semi-circle info into carry.

    value -= 1<<qN;                             // sine -> cosine calc
    value  = value       << (31 - qN);          // Mask with PI
    value  = value       >> (31 - qN);          // Note: SIGNED shift! (to qN)
    value  = value*value >> (2*qN - 14);        // x=x^2 To Q14

    auto result = B - (value*C >> 14);              // B - x^2*C
         result = (1 << qA) - (value*result >> 16); // A - x^2*(B-x^2*C)

    return c >= 0 ? result : -result;
}

gte_float sin(deg_t value) {
    return gte_float{.raw = hisin(value.raw)};
}

gte_float cos(deg_t value) {
    return gte_float{.raw = hisin(value.raw + (deg_t::full_circle/4))};
}

namespace JabyEngine {
    namespace GTE {
        namespace MatrixHelper {
            struct SinCosPair {
                int16_t sin;
                int16_t cos;

                static SinCosPair create_for(deg_t value) {
                    return SinCosPair{
                        .sin = static_cast<int16_t>(::sin(value)),
                        .cos = static_cast<int16_t>(::cos(value)),
                    };
                }
            };
        }

        static MATRIX  Stack[StackSize];
        static MATRIX* FreeStackEntry = Stack;

        ROTMATRIX& multiply_matrix(const ROTMATRIX& m0, const ROTMATRIX& m1, ROTMATRIX& result) {
            set_rot_matrix(m0);
            
            JabyEngine::GTE::ldclmv(m1, 0);
            JabyEngine::GTE::rtir();
            JabyEngine::GTE::stclmv(result, 0);
            
            JabyEngine::GTE::ldclmv(m1, 1);
            JabyEngine::GTE::rtir();
            JabyEngine::GTE::stclmv(result, 1);
            
            JabyEngine::GTE::ldclmv(m1, 2);
            JabyEngine::GTE::rtir();
            JabyEngine::GTE::stclmv(result, 2);

            return result;
        }

        void set_matrix(const MATRIX& matrix) {
            set_rot_matrix(matrix.rotation);
            set_trans_vector(matrix.transfer);
        }

        MATRIX get_matrix() {
            MATRIX matrix;

            get_rot_matrix(matrix.rotation);
            get_trans_vector(matrix.transfer);
            return matrix;
        }

        void push_matrix() {
            *FreeStackEntry = get_matrix();
            FreeStackEntry++;
        }

        void push_matrix_and_set(const MATRIX& matrix) {
            push_matrix();
            set_matrix(matrix);
        }

        void pop_matrix() {
            FreeStackEntry--;
            set_matrix(*FreeStackEntry);
        }

        MATRIX get_and_pop_matrix() {
            const auto matrix = get_matrix();
            pop_matrix();

            return matrix;
        }

        ROTMATRIX ROTMATRIX :: rotated(deg_t x, deg_t y, deg_t z) {
            using namespace MatrixHelper;
            
            const auto sincos_x = SinCosPair::create_for(x);
            const auto sincos_y = SinCosPair::create_for(y);
            const auto sincos_z = SinCosPair::create_for(z);
            
            auto rotX = ROTMATRIX::identity();
            rotX.matrix[1][1] = sincos_x.cos; rotX.matrix[1][2] = -sincos_x.sin;
            rotX.matrix[2][1] = sincos_x.sin; rotX.matrix[2][2] =  sincos_x.cos;

            auto rotY = ROTMATRIX::identity();
            rotY.matrix[0][0] =  sincos_y.cos; rotY.matrix[0][2] = sincos_y.sin;
            rotY.matrix[2][0] = -sincos_y.sin; rotY.matrix[2][2] = sincos_y.cos;

            auto rotZ = ROTMATRIX::identity();
            rotZ.matrix[0][0] = sincos_z.cos; rotZ.matrix[0][1] = -sincos_z.sin;
            rotZ.matrix[1][0] = sincos_z.sin; rotZ.matrix[1][1] =  sincos_z.cos;

            push_matrix();
            multiply_matrix(rotX, rotY, rotX);
            multiply_matrix(rotX, rotZ, rotX);
            pop_matrix();

            return rotX;
        }
    }
}