#define GL_GLEXT_PROTOTYPES
#define _USE_MATH_DEFINES

#if defined(_WIN32)
    #if defined(__GNUC__)
        #include <cpuid.h>
    #elif defined(_MSC_VER)
        #include <intrin.h>
    #endif
    //#define APIENTRY __stdcall
    #include <windows.h>
    #include <errno.h>
    #define GLEW_STATIC
    #include <GL/glew.h>
    //#include "glew/glew.h"
    //#include <GL/gl.h>
    //#include <GLES2/gl2.h>
    //#include <GLES2/gl2ext.h>
    //#define GLFW_INCLUDE_ES3
	#define GLFW_EXPOSE_NATIVE_WGL
    #define GLFW_EXPOSE_NATIVE_WIN32
#else
    #include <GL/gl.h>
    //#include <GL/glext.h>
	#define GLFW_EXPOSE_NATIVE_X11
	#define GLFW_EXPOSE_NATIVE_GLX
#endif

#define GLFW_INCLUDE_NONE
#include <GLFW/glfw3.h>
#include <GLFW/glfw3native.h>

//#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

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

#include "linmathv2.h"

#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif
#define degToRad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define radToDeg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))
#define CLAMP(x, upper, lower) (MIN(upper, MAX(x, lower)))


#include "custom.h"
#include "player.h"

#include "cjson/cJSON.h"
#include "input.h"
#include "sky.h"
//--#include <string.h>

GLFWmonitor *monitor;
const GLFWvidmode *vmode;
int mX,mY,mW,mH;
int fullScreen = 0,readyFullScreen = 0;

// settings
unsigned int samples = 4;
//unsigned int FOV = 200;
unsigned int FOV = 120; // set in input.c InitializePlayer
//unsigned int FOV = 90;

unsigned int SCR_WIDTH = 800; // auto fit to screen/window
unsigned int SCR_HEIGHT = 600;
float ratio = 0.0f;
float fovrad=0.0f;
float radian=0.0f;
unsigned int lastSW = 0;
unsigned int lastSH = 0;
unsigned int rendMode = 0;

int spamonce=0;

double limitFPS = 30.0;
//double deltaTime = 0, currentTime = 0;
double fixedDeltaTime = 0;
//double lastFrameTime = 0;//, timer = 0;
double lastFixedFrameTime = 0;
double timer = 0;
unsigned int updates = 0, frames = 0;
double renderDeltaTime = 0, renderCurrentTime = 0, renderLastTime = 0;
double updateDeltaTime = 0, updateCurrentTime = 0, updateLastTime = 0;
double updateDeltaLapsed = 0;

float saveTime=1*60; // 1 minute
float saveCount=20.0f; // begin at~, counting down to 0, resets to saveTime

struct Camera *camera = NULL;
struct Player *player = NULL;

unsigned int quadVAO, quadVBO;
unsigned int msaaFB, msaaSample, msaaDepth;//, msaaStencil;
unsigned int cfm_state = 0;
int CreateFrameMsaa() {
    if(cfm_state == 1) {
	    glDeleteFramebuffers(1, &msaaFB);
	    glDeleteTextures(1, &msaaSample);
	    glDeleteTextures(1, &msaaDepth);
	    //glDeleteTextures(1, &msaaStencil);
	    //glDeleteRenderbuffers(1, &rendbufferMSAADepthStencil);
        //printf("deleting old msaa fbo render and textures\n");
    } else { // first load
        //glfwGetFramebufferSize(window, &width, &height);
        // similar quad shaped of two triangles, no color applied
        // used to draw temporary framebuffer for antialiasing
        // this is then the screen you 'draw on' , and show it after~
        float quadVertices[] = {   // vertex attributes for a quad that fills the entire screen in Normalized Device Coordinates.
            // positions   // texCoords
            -1.0f,  1.0f,  0.0f, 1.0f,
            -1.0f, -1.0f,  0.0f, 0.0f,
             1.0f, -1.0f,  1.0f, 0.0f,
        
            -1.0f,  1.0f,  0.0f, 1.0f,
             1.0f, -1.0f,  1.0f, 0.0f,
             1.0f,  1.0f,  1.0f, 1.0f
        };

        // setup screen VAO
        glGenVertexArrays(1, &quadVAO);
        glGenBuffers(1, &quadVBO);
        glBindVertexArray(quadVAO);
        glBindBuffer(GL_ARRAY_BUFFER, quadVBO);
        glBufferData(GL_ARRAY_BUFFER, sizeof(quadVertices), &quadVertices, GL_STATIC_DRAW); // static vertices
        glEnableVertexAttribArray(0);
        glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void*)0);
        glEnableVertexAttribArray(1);
        glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void*)(2 * sizeof(float)));
        glBindBuffer(GL_ARRAY_BUFFER, 0);
        glBindVertexArray(0);
        
        printf("\nInitialization of msaa framebuffer and quadVAO\n");
    }
    cfm_state = 1;

    //printf("Setting up new msaa framebuffer\n");
	glGenFramebuffers(1, &msaaFB);
	glBindFramebuffer(GL_FRAMEBUFFER, msaaFB); //bind both read/write to the target framebuffer

	glGenTextures(1, &msaaSample);
	glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, msaaSample);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);

    //float aniso = 8.0f;
    //glGetFloatv(GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &aniso);
    //glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, aniso);


	glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, samples, GL_RGB, SCR_WIDTH, SCR_HEIGHT, GL_TRUE);
	glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, 0);


    glGenTextures(1, &msaaDepth); // texture object can access in shader
    glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, msaaDepth);
	//glTexStorage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, 1, GL_DEPTH_COMPONENT24, SCR_WIDTH, SCR_HEIGHT, GL_TRUE);
	glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, samples, GL_DEPTH_COMPONENT24, SCR_WIDTH, SCR_HEIGHT, GL_TRUE);
    glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, 0);


    //--// FBO opengl does not supposed separate depth/stencil, use custom shader to render depth only to separate texture instead of stencil?
    //--// stencil currently disabled, use GL_DEPTH_STENCIL to use packed
    //--//
    //--//glGenTextures(1, &msaaStencil); // texture object can access in shader
    //--//glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, msaaStencil);
	//--//glTexStorage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, samples, GL_STENCIL_INDEX8, SCR_WIDTH, SCR_HEIGHT, GL_TRUE);
	//--//glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, samples, GL_STENCIL_INDEX8, SCR_WIDTH, SCR_HEIGHT, GL_TRUE);
	//--//glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, 0);


	//--//glGenRenderbuffers(1, &rendbufferMSAADepthStencil);
	//--//glBindRenderbuffer(GL_RENDERBUFFER, rendbufferMSAADepthStencil);
	//--//glRenderbufferStorageMultisample(GL_RENDERBUFFER, samples, GL_DEPTH24_STENCIL8, SCR_WIDTH, SCR_HEIGHT);
	//--//glBindRenderbuffer(GL_RENDERBUFFER, 0); //unbind the render buffer

	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D_MULTISAMPLE, msaaSample, 0);

    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D_MULTISAMPLE, msaaDepth, 0);
    
    //glFramebufferTexture2D(GL_FRAMEBUFFER, GL_STENCIL_ATTACHMENT, GL_TEXTURE_2D_MULTISAMPLE, msaaStencil, 0);

    //glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_STENCIL_ATTACHMENT, GL_TEXTURE_2D, rendbufferMSAADepthStencil, 0);

	if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
	{
        printf("error on setup of msaa framebuffer\n");
	}
    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    return 0;
}

