#pragma once
#include "gte_instruction.hpp"

namespace JabyEngine {
    namespace GTE {
        static constexpr auto StackSize = 16;

        /*
            RotTrans

            TODO: Can we use gte_stsv instead of gte_stlvnl for writing to a SVECTOR?
                  Do we have to use gte_stflg??
                  Look at: RotTransSV???

            Perform coordinate transformation using a rotation matrix
            input:  Input vector
            output: Output vector
            flag:   flag output
        */
        static void rot_trans(const SVECTOR& input, VECTOR& output, int32_t& flag) {
            ldv0(input);
            rt();
            stlvnl(output);
            stflg(flag);
        }

        /*
            SetRotMatrix

            Sets a 3x3 matrix m as a constant rotation matrix.
            matrix: The rotation matrix to set
        */
        static void set_rot_matrix(const ROTMATRIX& matrix) {
            __asm__ volatile("lw   $12, 0(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("lw   $13, 4(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("ctc2 $12, $0"     :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("ctc2 $13, $1"     :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("lw   $12, 8(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("lw   $13, 12(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("lw   $14, 16(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $12, $2"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $13, $3"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $14, $4"     :: "r"(&matrix) : "$12", "$13", "$14");
        }

        /*
            GetRotMatrix

            Writes the current 3x3 constant rotation matrix to matrix
            (This doesn't require us to use memory clobber)
        */
        static void get_rot_matrix(ROTMATRIX &matrix) {
            __asm__ volatile("cfc2 $12, $0"     :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("cfc2 $13, $1"     :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("sw   $12, 0(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("sw   $13, 4(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("cfc2 $12, $2"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("cfc2 $13, $3"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("cfc2 $14, $4"     :: "r"(&matrix) : "$12", "$13", "$14");            
            __asm__ volatile("sw   $12, 8(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
            __asm__ volatile("sw   $13, 12(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("sw   $14, 16(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
        }

        /*
            SetTransMatrix

            Sets a constant parallel transfer vector specified by m
        */
        static void set_trans_vector(const TRANSFERVECTOR& vector)  {
            __asm__ volatile("lw   $12, 0(%0)" :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("lw   $13, 4(%0)" :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $12, $5"    :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("lw   $14, 8(%0)" :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $13, $6"    :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $14, $7"    :: "r"(&vector) : "$12", "$13", "$14");
        }

        /*
            GetTransMatrix

            Writes the current constant parallel transfer vector to matrix
            (This doesn't require us to use memory clobber)
        */
        static void get_trans_vector(TRANSFERVECTOR& vector) {
            __asm__ volatile("cfc2 $14, $7"    :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("cfc2 $13, $6"    :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("sw   $14, 8(%0)" :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("cfc2 $12, $5"    :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("sw   $13, 4(%0)" :: "r"(&vector) : "$12", "$13", "$14");
            __asm__ volatile("sw   $12, 0(%0)" :: "r"(&vector) : "$12", "$13", "$14");
        }
        
        /*
            ApplyMatrix
            m0: Matrix to apply
            v0: Vector to apply to
            v1: Result
            returns: result

            Applies the matrix to the vector
        */
       SVECTOR& apply_matrix(const MATRIX& m0, const SVECTOR& v0, SVECTOR& v1);

        /*
            MulMatrix0

            m0: first input
            m1: second input
            result: result of multiplication
            returns: result

            Multiplies two matrices m0 and m1.
            The function destroys the constant rotation matrix
        */
        ROTMATRIX& multiply_matrix(const ROTMATRIX& m0, const ROTMATRIX& m1, ROTMATRIX& result);

        /*
            CompMatrix

            m0: first input
            m1: second input
            result: result of computing m0 and m1
            return: returns result
        */
        static MATRIX& comp_matrix(const MATRIX& m0, const MATRIX& m1, MATRIX& result) {
            multiply_matrix(m0.rotation, m1.rotation, result.rotation);
            set_trans_vector(m0.transfer);
            GTE::ldlv0(reinterpret_cast<const VECTOR&>(m1.transfer));
            GTE::rt();
            GTE::stlvnl(reinterpret_cast<VECTOR&>(result.transfer));

			return result;
        } 

        /*
            matrix: first input

            Sets the 3x3 constant rotation matrix and the parallel transfer vector from input 
        */
        void set_matrix(const MATRIX& matrix);

        /*
            returns: current matrix

            Gets the current 3x3 constant rotation matrix and the parallel transfer vector
        */
        MATRIX get_matrix();

        /*
            matrix: optional input

            Pushes the current matrix (rotation and parallel) to an internal stack
            Optional: replaces current matrix (rotation and parallel) with input
        */
        void push_matrix();
        void push_matrix_and_set(const MATRIX& matrix);

        /*
            Restores the previous stored matrix (rotation and parallel)
        */
        MATRIX get_and_pop_matrix();
        void   pop_matrix();

        /*
            SetGeomOffset(ofx,ofy)

            Load GTE-offset.
        */
        static void set_geom_offset(int32_t off_x, int32_t off_y) {
            __asm__ volatile("sll  $12, %0, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
            __asm__ volatile("sll  $13, %1, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
            __asm__ volatile("ctc2 $12, $24"    :: "r"(off_x), "r"(off_y) : "$12", "$13");
            __asm__ volatile("ctc2 $13, $25"    :: "r"(off_x), "r"(off_y) : "$12", "$13");
        }

        /*
            SetGeomScreen(h)
            
            Load distance from viewpoint to screen.
        */
        static void set_geom_screen(int32_t h) {
            __asm__ volatile("ctc2 %0, $26" :: "r"(h));
        }
    }
}