#pragma once
#include "gte_instruction.hpp"

namespace JabyEngine {
    namespace GTE {
        /*
            RotTrans

            Jaby: 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 MATRIX& 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");
        }

        /*
            SetTransMatrix

            Sets a constant parallel transfer vector specified by m
        */
        static void set_trans_matrix(const MATRIX& matrix) {
            __asm__ volatile("lw   $12, 20(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("lw   $13, 24(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $12, $5"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("lw   $14, 28(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $13, $6"     :: "r"(&matrix) : "$12", "$13", "$14");
            __asm__ volatile("ctc2 $14, $7"     :: "r"(&matrix) : "$12", "$13", "$14");
        }

        /*
            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));
        }
    }
}