unsigned int mainFB, mainFBsample, mainFBdepth;
unsigned int cfa_state = 0;
int CreateFrameMain() {
    if(cfa_state == 1) { // fu
	    glDeleteFramebuffers(1, &mainFB);
	    glDeleteTextures(1, &mainFBsample);
	    glDeleteTextures(1, &mainFBdepth); // reset next run
	    //glDeleteRenderbuffers(1, &mainFBdepth); // reset next run
        //cfa_state = 0;
        //printf("deletingo old custom fbo--waterdepth\n");
    }
    cfa_state = 1;

    //printf("setting up new custom fbo--waterdepth\n");
    glGenFramebuffers(1, &mainFB);
	glBindFramebuffer(GL_FRAMEBUFFER, mainFB); //bind both read/write to the target framebuffer

    glGenTextures(1, &mainFBsample); // texture object
    glBindTexture(GL_TEXTURE_2D, mainFBsample);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, SCR_WIDTH, SCR_HEIGHT, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
	glBindTexture(GL_TEXTURE_2D, 0);

    glGenTextures(1, &mainFBdepth); // texture object can access in shader
    glBindTexture(GL_TEXTURE_2D, mainFBdepth);
    //glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32F, SCR_WIDTH, SCR_HEIGHT, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_BYTE, NULL);
    //glTexStorage2D(GL_TEXTURE_2D, 1, GL_DEPTH_COMPONENT32F, SCR_WIDTH, SCR_HEIGHT);
    glTexStorage2D(GL_TEXTURE_2D, 1, GL_DEPTH_COMPONENT24, SCR_WIDTH, SCR_HEIGHT);
    //glBindTexture(GL_TEXTURE_2D, 0);

    //glGenRenderbuffers(1, &mainFBdepth); // renderbuffer object cannot access in shader
    //glBindRenderbuffer(GL_RENDERBUFFER, mainFBdepth);
    //glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT,SCR_WIDTH, SCR_HEIGHT);
    //glBindRenderbuffer(GL_RENDERBUFFER, 0);
  
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, mainFBsample, 0);
//
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, mainFBdepth, 0);
    //glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, mainFBdepth);

	if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
	{
        printf("error on setup of main framebuffer\n");
	}
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    //glBindTexture(GL_TEXTURE_2D, 0);

    return 0;
}

float lerp(float a, float b, float f)
{
    return a * (1.0 - f) + (b * f);
}

// uses global theta phi for main camera controls
Vec3 getDirection(Direction *dir, float theta, float phi) { // dir = return
    float theta_left = theta + 90.0f;
    float phi_up = phi - 90.0f;
    if(theta_left>180.0f) theta_left -=360.0f;
    if(phi_up<-180.0f) phi_up += 360.0f;
   
    // right/left lock to plane 
    //--dir->right->x=sin(90.0f * radian) * cos(theta_left * radian);
    //--dir->right->y=cos(90.0f * radian);
    //--dir->right->z=sin(90.0f * radian) * sin(theta_left * radian);
    dir->right->x=sin(90.0f * radian) * cos(theta_left * radian);
    dir->right->y=cos(90.0f * radian);
    dir->right->z=sin(90.0f * radian) * sin(theta_left * radian);
    
    // up down lock to y
    //--dir->up->x=sin(phi_up * radian) * cos(90.0f * radian);
    //--dir->up->y=cos(phi_up * radian);
    //--dir->up->z=sin(phi_up * radian) * sin(90.0f * radian);
    dir->up->x=sin(phi_up * radian) * cos(theta * radian);
    dir->up->y=cos(phi_up * radian);
    dir->up->z=sin(phi_up * radian) * sin(theta * radian);
    
    //if(moveMode==0) { // free mode
        // forward no lock
        dir->forward->x=sin(phi * radian) * cos(theta * radian);
        dir->forward->y=cos(phi * radian);
        dir->forward->z=sin(phi * radian) * sin(theta * radian);
    //} else if(moveMode==1) { // player mode
    //    // issue locks the cam in
    //    dir->forward->x=-sin(theta * radian);
    //    dir->forward->y=0;
    //    dir->forward->z=cos(theta * radian);
    //}

    Vec3 XZonly={ // for player full directional speed not Y
        cos(theta * radian),
        0,
        sin(theta * radian)
    };
    return XZonly;
}


