// 3dcam
// With huge help from :
// @NicolasNoble : https://discord.com/channels/642647820683444236/646765703143227394/796876392670429204
// @Lameguy64
// @Impiaa
// @paul        
 /*        PSX screen coordinate system 
 *
 *                           Z+
 *                          /
 *                         /
 *                        +------X+
 *                       /|
 *                      / |
 *                     /  Y+
 *                   eye        */
// Blender debug mode
// bpy. app. debug = True 
#define _WCHAR_T
#include "../include/psx.h"
#include "../include/pad.h"
#include "../include/math.h"
#include "../include/camera.h"
#include "../include/physics.h"
#include "../include/graphics.h"
#include "../include/space.h"

#define USECD

// START OVERLAY
extern u_long load_all_overlays_here;
extern u_long __lvl0_end;
extern u_long __lvl1_end;
u_long overlaySize = 0;

#include "../levels/level0.h"
#include "../levels/level1.h"

//FIXME : Refresh BGc, lightmatrix on ovly upload...

volatile u_char level = 1;
// level 1 : 8003F05C -2147225508
// level 0 : 800AF744 -2146764988
//           80010000 -2147418112 -> -2147483648
// ovl     : 800b80d4 -2146729772
u_short levelWas = 0;
u_short levelHasChanged = 0;
static char* overlayFile;
// Display and draw environments, double buffered
DISPENV disp[2];
DRAWENV draw[2];
//~ // OT for BG/FG discrimination
u_long otdisc[2][OT2LEN] = {0};
// Main OT
u_long      ot[2][OTLEN]  = {0};                // Ordering table (contains addresses to primitives)
char    primbuff[2][PRIMBUFFLEN] = {0};         // Primitive list // That's our prim buffer
int         primcnt=0;                      // Primitive counter
char * nextpri = primbuff[0];                   // Primitive counter
char            db  = 0;                        // Current buffer counter
CVECTOR BGc = {50, 50, 75, 0};                  // Far color
VECTOR BKc = {128, 128, 128, 0};                // Back color   
MATRIX      rotlgt; 
SVECTOR     lgtang = {0, 0, 0}; 
MATRIX      light;
short vs;
CAMERA camera = {0};
// physics
u_long time = 0;
u_long timeS = 0;
//Pad
Controller_Buffer controllers[2];   // Buffers for reading controllers
Controller_Data theControllers[8];  // Processed controller data
int pressed = 0;
u_short timer = 0;
// Cam stuff 
int camMode = ACTOR;
VECTOR angle     = {250,0,0,0};
VECTOR angleCam  = {0,0,0,0};
int dist      = 150; 
int lerping    = 0;
short curCamAngle = 0;
// Inverted Cam coordinates for Forward Vector calc
VECTOR InvCamPos = {0,0,0,0};
VECTOR fVecActor = {0,0,0,0};
u_long triCount = 0;
// Default level : Initialize everything to 0
MATRIX cmat     = {0}, lgtmat   = {0};
MESH   actorPtr = {0}, levelPtr = {0} , propPtr = {0}, meshes[] = {0};
int    meshes_length = 0;
NODE   curNode  = {0};
CAMPATH camPath = {0};
CAMANGLE camPtr = {0}, camAngles[] = {0};
MESH   meshPlan = {0};
VECTOR modelPlan_pos = {0};
LEVEL curLvl = {
    &BGc,
    &cmat,
    &lgtmat,
    (MESH **)&meshes,
    &meshes_length,
    &actorPtr,
    &levelPtr,
    &propPtr,
    &camPtr,
    &camPath,
    (CAMANGLE **)&camAngles,
    &curNode,
    &meshPlan
};
LEVEL * loadLvl;
// Pad 
void callback();
int main() {
    if ( level == 0 ){
        overlayFile = "\\level0.bin;1";
        overlaySize = __lvl0_end;
        loadLvl     = &level0;
    } else if ( level == 1) {
        overlayFile = "\\level1.bin;1";
        overlaySize = __lvl1_end;
        loadLvl     = &level1;
    }
    // Load overlay
    #ifdef USECD
        CdInit();
        LoadLevelCD(overlayFile, &load_all_overlays_here);
    #endif
    // TODO : Add switch case to get the correct pointers
    // Get needed pointers from level file
    if ( level == 0 ) {
        LvlPtrSet( &curLvl, &level0);
    } else if ( level == 1) {
        LvlPtrSet( &curLvl, &level1);
    } 
    levelWas = level;
    // Overlay 
    VECTOR sp = {CENTERX,CENTERY,0};
    VECTOR wp = {0,0,0};
    // FIXME : Poly subdiv
    //~ DIVPOLYGON4 div4 = { 0 };
    //~ div4.pih = SCREENXRES;
    //~ div4.piv = SCREENYRES;
    //~ div4.ndiv = 2;
    //~ long OTc = 0;
    //~ DIVPOLYGON3 div3 = { 0 };
    //~ div3.pih = SCREENXRES;
    //~ div3.piv = SCREENYRES;
    //~ div3.ndiv = 1;
    init(disp, draw, db, curLvl.cmat, curLvl.BGc, &BKc);
    InitPAD(controllers[0].pad, 34, controllers[1].pad, 34);
    StartPAD();
    generateTable();
    VSyncCallback(callback);
    // Load textures
    for (int k = 0; k < *curLvl.meshes_length ; k++){
        LoadTexture(curLvl.meshes[k]->tim_data, curLvl.meshes[k]->tim);
    }
    // Load current BG
    if (curLvl.camPtr->tim_data){
        LoadTexture(curLvl.camPtr->tim_data, curLvl.camPtr->BGtim);
    }
    // Physics
    short physics = 1;
    long dt;
    VECTOR col_lvl, col_sphere, col_sphere_act = {0};
    // Cam stuff 
    VECTOR posToActor    = {0, 0, 0, 0};      // position of camera relative to actor    
    VECTOR camAngleToAct = {0, 0, 0, 0};      // rotation angles for the camera to point at actor
    // Sprite system
    VECTOR posToCam      = {0, 0, 0, 0};
    VECTOR objAngleToCam = {0, 0, 0, 0};
    //~ int angle     = 0;                      //PSX units = 4096 == 360° = 2Pi
                        //PSX units 
    short timediv = 1;
    int atime = 0;
    // Polycount
    for (int k = 0; k < *curLvl.meshes_length; k++){
            triCount += curLvl.meshes[k]->tmesh->len;
    }
    // Set camera starting pos
    setCameraPos(&camera, curLvl.camPtr->campos->pos, curLvl.camPtr->campos->rot);
    // Find curCamAngle if using pre-calculated BGs
    if (camMode == 2) {                              
        if (curLvl.camPtr->tim_data){
            curCamAngle = 1;
        }
    }
    // Main loop
    //~ while (1) {
    while ( VSync(1) ) {
        timeS = VSync(-1) / 60;
        if ( levelWas != level ){
            switch ( level ){
                case 0:
                    overlayFile = "\\level0.bin;1";
                    overlaySize = __lvl0_end;
                    loadLvl     = &level0;
                    break;
                case 1:
                    overlayFile = "\\level1.bin;1";
                    overlaySize = __lvl1_end;
                    loadLvl     = &level1;
                    break;
                default:
                    overlayFile = "\\level0.bin;1";
                    loadLvl     = &level0;
                    break;
            }
            #ifdef USECD
              LoadLevelCD( overlayFile, &load_all_overlays_here );
            #endif
            SwitchLevel( &curLvl, loadLvl);
            //~ levelHasChanged = 0;
            levelWas = level;
        }
        FntPrint("Ovl:%s\nLvl : %x\nLvl: %d %d \n%x", overlayFile, &level, level, levelWas, loadLvl);
        //~ FntPrint("%x\n", curLvl.actorPtr->tim);
        // Clear the main OT
        ClearOTagR(otdisc[db], OT2LEN);
        // Clear Secondary OT
        ClearOTagR(ot[db], OTLEN);
        // timeB = time;
        time ++;
        // atime is used for animations timing
        timediv = 1;
        if (time % timediv == 0){
            atime ++;
        }
        // Angle between camera and actor
        // using atantable (faster)
        camAngleToAct.vy = (patan(-posToActor.vx, -posToActor.vz) / 16) - 3076 ;
        camAngleToAct.vx = patan(dist, posToActor.vy) >> 4;
        // Sprite system WIP
        objAngleToCam.vy = patan( posToCam.vx,posToCam.vz );
        objAngleToCam.vx = patan( posToCam.vx,posToCam.vy );
        //~ objAngleToCam.vz = patan( posToCam.vz,posToCam.vy );
        //~ objAngleToCam.vx = patan( psqrt(posToCam.vx * posToCam.vx + posToCam.vy * posToCam.vy), posToCam.vy );
        //~ curLvl.meshPlan.rot->vx = -( (objAngleToCam.vx >> 4) - 3076 ) ;
        //~ curLvl.meshPlan.rot->vx = (( (objAngleToCam.vx >> 4) - 3076 ) * ( (objAngleToCam.vz >> 4) - 3076 ) >> 12) * (nsin(posToCam.vz) >> 10 < 0 ? -1 : 1);
        //~ curLvl.meshPlan.rot->vx = ( (objAngleToCam.vx >> 4) - 3076 ) * ( (objAngleToCam.vz >> 4) - 3076 ) >> 12 ;
        curLvl.meshPlan->rot.vy = -( (objAngleToCam.vy >> 4) + 1024 ) ;
        //~ posToCam = getVectorTo(*curLvl.meshPlan.pos, camera.pos);
        //~ posToCam = getVectorTo(camera.pos, *curLvl.meshPlan.pos);
        posToCam.vx = -camera.pos.vx - curLvl.meshPlan->pos.vx ;
        posToCam.vz = -camera.pos.vz - curLvl.meshPlan->pos.vz ;
        posToCam.vy = -camera.pos.vy - curLvl.meshPlan->pos.vy ;
        //~ psqrt(posToCam.vx * posToCam.vx + posToCam.vy * posToCam.vy);
        // Actor Forward vector for 3d relative orientation
        fVecActor = curLvl.actorPtr->pos;
        fVecActor.vx = curLvl.actorPtr->pos.vx + (nsin(curLvl.actorPtr->rot.vy/2));
        fVecActor.vz = curLvl.actorPtr->pos.vz - (ncos(curLvl.actorPtr->rot.vy/2));
    // Camera modes
        if(camMode != 2) {
            camera.rot.vy = camAngleToAct.vy;
            // using csin/ccos, no need for theta
            //~ camera.rot.vy = angle; 
            camera.rot.vx = camAngleToAct.vx;   
        }
        if(camMode < 4 ) {
            lerping = 0;
        }
        // Camera follows actor with lerp for rotations
        if(camMode == 0) {
            dist = 200;
            camera.pos.vx = -(camera.x/ONE);
            camera.pos.vy = -(camera.y/ONE);
            camera.pos.vz = -(camera.z/ONE);
            //~ InvCamPos.vx = camera.x/ONE;
            //~ InvCamPos.vz = camera.z/ONE;
            //~ applyVector(&InvCamPos, -1,-1,-1, *=);
            angle.vy = -(curLvl.actorPtr->rot.vy / 2) + angleCam.vy;
            //~ angle.vx += 10;
            //~ FntPrint("cos %d", (ncos(angle.vy) * ncos(angle.vx)) >> 12);
            //~ angle = curLvl.actorPtr->rot->vy;
            // Camera horizontal position
            getCameraZY(&camera.z, &camera.y, curLvl.actorPtr->pos.vz, curLvl.actorPtr->pos.vy, angle.vx, dist);
            getCameraXZ(&camera.x, &camera.z, curLvl.actorPtr->pos.vx, curLvl.actorPtr->pos.vz, angle.vy, dist);
            //~ getCameraXZY(&camera.x, &camera.z, &camera.y, curLvl.actorPtr->pos.vx, curLvl.actorPtr->pos.vz, curLvl.actorPtr->pos.vy, angle.vy, angle.vx, dist);
        //~ void getCameraXZY(int * x, int * z, int * y, int actorX, int actorZ, int actorY, int angle, int angleX, int distance) {
            // Camera vertical position
            //~ getCameraXZ(&camera.x, &camera.y, curLvl.actorPtr->pos.vx, curLvl.actorPtr->pos.vy, angle, dist);
            // FIXME! camera lerping to pos
            //~ angle += lerp(camera.rot.vy, -curLvl.actorPtr->rot->vy, 128);
            //~ angle = lerpD(camera.rot.vy << 12, curLvl.actorPtr->rot->vy << 12, 1024 << 12) >> 12;
        }
        // Camera rotates continuously around actor
        if (camMode == 1) {                      
            dist = 150;
            camera.pos.vx = -(camera.x/ONE);
            //~ camera.pos.vy = -(camera.y/ONE);
            camera.pos.vy = 100;
            camera.pos.vz = -(camera.z/ONE);
            //~ fVecActor = *curLvl.actorPtr->pos;
            //~ fVecActor.vx = curLvl.actorPtr->pos->vx + (nsin(curLvl.actorPtr->rot->vy));
            //~ fVecActor.vz = curLvl.actorPtr->pos->vz - (ncos(curLvl.actorPtr->rot->vy));
            getCameraXZ(&camera.x, &camera.z, curLvl.actorPtr->pos.vx, curLvl.actorPtr->pos.vz, angle.vy, dist);
            angle.vy += 10;
        }
        // Fixed Camera with actor tracking
        if (camMode == 3) {                              
            // Using precalc sqrt
            dist = psqrt( (posToActor.vx * posToActor.vx ) + (posToActor.vz * posToActor.vz) );
            camera.pos.vx = 190;
            camera.pos.vz = 100;
            camera.pos.vy = 180;
        }
        // Fixed Camera angle
        if (camMode == 2) {                              
            // If BG images exist
            if (curLvl.camPtr->tim_data){
                 checkLineW( &curLvl.camAngles[ curCamAngle ]->fw.v3, &curLvl.camAngles[ curCamAngle ]->fw.v2, curLvl.actorPtr);
                if ( curLvl.camAngles[ curCamAngle ]->fw.v0.vx ) {
                    //~ FntPrint("BL x : %d, y : %d\n", camAngles[ curCamAngle ]->fw.v3.vx, camAngles[ curCamAngle ]->fw.v3.vy); 
                    //~ FntPrint("BR x : %d, y : %d\n", camAngles[ curCamAngle ]->fw.v2.vx, camAngles[ curCamAngle ]->fw.v2.vy); 
                    //~ FntPrint("Pos : %d\n", checkLineW( &camAngles[ curCamAngle ]->fw.v3, &camAngles[ curCamAngle ]->fw.v2, curLvl.actorPtr) );
                    //~ FntPrint("Pos : %d\n", checkLineW( &camAngles[ curCamAngle ]->bw.v2, &camAngles[ curCamAngle ]->bw.v3, curLvl.actorPtr) );
                    // If actor in camAngle->fw area of screen
                    if ( checkLineW( &curLvl.camAngles[ curCamAngle ]->fw.v3, &curLvl.camAngles[ curCamAngle ]->fw.v2, curLvl.actorPtr) == -1  && 
                         ( checkLineW( &curLvl.camAngles[ curCamAngle ]->bw.v2, &curLvl.camAngles[ curCamAngle ]->bw.v3, curLvl.actorPtr) >= 0 
                           ) 
                       ) { 
                        if (curCamAngle < 5) {
                            curCamAngle++;
                            curLvl.camPtr = curLvl.camAngles[ curCamAngle ];
                            LoadTexture(curLvl.camPtr->tim_data, curLvl.camPtr->BGtim);
                        }
                    }
                }
                if ( curLvl.camAngles[ curCamAngle ]->bw.v0.vx ) {
                    //~ FntPrint("BL x : %d, y : %d\n", camAngles[ curCamAngle ]->bw.v3.vx, camAngles[ curCamAngle ]->bw.v3.vy); 
                    //~ FntPrint("BR x : %d, y : %d\n", camAngles[ curCamAngle ]->bw.v2.vx, camAngles[ curCamAngle ]->bw.v2.vy); 
                    //~ // FntPrint("Pos : %d\n", checkLineW( &camAngles[ curCamAngle ]->bw.v2, &camAngles[ curCamAngle ]->bw.v3, curLvl.actorPtr) );
                    // If actor in camAngle->bw area of screen
                    if ( checkLineW( &curLvl.camAngles[ curCamAngle ]->fw.v3, &curLvl.camAngles[ curCamAngle ]->fw.v2, curLvl.actorPtr) >= 0  && 
                         checkLineW( &curLvl.camAngles[ curCamAngle ]->bw.v2, &curLvl.camAngles[ curCamAngle ]->bw.v3, curLvl.actorPtr) == -1 
                       ) {
                        if (curCamAngle > 0) {
                            curCamAngle--;
                            curLvl.camPtr = curLvl.camAngles[ curCamAngle ];
                            LoadTexture(curLvl.camPtr->tim_data, curLvl.camPtr->BGtim);
                        }
                    }
                }
            }
            setCameraPos(&camera, curLvl.camPtr->campos->pos, curLvl.camPtr->campos->rot);
        }
        // Flyby mode with LERP from camStart to camEnd
        if (camMode == 4) {                               
            // If key pos exist for camera
            if (curLvl.camPath->len) {
                // Lerping sequence has not begun
                if (!lerping){
                    // Set cam start position ( first key pos )
                    camera.pos.vx = curLvl.camPath->points[curLvl.camPath->cursor].vx;
                    camera.pos.vy = curLvl.camPath->points[curLvl.camPath->cursor].vy;
                    camera.pos.vz = curLvl.camPath->points[curLvl.camPath->cursor].vz;
                    // Lerping sequence is starting
                    lerping = 1;
                    // Set cam pos index to 0
                    curLvl.camPath->pos = 0;
                    }
                // Pre calculated sqrt ( see psqrt() )
                dist = psqrt( (posToActor.vx * posToActor.vx ) + (posToActor.vz * posToActor.vz));
                // Fixed point precision 2^12 == 4096
                int precision = 12;
                camera.pos.vx = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vx << precision, curLvl.camPath->points[curLvl.camPath->cursor+1].vx << precision, curLvl.camPath->pos << precision) >> precision;
                camera.pos.vy = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vy << precision, curLvl.camPath->points[curLvl.camPath->cursor+1].vy << precision, curLvl.camPath->pos << precision) >> precision;
                camera.pos.vz = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vz << precision, curLvl.camPath->points[curLvl.camPath->cursor+1].vz << precision, curLvl.camPath->pos << precision) >> precision;
                //~ FntPrint("Cam %d, %d\n", (int32_t)curLvl.camPath->points[curLvl.camPath->cursor].vx, curLvl.camPath->points[curLvl.camPath->cursor+1].vx);
                //~ FntPrint("Cam %d, %d, %d\n", camera.pos.vx, camera.pos.vy, camera.pos.vz);
                //~ FntPrint("Theta y: %d x: %d\n", theta.vy, theta.vx);
                //~ FntPrint("Pos: %d Cur: %d\nTheta y: %d x: %d\n", curLvl.camPath->pos, curLvl.camPath->cursor, theta.vy, theta.vx);
                // Linearly increment the lerp factor
                curLvl.camPath->pos += 20;
                // If camera has reached next key pos, reset pos index, move cursor to next key pos
                if (curLvl.camPath->pos > (1 << precision) ){
                    curLvl.camPath->pos = 0;
                    curLvl.camPath->cursor ++;
                }                    
                // Last key pos is reached, reset cursor to first key pos, lerping sequence is over
                if ( curLvl.camPath->cursor == curLvl.camPath->len - 1 ){
                    lerping = 0;
                    curLvl.camPath->cursor = 0;
                }
            } else { 
                // if no key pos exists, switch to next camMode
                camMode ++; }
        }
        // Camera "on a rail" - cam is tracking actor, and moving with constraints on all axis
        if (camMode == 5) {
            // track actor. If theta (actor/cam rotation angle) is above or below an arbitrary angle, 
            // move cam so that the angle doesn't increase/decrease anymore.
            short cameraSpeed = 40;
            if (curLvl.camPath->len) {
            // Lerping sequence has not begun
                if (!lerping){
                    // Set cam start position ( first key pos )
                    camera.pos.vx = curLvl.camPath->points[curLvl.camPath->cursor].vx;
                    camera.pos.vy = curLvl.camPath->points[curLvl.camPath->cursor].vy;
                    camera.pos.vz = curLvl.camPath->points[curLvl.camPath->cursor].vz;
                    // Lerping sequence is starting
                    lerping = 1;
                    // Set cam pos index to 0
                    curLvl.camPath->pos = 0;
                    }
                // Pre calculated sqrt ( see psqrt() )
                dist = psqrt( (posToActor.vx * posToActor.vx ) + (posToActor.vz * posToActor.vz));
                // Fixed point precision 2^12 == 4096
                short precision = 12;
                camera.pos.vx = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vx << precision, curLvl.camPath->points[curLvl.camPath->cursor + 1].vx << precision, curLvl.camPath->pos << precision) >> precision;
                camera.pos.vy = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vy << precision, curLvl.camPath->points[curLvl.camPath->cursor + 1].vy << precision, curLvl.camPath->pos << precision) >> precision;
                camera.pos.vz = lerpD(curLvl.camPath->points[curLvl.camPath->cursor].vz << precision, curLvl.camPath->points[curLvl.camPath->cursor + 1].vz << precision, curLvl.camPath->pos << precision) >> precision;
                //~ FntPrint("%d %d %d %d\n", camAngleToAct.vy, camera.pos.vx, camera.rot.vy, dist);
                // Ony move cam if position is between first curLvl.camPath->vx and last curLvl.camPath->vx
                if ( camAngleToAct.vy < -50 && camera.pos.vx > curLvl.camPath->points[curLvl.camPath->len - 1].vx ) {  
                    // Clamp curLvl.camPath position to cameraSpeed
                    curLvl.camPath->pos += dist < cameraSpeed ? 0 : cameraSpeed ;
                }
                if ( camAngleToAct.vy > 50 && camera.pos.vx > curLvl.camPath->points[curLvl.camPath->cursor].vx ) {  
                    curLvl.camPath->pos -= dist < cameraSpeed ? 0 : cameraSpeed;
                }
                // If camera has reached next key pos, reset pos index, move cursor to next key pos
                if (curLvl.camPath->pos > (1 << precision) ){
                    curLvl.camPath->pos = 0;
                    curLvl.camPath->cursor ++;
                } 
                if (curLvl.camPath->pos < -100 ){
                    curLvl.camPath->pos = 1 << precision;
                    curLvl.camPath->cursor --;
                }                   
                // Last key pos is reached, reset cursor to first key pos, lerping sequence is over
                if ( curLvl.camPath->cursor == curLvl.camPath->len - 1 || curLvl.camPath->cursor < 0 ){
                    lerping = 0;
                    curLvl.camPath->cursor = 0;
                }
            } else { 
                // if no key pos exists, switch to next camMode
                camMode ++;
            }
        }
    // Spatial partitioning
        if (curLvl.curNode){
            for ( int msh = 0; msh < curLvl.curNode->siblings->index; msh ++ ) {
                // Actor
                if ( !getIntCollision( *curLvl.actorPtr->body , *curLvl.curNode->siblings->list[msh]->plane->body).vx &&
                     !getIntCollision( *curLvl.actorPtr->body , *curLvl.curNode->siblings->list[msh]->plane->body).vz )
                {
                    if ( curLvl.curNode != curLvl.curNode->siblings->list[msh] ) {
                        curLvl.curNode = curLvl.curNode->siblings->list[msh];
                        curLvl.levelPtr = curLvl.curNode->plane;
                    }
                }
                // DONTNEED ?
                // Moveable prop
                //~ if ( !getIntCollision( *propPtr->body , *curLvl.curNode->siblings->list[msh]->plane->body).vx &&
                     //~ !getIntCollision( *propPtr->body , *curLvl.curNode->siblings->list[msh]->plane->body).vz ) {
                    //~ if ( propPtr->node != curLvl.curNode->siblings->list[ msh ]){
                        //~ propPtr->node = curLvl.curNode->siblings->list[ msh ];
                    //~ }
                //~ }
                if ( !getIntCollision( *curLvl.propPtr->body , *curLvl.curNode->plane->body).vx &&
                     !getIntCollision( *curLvl.propPtr->body , *curLvl.curNode->plane->body).vz ) {
                    curLvl.propPtr->node = curLvl.curNode;
                }
            }
        }
    // Physics
        if ( physics ) {
            // if(time%1 == 0){
                 for ( int k = 0; k < *curLvl.meshes_length; k ++ ) {
                //~ for ( int k = 0; k < curLvl.curNode->objects->index ; k ++){
                     if ( ( curLvl.meshes[k]->isRigidBody == 1 ) ) {
                    //~ if ( ( *curLvl.curNode->rigidbodies->list[k]->isRigidBody == 1 ) ) {
                        //~ applyAcceleration(curLvl.curNode->rigidbodies->list[k]->body);
                        applyAcceleration( curLvl.meshes[k]->body );
                        // Get col with level                         ( modelgnd_body )        
                        col_lvl = getIntCollision( *curLvl.meshes[k]->body , *curLvl.levelPtr->body );
                        col_sphere = getIntCollision( *curLvl.propPtr->body, *curLvl.propPtr->node->plane->body );
                        // col_sphere = getIntCollision( *propPtr->body, *levelPtr->body );
                        col_sphere_act = getExtCollision( *curLvl.actorPtr->body, *curLvl.propPtr->body );
                        // If no col with ground, fall off
                        if ( col_lvl.vy ) {
                            if ( !col_lvl.vx && !col_lvl.vz ) { 
                                curLvl.actorPtr->body->position.vy = curLvl.actorPtr->body->min.vy;
                            }
                        }
                        if (col_sphere.vy){
                            if ( !col_sphere.vx && !col_sphere.vz ) {
                                curLvl.propPtr->body->position.vy = curLvl.propPtr->body->min.vy;
                            }
                        }
                        if (col_sphere_act.vx && col_sphere_act.vz ) {
                            curLvl.propPtr->body->velocity.vx += curLvl.actorPtr->body->velocity.vx;
                            curLvl.propPtr->body->velocity.vz += curLvl.actorPtr->body->velocity.vz;
                            if ( curLvl.propPtr->isRound && curLvl.propPtr->body->velocity.vx ) {
                                VECTOR L = angularMom( *curLvl.propPtr->body );
                                curLvl.propPtr->rot.vz -= L.vx;
                            }
                            if ( curLvl.propPtr->isRound && curLvl.propPtr->body->velocity.vz ) {
                                VECTOR L = angularMom( *curLvl.propPtr->body );
                                curLvl.propPtr->rot.vx -= L.vz;
                            }
                        }
                        curLvl.meshes[k]->pos.vx = curLvl.meshes[k]->body->position.vx;
                        curLvl.meshes[k]->pos.vy = curLvl.meshes[k]->body->position.vy ;
                        curLvl.meshes[k]->pos.vz = curLvl.meshes[k]->body->position.vz;
                    }
                    curLvl.meshes[k]->body->velocity.vy = 0;
                    curLvl.meshes[k]->body->velocity.vx = 0;
                    curLvl.meshes[k]->body->velocity.vz = 0;
                }
            // }
        }
        if ( (camMode == 2) && (curLvl.camPtr->tim_data ) ) {
            worldToScreen( &curLvl.actorPtr->pos, &curLvl.actorPtr->pos2D );
        }
    // Camera setup 
        // position of cam relative to actor
        posToActor.vx = curLvl.actorPtr->pos.vx + camera.pos.vx;
        posToActor.vz = curLvl.actorPtr->pos.vz + camera.pos.vz;
        posToActor.vy = curLvl.actorPtr->pos.vy + camera.pos.vy;
    // Polygon drawing
        if (curLvl.curNode){
            static long Flag;
            if ( (camMode == 2) && (curLvl.camPtr->tim_data ) ) {
                drawBG(curLvl.camPtr, &nextpri, otdisc[db], &db);
                // Loop on camAngles
                for ( int mesh = 0 ; mesh < curLvl.camAngles[ curCamAngle ]->index; mesh ++ ) {
                    transformMesh(&camera, curLvl.camAngles[curCamAngle]->objects[mesh]);
                    drawPoly(curLvl.camAngles[curCamAngle]->objects[mesh], &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                    //                                                          int * camMode, char ** nextpri, u_long * ot, char * db, DRAWENV * draw)
                }
            }
            else {
                // Draw current node's plane
                drawPoly( curLvl.curNode->plane, &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                // Draw surrounding planes 
                for ( int sibling = 0; sibling < curLvl.curNode->siblings->index; sibling++ ) {
                    drawPoly(curLvl.curNode->siblings->list[ sibling ]->plane, &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                }
                // Draw adjacent planes's children
                for ( int sibling = 0; sibling < curLvl.curNode->siblings->index; sibling++ ) {
                    for ( int object = 0; object < curLvl.curNode->siblings->list[ sibling ]->objects->index; object++ ) {
                        long t = 0;
                        transformMesh(&camera, curLvl.curNode->siblings->list[ sibling ]->objects->list[ object ]);
                        drawPoly( curLvl.curNode->siblings->list[ sibling ]->objects->list[ object ], &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                    }
                }
                // Draw current plane children
                for ( int object = 0; object < curLvl.curNode->objects->index; object++ ) {
                    transformMesh(&camera, curLvl.curNode->objects->list[ object ]);
                    drawPoly( curLvl.curNode->objects->list[ object ], &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                }
                // Draw rigidbodies
                for ( int object = 0; object < curLvl.curNode->rigidbodies->index; object++ ) {
                    transformMesh(&camera, curLvl.curNode->rigidbodies->list[ object ]);
                    drawPoly( curLvl.curNode->rigidbodies->list[ object ], &Flag, atime, &camMode, &nextpri, ot[db], &db, &draw[db]);
                }
            }    
        }
        // Find and apply light rotation matrix
        RotMatrix(&lgtang, &rotlgt);    
        MulMatrix0(curLvl.lgtmat, &rotlgt, &light);
        SetLightMatrix(&light);
        // Set camera
        applyCamera(&camera);
        // Add secondary OT to main OT
        AddPrims(otdisc[db], ot[db] + OTLEN - 1, ot[db]);
        //~ FntPrint("curLvl.curNode : %x\nIndex: %d", curLvl.curNode, curLvl.curNode->siblings->index);
        FntPrint("Time    : %d dt :%d", timeS, dt);
        //~ FntPrint("%d\n", curCamAngle );
        //~ FntPrint("%x\n", primbuff[db]);
        //~ FntPrint("Actor    : %d %d\n", curLvl.actorPtr->pos->vx, curLvl.actorPtr->pos->vy);
        //~ FntPrint("%d %d\n", curLvl.actorPtr->pos->vx, curLvl.actorPtr->pos->vz);
        //~ FntPrint("%d %d\n", curLvl.actorPtr->pos2D.vx + CENTERX, curLvl.actorPtr->pos2D.vy + CENTERY);
        //~ FntPrint(" %d %d %d\n", wp.vx, wp.vy, wp.vz);
        FntFlush(-1);
        display( &disp[db], &draw[db], otdisc[db], primbuff[db], &nextpri, &db);
        //~ display(disp, draw, otdisc[db], primbuff[db], nextpri, db);
        //~ frame = VSync(-1);
    }
    return 0;
}
void callback() {
    // Pad 1
    read_controller( &theControllers[0], &controllers[0].pad[0], 0 );  // Read controllers
    // Pad 2
    read_controller( &theControllers[1], &controllers[1].pad[0], 1 );
    //~ u_short pad = PadRead(0);
    //~ u_short pad = 0;
    u_char PADL = ~theControllers[0].button1;
    u_char PADR = ~theControllers[0].button2;
    static u_short lastPad;
    static short forceApplied = 0;
    int div = 32;
    static int lerpValues[4096 >> 7];
    static short cursor = 0;
    static short angleCamTimer = 0;
    //~ static short curCamAngle = 0;
    if( !lerpValues[0] ) {
        for ( long long i = 0; i < div ; i++ ){
            lerpValues[(div-1)-i] = lerp(-24, -264, easeIn(i));
        }
    }
    if( timer ) {
        timer--;
    }    
    if( cursor ) {
        cursor--;
    }
    if (angleCam.vy > 2048 || angleCam.vy < -2048) {
        angleCam.vy = 0;
    }
    if ( PADR & PadShldR1 && !timer ) {
        if (!curLvl.camPtr->tim_data){
            if(camMode < 6){ 
                    camMode ++;
                    lerping = 0;
            } else {
                setCameraPos(&camera, curLvl.camPtr->campos->pos, curLvl.camPtr->campos->rot);
                curLvl.camPath->cursor = 0;
                camMode = 0;
                lerping = 0;
            }
        } else {
            if (curCamAngle > 4) {
                curCamAngle = 0;
            }
            if (curCamAngle < 5) {
                curCamAngle++;
                curLvl.camPtr = curLvl.camAngles[ curCamAngle ];
                LoadTexture(curLvl.camPtr->tim_data, curLvl.camPtr->BGtim);
            } 
        }
        lastPad = PADR;
        timer = 10;
    }
    //~ if ( !(PADR & PadShldR1) && lastPad & PadShldR1 ) {
        //pressed = 0;
    //~ }
    if ( PADR & PadShldL2 ) {
        lgtang.vy += 32;
    }
    if ( PADR & PadShldL1 ) {
        lgtang.vz += 32;
    }
    if ( PADR & PadUp && !timer ){
        if (curLvl.actorPtr->isPrism){
            curLvl.actorPtr->isPrism = 0;
        } else {
            curLvl.actorPtr->isPrism = 1;
        }
        timer = 10;
        lastPad = PADR;
    }
    if ( PADR & PadDown && !timer ){
        if (curLvl.actorPtr->body->gForce.vy >= 0 && curLvl.actorPtr->body->position.vy >= curLvl.actorPtr->body->min.vy  ){
                forceApplied -= 150;
        }
        cursor = div - 15;
        timer = 30;
        lastPad = PADR;
    }
    if ( !(PADR & PadDown) && lastPad & PadDown ) {
        //~ lastPad = pad;
    }
    if ( PADR & PadLeft && !timer ) {
        if (curLvl.actorPtr->anim->interpolate){
            curLvl.actorPtr->anim->interpolate = 0;
        } else {
            curLvl.actorPtr->anim->interpolate = 1;
        }
        timer = 10;
        lastPad = PADR;
    }
    if (theControllers[0].type == 0x73){
        // Analog stick L up
        if ( theControllers[0].analog3 >= 0 && theControllers[0].analog3 < 108 ) {
            curLvl.actorPtr->body->gForce.vz = getVectorTo(fVecActor, curLvl.actorPtr->pos).vz *  (128 - theControllers[0].analog3 ) >> 15 ;
            curLvl.actorPtr->body->gForce.vx = -getVectorTo(fVecActor, curLvl.actorPtr->pos).vx * (128 - theControllers[0].analog3 ) >> 15 ;
            lastPad = PADL;
        }
        // Analog stick L down
        if ( theControllers[0].analog3 > 168 && theControllers[0].analog3 <= 255 ) {
            curLvl.actorPtr->body->gForce.vz = -getVectorTo(fVecActor, curLvl.actorPtr->pos).vz *  ( theControllers[0].analog3 - 128 ) >> 15 ;
            curLvl.actorPtr->body->gForce.vx = getVectorTo(fVecActor, curLvl.actorPtr->pos).vx * ( theControllers[0].analog3 - 128 ) >> 15 ;
            lastPad = PADL;
        }
        // Analog stick L dead zone
        if ( theControllers[0].analog3 > 108 && theControllers[0].analog3 < 148 ) {
            curLvl.actorPtr->body->gForce.vz = 0;
            curLvl.actorPtr->body->gForce.vx = 0;
        }
        // Analog stick L left
        if ( theControllers[0].analog2 >= 0 && theControllers[0].analog2 < 108 ) {
            curLvl.actorPtr->rot.vy -= ( 40 * ( 128 - theControllers[0].analog2 ) ) >> 7 ;
        }
        // Analog stick L right
        if ( theControllers[0].analog2 > 148 && theControllers[0].analog2 <= 255 ) {
            curLvl.actorPtr->rot.vy += ( 40 * ( theControllers[0].analog2 - 128 ) ) >> 7 ;
        }
    }
    if ( PADL & PadUp ) {
        curLvl.actorPtr->body->gForce.vz = getVectorTo(fVecActor, curLvl.actorPtr->pos).vz >> 8 ;
        curLvl.actorPtr->body->gForce.vx = -getVectorTo(fVecActor, curLvl.actorPtr->pos).vx >> 8 ;
        lastPad = PADL;
    }
    if ( !(PADL & PadUp) && lastPad & PadUp) {
        curLvl.actorPtr->body->gForce.vz = 0;
        curLvl.actorPtr->body->gForce.vx = 0;
        lastPad = PADL;
    }
    if ( PADL & PadDown ) {
        curLvl.actorPtr->body->gForce.vz = -getVectorTo(fVecActor, curLvl.actorPtr->pos).vz >> 8 ;
        curLvl.actorPtr->body->gForce.vx = getVectorTo(fVecActor, curLvl.actorPtr->pos).vx >> 8 ;
        lastPad = PADL;
    }
    if ( !( PADL & PadDown ) && lastPad & PadDown) {
        curLvl.actorPtr->body->gForce.vz = 0;
        curLvl.actorPtr->body->gForce.vx = 0;
        lastPad = PADL;
    }
    if ( PADL & PadLeft ) {
        curLvl.actorPtr->rot.vy -= 32;
        lastPad = PADL;
    }
    if ( PADL & PadRight ) {
        curLvl.actorPtr->rot.vy += 32;
        lastPad = PADL;
    }
    if ( PADL & PadSelect && !timer ) {
        //~ if (!levelHasChanged){
        //~ #ifndef USECD
        printf("load:%p:%08x:%s", &load_all_overlays_here, &level, overlayFile);
        //~ PCload( &load_all_overlays_here, &levelHasChanged, overlayFile );
        //~ #endif
        #ifdef USECD
            level = !level;
            //~ levelHasChanged = 1;
        #endif
        //~ }
        timer = 30;
        lastPad = PADL;
    }
    if( theControllers[0].type == 0x73 && camMode == 0){
        // Cam control - horizontal
        if ( theControllers[0].analog0 >= 0 && theControllers[0].analog0 < 108) {
            angleCam.vy -= ( 16 * ( 128 - theControllers[0].analog0 ) ) >> 7 ;
            angleCamTimer = 120;
        }
        if ( theControllers[0].analog0 > 148 && theControllers[0].analog0 <= 255) {
            angleCam.vy += ( 16 * ( theControllers[0].analog0 - 128 ) ) >> 7 ;
            angleCamTimer = 120;
        }
        if ( theControllers[0].analog0 >= 0 && theControllers[0].analog0 < 108) {
            angleCam.vy -= ( 16 * ( 128 - theControllers[0].analog0 ) ) >> 7 ;
            angleCamTimer = 120;
        }
        if ( theControllers[0].analog0 > 148 && theControllers[0].analog0 <= 255) {
            angleCam.vy += ( 16 * ( theControllers[0].analog0 - 128 ) ) >> 7 ;
            angleCamTimer = 120;
        }
        // Timer to lerp cam back behind actor
        if ( angleCamTimer ){
            angleCamTimer --;
        }
        if (!angleCamTimer && angleCam.vy){
            angleCam.vy += lerp( angleCam.vy, 0, 64 ) == 0 ? 1 : lerp( angleCam.vy, 0, 64 );
        }
    } 
    //~ FntPrint("level :%d", level);
    //~ FntPrint("angleCam :%d %d\n", angleCam.vy, lerp( angleCam.vy, 0, 64) );
    //~ FntPrint( "Pad 1 : %02x\nButtons:%02x %02x, Stick:%02d %02d %02d %02d\n",
            //~ theControllers[0].type,             // Controller type : 0x00 == none,  0x41 == standard, 0x73 == analog/dualshock, 0x12 == mouse, 0x23 == steering wheel, 0x63 == gun, 0x53 == analog joystick
            //~ theControllers[0].button1,          // 
            //~ theControllers[0].button2,
            //~ theControllers[0].analog0,  // R3 hor  : left: 0 7F right: 7F FF   dz 78 83
            //~ theControllers[0].analog1,  // R3 vert :  up : 0 7F down : 7F FF : dz 83 86
            //~ theControllers[0].analog2,  // L3 hor : left : 0 7F right: 7F FF : dz 69 81 68 - 8E 
            //~ theControllers[0].analog3 ); // L3 vert : up : 0 7F down : 7F FF : dz 74 8D
    if ( cursor ) {
        curLvl.actorPtr->body->position.vy = lerpValues[cursor];}
};