int saveGame() {
    cJSON *player_position = cJSON_CreateObject();
    cJSON *player_rotation = cJSON_CreateObject();
    cJSON *player_direction = cJSON_CreateObject();
    cJSON *camera_direction = cJSON_CreateObject();
    cJSON *player_data = cJSON_CreateObject();
    cJSON *camera_data = cJSON_CreateObject();
    cJSON *window_data = cJSON_CreateObject();
    cJSON *dev_data = cJSON_CreateObject();
    cJSON *json = cJSON_CreateObject();

    cJSON_AddNumberToObject(player_position,"x",player->transform->position->x);
    cJSON_AddNumberToObject(player_position,"y",player->transform->position->y);
    cJSON_AddNumberToObject(player_position,"z",player->transform->position->z);
    cJSON_AddNumberToObject(player_rotation,"x",player->transform->rotation->x);
    cJSON_AddNumberToObject(player_rotation,"y",player->transform->rotation->y);
    cJSON_AddNumberToObject(player_rotation,"z",player->transform->rotation->z);
    cJSON_AddNumberToObject(player_direction,"x",player->dir->forward->x);
    cJSON_AddNumberToObject(player_direction,"y",player->dir->forward->y);
    cJSON_AddNumberToObject(player_direction,"z",player->dir->forward->z);

    cJSON_AddNumberToObject(camera_direction,"x",camera->dir->forward->x);
    cJSON_AddNumberToObject(camera_direction,"y",camera->dir->forward->x);
    cJSON_AddNumberToObject(camera_direction,"z",camera->dir->forward->x);
    cJSON_AddItemToObject(player_data,"position",player_position);
    cJSON_AddItemToObject(player_data,"rotation",player_rotation);
    cJSON_AddItemToObject(player_data,"direction",player_direction);
    cJSON_AddNumberToObject(player_data,"phi",player->phi);
    cJSON_AddNumberToObject(player_data,"lapseddays",lapsedDays); // sky.c
    cJSON_AddNumberToObject(player_data,"totaltime",totalTime);

    cJSON_AddItemToObject(camera_data,"direction",camera_direction);
    cJSON_AddNumberToObject(camera_data,"theta",camera->theta);
    cJSON_AddNumberToObject(camera_data,"phi",camera->phi);
    cJSON_AddNumberToObject(camera_data,"xoffset",camera->xOffset);
    cJSON_AddNumberToObject(camera_data,"zoom",zoomDist);
    cJSON_AddNumberToObject(camera_data,"lastzoom",lastZoomDist);
    cJSON_AddNumberToObject(camera_data,"fov",FOV);

    cJSON_AddNumberToObject(window_data,"mX",mX);
    cJSON_AddNumberToObject(window_data,"mY",mY);
    cJSON_AddNumberToObject(window_data,"mW",mW);
    cJSON_AddNumberToObject(window_data,"mH",mH);
    cJSON_AddNumberToObject(window_data,"vsync",vsync);
    cJSON_AddNumberToObject(window_data,"fullscreen",fullScreen);

    cJSON_AddNumberToObject(dev_data,"mode",moveMode);
    cJSON_AddNumberToObject(dev_data,"scale",cubeScale);
    cJSON_AddNumberToObject(dev_data,"fontidx",fontIdx);
    cJSON_AddNumberToObject(dev_data,"bounds",boundsTog);
    cJSON_AddNumberToObject(dev_data,"grid",gridOn);
    cJSON_AddNumberToObject(dev_data,"tracking",enableTracking);
    cJSON_AddNumberToObject(dev_data,"sky",hiddenSky);
    cJSON_AddNumberToObject(dev_data,"cloud",hiddenCloud);
    cJSON_AddNumberToObject(dev_data,"terrain",hiddenTerrain);
    cJSON_AddNumberToObject(dev_data,"water",hiddenWater);

    cJSON_AddItemToObject(json,"player",player_data);
    cJSON_AddItemToObject(json,"camera",camera_data);
    cJSON_AddItemToObject(json,"window",window_data);
    cJSON_AddItemToObject(json,"dev",dev_data);

    char *json_str = cJSON_Print(json);

    FILE *fp = fopen("data.json","w");
    if(fp==NULL) {
        printf("error: unable to open file\n");
    } else {
        //--printf("%s\n",json_str);
        fputs(json_str,fp);
        fclose(fp);
    }

    cJSON_free(json_str);
    //cJSON_Delete(camera_direction);
    //cJSON_Delete(player_position);
    //cJSON_Delete(player_data);
    cJSON_Delete(json);

    return 0;
}

int loadGame(FILE *fp) {
    //FILE *fp = fopen("data.json","r");

    char buffer[1024*2]; // save currenty ~917 len
    int len=fread(buffer,1,sizeof(buffer),fp);
    fclose(fp);

    printf("read file buffer len=%d\n",len);

    cJSON *player_data = NULL;
    cJSON *camera_data = NULL;
    cJSON *window_data = NULL;
    cJSON *dev_data = NULL;

    cJSON *player_position = NULL;
    cJSON *player_rotation = NULL;
    cJSON *player_direction = NULL;
    
    cJSON *camera_direction = NULL;

    cJSON *json = cJSON_Parse(buffer);

    if(json==NULL) {
        const char *error_ptr = cJSON_GetErrorPtr();
        if(error_ptr!=NULL) {
            printf("Error: %s\n", error_ptr);
        }
        cJSON_Delete(json);
        return 1;
    }

    //name = cJSON_GetObjectItemCaseSensitive(json, "name");
    //if(cJSON_IsString(name) && (name->valuestring!=NULL)) {
    //    printf("name: %s\n", name->valuestring);
    //}
    player_data = cJSON_GetObjectItemCaseSensitive(json,"player");
    camera_data = cJSON_GetObjectItemCaseSensitive(json,"camera");
    window_data = cJSON_GetObjectItemCaseSensitive(json,"window");
    dev_data = cJSON_GetObjectItemCaseSensitive(json,"dev");

    player_position = cJSON_GetObjectItemCaseSensitive(player_data,"position");
    player_rotation = cJSON_GetObjectItemCaseSensitive(player_data,"rotation");
    player_direction = cJSON_GetObjectItemCaseSensitive(player_data,"direction");

    cJSON *pp_x = cJSON_GetObjectItemCaseSensitive(player_position,"x");
    cJSON *pp_y = cJSON_GetObjectItemCaseSensitive(player_position,"y");
    cJSON *pp_z = cJSON_GetObjectItemCaseSensitive(player_position,"z");
    cJSON *pr_x = cJSON_GetObjectItemCaseSensitive(player_rotation,"x");
    cJSON *pr_y = cJSON_GetObjectItemCaseSensitive(player_rotation,"y");
    cJSON *pr_z = cJSON_GetObjectItemCaseSensitive(player_rotation,"z");
    cJSON *pd_x = cJSON_GetObjectItemCaseSensitive(player_direction,"x");
    cJSON *pd_y = cJSON_GetObjectItemCaseSensitive(player_direction,"y");
    cJSON *pd_z = cJSON_GetObjectItemCaseSensitive(player_direction,"z");
    cJSON *p_phi = cJSON_GetObjectItemCaseSensitive(player_data,"phi");
    cJSON *p_ld = cJSON_GetObjectItemCaseSensitive(player_data,"lapseddays");
    cJSON *p_tt = cJSON_GetObjectItemCaseSensitive(player_data,"totaltime");
    if(cJSON_IsNumber(p_ld)) lapsedDays=p_ld->valuedouble;
    if(cJSON_IsNumber(p_tt)) totalTime=p_tt->valuedouble;
    if(cJSON_IsNumber(p_phi)) player->phi=p_phi->valuedouble;

    if(cJSON_IsNumber(pd_x)) player->dir->forward->x=pd_x->valuedouble;
    if(cJSON_IsNumber(pd_y)) player->dir->forward->y=pd_y->valuedouble;
    if(cJSON_IsNumber(pd_z)) player->dir->forward->z=pd_z->valuedouble;
    if(cJSON_IsNumber(pr_x)) player->transform->rotation->x=pr_x->valuedouble;
    if(cJSON_IsNumber(pr_y)) player->transform->rotation->y=pr_y->valuedouble;
    if(cJSON_IsNumber(pr_z)) player->transform->rotation->z=pr_z->valuedouble;
    if(cJSON_IsNumber(pp_x)) player->transform->position->x=pp_x->valuedouble;
    if(cJSON_IsNumber(pp_y)) player->transform->position->y=pp_y->valuedouble;
    if(cJSON_IsNumber(pp_z)) player->transform->position->z=pp_z->valuedouble;
    
    cJSON *cd_x = cJSON_GetObjectItemCaseSensitive(camera_direction,"x");
    cJSON *cd_y = cJSON_GetObjectItemCaseSensitive(camera_direction,"y");
    cJSON *cd_z = cJSON_GetObjectItemCaseSensitive(camera_direction,"z");
    cJSON *c_theta = cJSON_GetObjectItemCaseSensitive(camera_data,"theta");
    cJSON *c_phi = cJSON_GetObjectItemCaseSensitive(camera_data,"phi");
    cJSON *c_xoffset = cJSON_GetObjectItemCaseSensitive(camera_data,"xoffset");
    cJSON *c_zoom = cJSON_GetObjectItemCaseSensitive(camera_data,"zoom");
    cJSON *c_lastzoom = cJSON_GetObjectItemCaseSensitive(camera_data,"lastzoom");
    cJSON *c_fov = cJSON_GetObjectItemCaseSensitive(camera_data,"fov");
    if(cJSON_IsNumber(c_fov)) FOV=c_fov->valuedouble;
    if(cJSON_IsNumber(c_lastzoom)) lastZoomDist=c_lastzoom->valuedouble;
    if(cJSON_IsNumber(c_zoom)) zoomDist=c_zoom->valuedouble;
    if(cJSON_IsNumber(c_xoffset)) camera->xOffset=c_xoffset->valuedouble;
    if(cJSON_IsNumber(c_phi)) camera->phi=c_phi->valuedouble;
    if(cJSON_IsNumber(c_theta)) camera->theta=c_theta->valuedouble;

    if(cJSON_IsNumber(cd_x)) camera->dir->forward->x=cd_x->valuedouble;
    if(cJSON_IsNumber(cd_y)) camera->dir->forward->y=cd_y->valuedouble;
    if(cJSON_IsNumber(cd_z)) camera->dir->forward->z=cd_z->valuedouble;

    cJSON *w_mx = cJSON_GetObjectItemCaseSensitive(window_data,"mX");
    cJSON *w_my = cJSON_GetObjectItemCaseSensitive(window_data,"mY");
    cJSON *w_mh = cJSON_GetObjectItemCaseSensitive(window_data,"mH");
    cJSON *w_mw = cJSON_GetObjectItemCaseSensitive(window_data,"mW");
    cJSON *w_vs = cJSON_GetObjectItemCaseSensitive(window_data,"vsync");
    cJSON *w_full = cJSON_GetObjectItemCaseSensitive(window_data,"fullscreen");
    
    cJSON *d_mode = cJSON_GetObjectItemCaseSensitive(dev_data,"mode");
    cJSON *d_scale = cJSON_GetObjectItemCaseSensitive(dev_data,"scae");
    cJSON *d_font = cJSON_GetObjectItemCaseSensitive(dev_data,"fontidx");
    cJSON *d_bound = cJSON_GetObjectItemCaseSensitive(dev_data,"bounds");
    cJSON *d_grid = cJSON_GetObjectItemCaseSensitive(dev_data,"grid");
    cJSON *d_track = cJSON_GetObjectItemCaseSensitive(dev_data,"tracking");
    cJSON *d_sky = cJSON_GetObjectItemCaseSensitive(dev_data,"sky");
    cJSON *d_cloud = cJSON_GetObjectItemCaseSensitive(dev_data,"cloud");
    cJSON *d_terrain = cJSON_GetObjectItemCaseSensitive(dev_data,"terrain");
    cJSON *d_water = cJSON_GetObjectItemCaseSensitive(dev_data,"water");

    if(cJSON_IsNumber(d_water)) hiddenWater=d_water->valueint;
    if(cJSON_IsNumber(d_terrain)) hiddenTerrain=d_terrain->valueint;
    if(cJSON_IsNumber(d_sky)) hiddenSky=d_sky->valueint;
    if(cJSON_IsNumber(d_cloud)) hiddenCloud=d_cloud->valueint;
    if(cJSON_IsNumber(d_track)) enableTracking=d_track->valueint;
    if(cJSON_IsNumber(d_grid)) gridOn=d_grid->valueint;
    if(cJSON_IsNumber(d_bound)) boundsTog=d_bound->valueint;
    if(cJSON_IsNumber(d_font)) fontIdx=d_font->valueint;
    if(cJSON_IsNumber(d_scale)) cubeScale=d_scale->valueint;
    if(cJSON_IsNumber(d_mode)) moveMode=d_mode->valueint;

    if(cJSON_IsNumber(w_full)) readyFullScreen=w_full->valueint;
    if(cJSON_IsNumber(w_vs)) vsync=w_vs->valueint;
    if(cJSON_IsNumber(w_mh)) mH=w_mh->valueint;
    if(cJSON_IsNumber(w_mw)) mW=w_mw->valueint;
    if(cJSON_IsNumber(w_my)) mY=w_my->valueint;
    if(cJSON_IsNumber(w_mx)) mX=w_mx->valueint;

    
    //cJSON *camera_direction = cJSON_GetObjectItemCaseSensitive(camera_data,"direction");

    //cJSON_ArrayForEach(itm, player_data) {
    //    cJSON *dev_data = cJSON_GetObjectItemCaseSensitive(pd,"position");
    //    cJSON_ArrayForEach(dd, dev_data) {
    //        
    //    }
    //    cJSON *player_position = cJSON_GetObjectItemCaseSensitive(pd,"position");
    //    cJSON *camera_direction = cJSON_GetObjectItemCaseSensitive(pd,"direction");
    //}

    cJSON_Delete(json);

    return 0;
}

int InitializePlayer() {
    camera->eye->position->x = 0.0f;
    camera->eye->position->y = 0.0f;
    camera->eye->position->z = 1.0f;
    camera->eye->rotation->x = 0.0f;
    camera->eye->rotation->y = 0.0f;
    camera->eye->rotation->z = 0.0f;
    camera->eye->scale->x = 0.0f;
    camera->eye->scale->y = 0.0f;
    camera->eye->scale->z = 0.0f;

    camera->dir->right->x = 0.0f;
    camera->dir->right->y = 0.0f;
    camera->dir->right->z = 0.0f;
    camera->dir->up->x = 0.0f;
    camera->dir->up->y = 0.0f;
    camera->dir->up->z = 0.0f;
    camera->dir->forward->x = 0.0f;
    camera->dir->forward->y = 0.0f;
    camera->dir->forward->z = 0.0f;

    camera->center->x = 0.0f;
    camera->center->y = 0.0f;
    camera->center->z = 0.0f;

    player->transform->position->x = 0.0f;
    player->transform->position->y = 0.0f;
    player->transform->position->z = 0.0f;
    player->transform->rotation->x = 0.0f;
    player->transform->rotation->y = 0.0f;
    player->transform->rotation->z = 0.0f;
    player->transform->scale->x = 0.0f;
    player->transform->scale->y = 0.0f;
    player->transform->scale->z = 0.0f;

    player->dir->right->x = 0.0f;
    player->dir->right->y = 0.0f;
    player->dir->right->z = 0.0f;
    player->dir->up->x = 0.0f;
    player->dir->up->y = 0.0f;
    player->dir->up->z = 0.0f;
    player->dir->forward->x = 0.0f;
    player->dir->forward->y = 0.0f;
    player->dir->forward->z = 0.0f;

    player->phi=90.0f; // used for simming or flying in playmode
                       // movemode=1, changes in player.c with RMB

    camera->theta = 90.0f;
    camera->phi = 90.0f;
    camera->xOffset = 0.0f;

    //player->theta = 90.0f;
    //player->phi = 90.0f;
    zoomDist = 3.0f;
    lastZoomDist = zoomDist; // set in input.c

    return 0;
}

int terrainTexLen=4; // assigned below
unsigned int terrainTextures[20];

int terrainMaskLen=1;
unsigned int terrainMasks[20];

int unloadTerrainTextures() {
    for(int i=0;i<terrainTexLen;++i) {
        glDeleteTextures(1, &terrainTextures[i]);
    }
    for(int i=0;i<terrainMaskLen;++i) {
        glDeleteTextures(1, &terrainMasks[i]);
    }

    return 0;
}

unsigned int loadTexture(char *file) {
    char *path=malloc(sizeof(char)*100);
    sprintf(path,"data/textures/%s",file);

    int width, height, nChannels;
    unsigned char *data = stbi_load(path, &width, &height, &nChannels, 0);

    unsigned int newTex;
    glGenTextures(1, &newTex);
    glBindTexture(GL_TEXTURE_2D, newTex);
    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S,GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T,GL_REPEAT);
    //glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY, 8.0f );
    // expermental no result attempt to remove jitter in distance
    float aniso = 8.0f;
    glGetFloatv(GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &aniso);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, aniso);
    glTexEnvf(GL_LINEAR, GL_TEXTURE_LOD_BIAS, -10);
    //glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
    //glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, data);

    glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP, GL_TRUE); // automatic mipmap
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
    glGenerateMipmap(GL_TEXTURE_2D);
    glBindTexture(GL_TEXTURE_2D, 0);

    stbi_image_free(data); // free after building vertex array
    return newTex;
}

int InitializeTextures() {
    // init textures -- all opengl stuff must be called on same thread the gl context was created or images will appear empty~
    terrainTextures[0] = loadTexture("cubetex.png");
    terrainTextures[1] = loadTexture("gridtex.png");
    terrainTextures[2] = loadTexture("texture8/diffuse/grass1.png"); // grass
    terrainTextures[3] = loadTexture("texture8/diffuse/stone1.png"); // stone
    terrainTextures[4] = loadTexture("texture8/diffuse/rocks3.png"); // road
    terrainTextures[5] = loadTexture("texture7/3d/diffuse/earth1.png"); // dirt
    terrainTextures[6] = loadTexture("texture7/3d/diffuse/sand1.png"); // sand
    terrainTextures[7] = loadTexture("texture7/3d/diffuse/earth5.png"); // grass
    // keep terrainTexLen in sync above for freeing on exit
    
    terrainMasks[0] = loadTexture("mask2.png");
    return 0;
}


//unsigned long upper_power_of_two(unsigned long v)
//https://stackoverflow.com/questions/466204/rounding-up-to-next-power-of-2
//any loop can be 200+ cycles.. avoid ceil(log?
unsigned long to_nearest_pow2(unsigned long v)
{
    v--;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    //v |= v >> 32; // ?
    v++;
    return v;
}

float Q_rsqrt(float number) // # include <stdint.h> // uint32_t
{
  union {
    float    f;
    uint32_t i;
  } conv = { .f = number };
  conv.i  = 0x5f3759df - (conv.i >> 1);
  conv.f *= 1.5F - (number * 0.5F * conv.f * conv.f);
  return conv.f;
}

void printBits(size_t const size, void const * const ptr)
{
    unsigned char *b = (unsigned char*) ptr;
    unsigned char byte;
    int i, j;

    for (i = size-1; i >= 0; i--) {
        for (j = 7; j >= 0; j--) {
            byte = (b[i] >> j) & 1;
            printf("%u", byte);
        }
    }
    puts("");
}

//--char *strdup(const char *s) {
//--    size_t size = strlen(s) + 1;
//--    char *p = malloc(size);
//--    if (p) {
//--        memcpy(p, s, size);
//--    }
//--    return p;
//--}
//--
//--char** split(const char *str, const char *delimiter, size_t *len){
//--    char *text, *p, *first, **array;
//--    int c;
//--    char** ret;
//--
//--    *len = 0;
//--    text=strdup(str);//strdup not standard
//--    if(text==NULL) return NULL;
//--    for(c=0,p=text;NULL!=(p=strtok(p, delimiter));p=NULL, c++)//count item
//--        if(c==0) first=p; //first token top
//--
//--    ret=(char**)malloc(sizeof(char*)*c+1);//+1 for NULL
//--    if(ret==NULL){
//--        free(text);
//--        return NULL;
//--    }
//--    //memmove?
//--    strcpy(text, str+(first-text));//skip until top token
//--    array=ret;
//--    for(p=text;NULL!=(p=strtok(p, delimiter));p=NULL){
//--        *array++=p;
//--    }
//--    *array=NULL;
//--    *len=c;
//--    return ret;
//--}
//--void free4split(char** sa){
//--    char **array=sa;
//--
//--    if(sa!=NULL){
//--        free(array[0]);//for text
//--        free(sa);      //for array
//--    }
//--}
//https://stackoverflow.com/questions/8461170/how-to-remove-spaces-that-are-in-a-string-sentence/8464902#8464902

//int qmode=0;

// This is not in game format, it is in mathematical format.
//Quaternion ToQuaternion(double roll, double pitch, double yaw) {
Quaternion ToQuaternion(Vec3 euler) {
    // Abbreviations for the various angular functions
    //--double cr = cos(euler.z * 0.5); // roll
    //--double sr = sin(euler.z * 0.5); // roll
    //--double cp = cos(euler.x * 0.5); // pitch
    //--double sp = sin(euler.x * 0.5); // pitch
    //--double cy = cos(euler.y * 0.5); // yaw
    //--double sy = sin(euler.y * 0.5); // yaw

    //--Quaternion q;
    //--q.w = cr * cp * cy + sr * sp * sy;
    //--q.x = sr * cp * cy - cr * sp * sy;
    //--q.y = cr * sp * cy + sr * cp * sy;
    //--q.z = cr * cp * sy - sr * sp * cy;
    
 //   double yaw = euler.x;
 //   double pitch = euler.y;
 //   double roll = euler.z;
 //   Vec3 zero = { 0.0f, 0.0f, 0.0f };

 //--   //double theta = angle_between(euler,zero);
  //  Vec3 rot = { 0.0f, 0.0f, 0.0f };
   // rotate_towards(&rot, &euler, &zero);
   //Vec3 rot = euler;
    double yaw = euler.y;
    double pitch = euler.z;
    double roll = 0;
    //yaw = rot.x; pitch = rot.y; roll = 0;
    //printf("qmode: %d, yaw:%f, pitch:%f, roll:%f\n",qmode,yaw,pitch,roll);

    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;
    printf("quat: w:%f, x:%f, y:%f, z:%f\n",q.w,q.x,q.y,q.z);

    return q;
}

// this implementation assumes normalized quaternion
//----Quaternion ToQuaternion(Vec3 dir) {
//----    // Convert direction to quaternion using Euler angles
//----    // Assume dir is a vec3 with (x, y, z)
//----    
//----    //--double pitch = asin(dir.x);    // Pitch angle from z-axis
//----    //--double yaw = atan2(dir.y, dir.z); // Yaw angle from x-axis
//----    //--double roll = 0;                   // Roll angle (assumed to be zero here)
//----   
//----    //--double matZ[3][3] = {
//----    //--    {1, 0, 0},
//----    //--    {0, cos(roll), -sin(roll)},
//----    //--    {0, sin(roll), cos(roll)}
//----    //--};
//----    //--double matX[3][3] = {
//----    //--    {cos(pitch), 0, -sin(pitch)},
//----    //--    {0, 1, 0},
//----    //--    {sin(pitch), 0, cos(pitch)}
//----    //--};
//----    //--double matY[3][3] = {
//----    //--    {cos(yaw), -sin(yaw), 0},
//----    //--    {sin(yaw), cos(yaw), 0},
//----    //--    {0, 0, 1}
//----    //--};
//----
//----    double pitch = asin(dir.z);      // Pitch angle from z-axis
//----    double yaw = atan2(dir.x, dir.y); // Yaw angle from x-axis
//----    double roll = 0;                   // Roll angle (assumed to be zero here)
//----
//----    // Define rotation matrices for each axis
//----    double matX[3][3] = {
//----        {cos(pitch), 0, -sin(pitch)},
//----        {0, 1, 0},
//----        {sin(pitch), 0, cos(pitch)}
//----    };
//----    double matY[3][3] = {
//----        {cos(yaw), -sin(yaw), 0},
//----        {sin(yaw), cos(yaw), 0},
//----        {0, 0, 1}
//----    };
//----    double matZ[3][3] = {
//----        {1, 0, 0},
//----        {0, cos(roll), -sin(roll)},
//----        {0, sin(roll), cos(roll)}
//----    };
//----    
//----    //--// Combine rotation matrices
//----    //--double totalMat[3][3];
//----    //--for (int i = 0; i < 3; i++) {
//----    //--    for (int j = 0; j < 3; j++) {
//----    //--        totalMat[i][j] = matX[2][i] * matY[2][j] +
//----    //--                         matX[1][i] * matZ[2][j];
//----    //--    }
//----    //--}
//----
//----    // Combine rotation matrices in the correct order
//----    double totalMat[3][3];
//----    for (int i = 0; i < 3; i++) {
//----        for (int j = 0; j < 3; j++) {
//----            totalMat[i][j] = matX[i][2] * matY[i][j] + matX[1][i] * matZ[i][j];
//----        }
//----    }
//----     
//----    Quaternion q;
//----    // Convert resulting vector to quaternion (assuming w is the magnitude)
//----    //--double w = Q_rsqrt(dir.x*dir.x + dir.y*dir.y + dir.z*dir.z); // magic number square
//----    // Convert to quaternion by normalizing the transformed vector
//----    double magnitude = sqrt(totalMat[0][0]*totalMat[0][0] + totalMat[0][1]*totalMat[0][1] + totalMat[0][2]*totalMat[0][2]);
//----    //if (w == 0) {
//----    if (magnitude == 0) {
//----        // Handle case where direction is zero
//----        q.x = 0.0f;
//----        q.y = 0.0f;
//----        q.z = 0.0f;
//----        q.w = 1.0f;
//----        return q; // quat identity
//----    }
//----    
//----    //--q.w = w;
//----    //--q.x /= w;
//----    //--q.y /= w;
//----    //--q.z /= w;
//----    // Multiply with direction vector
//----    q.w = totalMat[0][2] / magnitude;
//----    //--q.x = dir.x * totalMat[0][0];
//----    //--q.y = dir.y * totalMat[0][1];
//----    //--q.z = dir.z * totalMat[0][2];
//----    q.x = totalMat[0][0] / magnitude;
//----    q.y = totalMat[0][1] / magnitude;
//----    q.z = -totalMat[0][2] / magnitude; // Using the right-hand rule
//----    
//----    //--q.x = dir.x * totalMat[0][0] * totalMat[1][0] * totalMat[2][0];
//----    //--q.y = dir.y * totalMat[0][1] * totalMat[1][1] * totalMat[2][1];
//----    //--q.z = dir.z * totalMat[0][2] * totalMat[1][2] * totalMat[2][2];
//----    
//----
//----    return q;
//----}

// converts to Euler angles in 3-2-1 sequence
//EulerAngles ToEulerAngles(Quaternion q) {
Vec3 ToEulerAngles(Quaternion q) {
    //EulerAngles euler;
    Vec3 euler;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    euler.z = atan2(sinr_cosp, cosr_cosp); // roll

    // pitch (y-axis rotation)
    double sinp = Q_rsqrt(1 + 2 * (q.w * q.y - q.x * q.z));
    double cosp = Q_rsqrt(1 - 2 * (q.w * q.y - q.x * q.z));
    euler.x = 2 * atan2(sinp, cosp) - M_PI / 2; // pitch

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    euler.y = atan2(siny_cosp, cosy_cosp); // yaw

    return euler;
}

// Matrix multiplication function: Matrix * Vector = Result
void mul_mat3_vec3(Vec3 *r, Mat3 *b, Vec3 a) {
    r->x = a.x * b->x1 + a.y * b->y1 + a.z * b->z1;
    r->y = a.x * b->x2 + a.y * b->y2 + a.z * b->z2;
    r->z = a.x * b->x3 + a.y * b->y3 + a.z * b->z3;
}

// Normalize the vector (though for direction, we don't need to return unit length)
float magnitude(Vec3 *result) {
    float mag = Q_rsqrt(result->x * result->x + result->y * result->y + result->z * result->z);
    if (mag != 0) {
        result->x /= mag;
        result->y /= mag;
        result->z /= mag;
    }
    return mag;
}

// Function to create a scaling matrix
void createScalingMatrix(mat4x4 m, float sx, float sy, float sz) {
    // Initialize to identity
    for (int i=0;i<4;i++)
        for (int j=0;j<4;j++)
            m[i][j] = 0.0f;

    m[0][0] = sx;   // Scale x
    m[1][1] = sy;   // Scale y
    m[2][2] = sz;   // Scale z
    m[3][3] = 1.0f; // Homogeneous coordinate
}

// Function to apply scaling to the model matrix
void applyScalingToModel(mat4x4 model, float sx, float sy, float sz) {
    mat4x4 scalingMatrix;
    createScalingMatrix(scalingMatrix, sx, sy, sz);

    // Multiply scaling with the current model matrix
    // Scaling is applied first, so scalingMatrix * model
    mat4x4 temp;
    //multiplyMat4x4(temp, scalingMatrix, model);
    mat4x4_mul(temp,scalingMatrix,model);

    // Copy the result back to model
    for (int i=0;i<4;i++)
        for (int j=0;j<4;j++)
            model[i][j] = temp[i][j];
}

//Vec3 eulerToDirection(float yaw, float pitch, float roll) {
Vec3 eulerToDirection(Vec3 *erot) {
    Vec3 dir;
    float cy = cos(erot->y);
    float sy = sin(erot->y);
    float cp = cos(erot->x);
    float sp = sin(erot->x);
    float cr = cos(erot->z);
    float sr = sin(erot->z);

    dir.x = cr * sy + sr * sp * cy;
    dir.y = sr * sy - cr * sp * cy;
    dir.z = cp * cy;

    return dir;
}

//Quaternion eulerToQuaternion(float yaw, float pitch, float roll) {
Quaternion eulerToQuaternion(Vec3 *erot) {
    float cy = cos(erot->x * 0.5f);
    float sy = sin(erot->x * 0.5f);
    float cp = cos(erot->y * 0.5f);
    float sp = sin(erot->y * 0.5f);
    float cr = cos(erot->z * 0.5f);
    float sr = sin(erot->z * 0.5f);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * sp * cr - sy * cp * sr;
    q.y = sy * cp * cr + cy * sp * sr;
    q.z = sy * sp * cr - cy * cp * sr;

    return q;
}

Vec3 quaternionToDirection(Quaternion q) {
    Vec3 dir;
    dir.x = 2 * (q.x * q.z + q.w * q.y);
    dir.y = 2 * (q.y * q.z - q.w * q.x);
    dir.z = 1 - 2 * (q.x * q.x + q.y * q.y);
    return dir;
}

// Convert normalized direction vector to quaternion
Quaternion directionToQuaternion(Vec3 D) {
    // World up vector
    Vec3 W = {0.0f, 1.0f, 0.0f};

    // Compute right vector: R = W × D
    Vec3 R = cross(W, D);
    float R_mag = sqrtf(dot(R, R));

    // Handle case where D is parallel to W
    if (R_mag < 1e-6f) {
        Vec3 temp = {1.0f, 0.0f, 0.0f};
        R = cross(D, temp);
        R = normalize(R);
    } else {
        R = normalize(R);
    }

    // Compute up vector: U = D × R
    Vec3 U = cross(D, R);  // U is unit length since D and R are unit and perpendicular

    // Define rotation matrix elements
    float m00 = R.x, m01 = U.x, m02 = D.x;
    float m10 = R.y, m11 = U.y, m12 = D.y;
    float m20 = R.z, m21 = U.z, m22 = D.z;

    // Convert matrix to quaternion
    Quaternion q;
    float tr = m00 + m11 + m22;

    if (tr > 0.0f) {
        float s = sqrtf(tr + 1.0f) * 2.0f;
        q.w = 0.25f * s;
        q.x = (m21 - m12) / s;
        q.y = (m02 - m20) / s;
        q.z = (m10 - m01) / s;
    } else if (m00 > m11 && m00 > m22) {
        float s = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
        q.x = 0.25f * s;
        q.y = (m01 + m10) / s;
        q.z = (m02 + m20) / s;
        q.w = (m21 - m12) / s;
    } else if (m11 > m22) {
        float s = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
        q.y = 0.25f * s;
        q.x = (m01 + m10) / s;
        q.z = (m12 + m21) / s;
        q.w = (m02 - m20) / s;
    } else {
        float s = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
        q.z = 0.25f * s;
        q.x = (m02 + m20) / s;
        q.y = (m12 + m21) / s;
        q.w = (m10 - m01) / s;
    }

    // Normalize quaternion to ensure unit length
    float q_mag = sqrtf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
    q.x /= q_mag;
    q.y /= q_mag;
    q.z /= q_mag;
    q.w /= q_mag;

    return q;
}

double* normalizeVector3(double* v) {
    double eps = 1e-12;
    double len2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
    
    if (len2 < eps) {
        // Return a zero vector
        return malloc(sizeof(double)*3);
    }
    
    double inv_len = Q_rsqrt(len2);
    double* result = malloc(sizeof(double)*3);
    result[0] = v[0] / inv_len;
    result[1] = v[1] / inv_len;
    result[2] = v[2] / inv_len;
    
    return result;
}
// Normalize a vector
Vec3 normalize(Vec3 v) {
    float mag = sqrtf(dot(v, v));
    Vec3 result;
    result.x = v.x / mag;
    result.y = v.y / mag;
    result.z = v.z / mag;
    return result;
}



double dot_product(Vec3 *a, Vec3 *b) {
    return a->x * b->x + a->y * b->y + a->z * b->z;
}
// Compute dot product of two vectors
float dot(Vec3 a, Vec3 b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

void cross_product(Vec3 *result, Vec3 *a, Vec3 *b) {
    result->x = a->y * b->z - a->z * b->y;
    result->y = a->z * b->x - a->x * b->z;
    result->z = a->x * b->y - a->y * b->x;
}
// Compute cross product of two vectors
Vec3 cross(Vec3 a, Vec3 b) {
    Vec3 result;
    result.x = a.y * b.z - a.z * b.y;
    result.y = a.z * b.x - a.x * b.z;
    result.z = a.x * b.y - a.y * b.x;
    return result;
}

// Function to calculate the distance between two points in 3D space
double calculate_distance(Vec3 *p1, Vec3 *p2) {
    double dx = p2->x - p1->x;         // Difference in x coordinates
    double dy = p2->y - p1->y;         // Difference in y coordinates
    double dz = p2->z - p1->z;         // Difference in z coordinates
    double sum_squares = dx * dx + dy * dy + dz * dz; // Sum of squared differences
    double distance = sqrt(sum_squares); // Square root to get distance
    return distance;
}

double vector3_distance(Vec3 *v1, Vec3 *v2) {
    double dx,dy,dz;
    dx=v2->x - v1->x;
    dy=v2->y - v1->y;
    dz=v2->z - v1->z;
    return Q_rsqrt(dx*dx + dy*dy + dz*dz);
}

double vector_length(Vec3 *v) {
    return Q_rsqrt(v->x * v->x + v->y * v->y + v->z * v->z);
}
double quaternion_length(Quaternion *q) {
    return Q_rsqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
}

double angle_between(Vec3 *a, Vec3 *b) {
    double dot = dot_product(a, b);
    double mag_a = vector_length(a);
    double mag_b = vector_length(b);

    if (mag_a == 0 || mag_b == 0) return 0;

    double cos_theta = dot / (mag_a * mag_b);
    double theta = acos(cos_theta);
    return theta;
}

void rotate_towards(Vec3 *rotation, Vec3 *dir, Vec3 *target) {
    double theta = angle_between(dir, target);
    //Vec3 rotation;
    rotation->x = dir->y * sin(theta);
    rotation->y = dir->z * sin(theta);
    rotation->z = dir->x * cos(theta) - dir->y * sin(theta);
    //return rotation;
}

//Optional quaternion implementation for more complex rotations:
//Vec3 quaternion_rotation(Vec3 *q) {
void quaternion_rotation(Quaternion *q, Vec3 *v, float angle) {
    q->w = Q_rsqrt(2 - (v->x*v->x + v->y*v->y));
    q->x = v->x * sin(angle/2);
    q->y = v->y * cos(angle/2);
    q->z = v->z * sin(angle/2);
}

void AngleAxis(Quaternion *q, double angle, Vec3 *axis) {
    q->x += angle * axis->x;
    q->y += angle * axis->y;
    q->z += angle * axis->z;
}

void QuaternionToMat4(mat4x4 M, Quaternion *q)
{
    //based on algorithm on wikipedia
    // http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion
    float w = q->w;
    float x = q->x;
    float y = q->y;
    float z = q->z;

    //float n = q.lengthSquared();
    float n = quaternion_length(q);
    float s =  n == 0?  0 : 2 / n;
    float wx = s * w * x, wy = s * w * y, wz = s * w * z;
    float xx = s * x * x, xy = s * x * y, xz = s * x * z;
    float yy = s * y * y, yz = s * y * z, zz = s * z * z;

    float m[16] = { 1 - (yy + zz),      xy + wz ,      xz - wy ,0,
                         xy - wz , 1 - (xx + zz),      yz + wx ,0,
                         xz + wy ,      yz - wx , 1 - (xx + yy),0,
                               0 ,            0 ,            0 ,1};
    //QMatrix4x4 result =  QMatrix4x4(m,4,4);
    int idx=0;
    for(int i=0;i<4;i++) {
        for(int j=0;j<4;j++) {
            M[i][j] = m[idx];
            idx++;
        }
    }
}

// Convert spherical coordinates to Cartesian coordinates
Vec3 convert_spherical_to_cartesian(double radius, double theta, double phi) {
    Vec3 result = {0, 0, 0};

    if (radius == 0) {
        return result;
    } 
    
    double sin_theta = sin(theta);
    double cos_theta = cos(theta);
    double sin_phi = sin(phi);
    double cos_phi = cos(phi);

    // Convert to Cartesian coordinates
    //result.x = radius * cos_phi * sin_theta;
    //result.y = radius * sin_phi * sin_theta;
    //result.z = radius * cos_theta;
    result.x = radius * sin_phi * cos_theta;
    result.y = radius * cos_phi;
    result.z = radius * sin_phi * sin_theta;

    // Normalize the vector (though for direction, we don't need to return unit length)
    double mag = magnitude(&result);
    if (mag != 0) {
        result.x /= mag;
        result.y /= mag;
        result.z /= mag;
    }

    return result;
}

// Function to convert yaw and pitch angles to a direction vector
Vec3 unit_vector_from_spherical(double theta, double phi) {
    Vec3 dir = convert_spherical_to_cartesian(1, theta, phi);
    
    // Apply Y-axis rotation (pitch)
    // First rotate around Y-axis by phi
    Mat3 y_rot = {
        cos(phi), 0, sin(phi),
        0, 1, 0,
        -sin(phi), 0, cos(phi)
    };

    // Then apply X-axis rotation (azimuth)
    Mat3 x_rot = {
        1, 0, 0,
        0, cos(theta), -sin(theta),
        0, sin(theta), cos(theta)
    };

    //dir = y_rot * dir;
    //dir = x_rot * dir;
    mul_mat3_vec3(&dir,&y_rot,dir);
    mul_mat3_vec3(&dir,&x_rot,dir);

    return dir;
}

Vec3 Vec3_sub(Vec3 a, Vec3 b) {
    Vec3 r;
    r.x=a.x-b.x;
    r.y=a.y-b.y;
    r.z=a.z-b.z;
    return r;
}


