#include <math.h>

#include "stb/stb_ds.h"

#include "fluid/queue/boundsolver.h"
#include "fluid/queue/chunkmask.h"
#include "fluid/queue/chunk.h"
#include "fluid/env/environment.h"
#include "fluid/env/utilities.h"
#include "fluid/sim/pressurecell/density.h"
#include "fluid/sim/pressurecell/velocity.h"
#include "fluid/sim/pressurecell/solver_consts.h"
#include "math/ode/multigrid.h"
#include "../../../util/chunk_test_utils.h"
#include "../../../util/test.h"

/**
 * Error margin for tests
 */
#define FLUID_PRESSURE_CELL_ERROR_MARGIN 0.00001f

/**
 * Number of chunks
 */
#define CHUNK_DIM 4

/**
 * Testing diffusing values
 */
int fluid_sim_pressurecell_diffuse_test1(){
    printf("fluid_sim_pressurecell_diffuse_test1\n");
    int rVal = 0;
    Environment * env = fluid_environment_create();
    Chunk ** queue = NULL;
    queue = createChunkGrid(env,CHUNK_DIM,CHUNK_DIM,CHUNK_DIM);
    int chunkCount = arrlen(queue);



    //setup chunk values
    Chunk * currentChunk = queue[0];
    currentChunk->d[CENTER_LOC][IX(4,4,4)] = MAX_FLUID_VALUE;

    //actually simulate
    pressurecell_diffuse_density(env,currentChunk);

    //test the result
    float expected, actual;

    //
    // cell that originall had values
    //
    expected = MAX_FLUID_VALUE - FLUID_PRESSURECELL_DIFFUSION_CONSTANT * 6 * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(4,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (4,4,4)!  expected: %f    actual: %f  \n");
    }


    //
    // neighbors
    //
    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(3,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (3,4,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(5,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN * env->consts.dt){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (5,4,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(4,3,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (4,3,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(4,5,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (4,5,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(4,4,3)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (4,4,3)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->dTempCache[IX(4,4,5)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse density correctly (4,4,5)!  expected: %f    actual: %f  \n");
    }

    return rVal;
}

/**
 * Testing diffusing values
 */
int fluid_sim_pressurecell_diffuse_test2(){
    printf("fluid_sim_pressurecell_diffuse_test2\n");
    int rVal = 0;
    Environment * env = fluid_environment_create();
    Chunk ** queue = NULL;
    queue = createChunkGrid(env,CHUNK_DIM,CHUNK_DIM,CHUNK_DIM);
    int chunkCount = arrlen(queue);



    //setup chunk values
    Chunk * currentChunk = queue[0];
    currentChunk->u[CENTER_LOC][IX(4,4,4)] = FLUID_PRESSURECELL_MAX_VELOCITY;
    currentChunk->v[CENTER_LOC][IX(4,4,4)] = FLUID_PRESSURECELL_MAX_VELOCITY;
    currentChunk->w[CENTER_LOC][IX(4,4,4)] = FLUID_PRESSURECELL_MAX_VELOCITY;

    //actually simulate
    pressurecell_diffuse_velocity(env,currentChunk);

    //test the result
    float expected, actual;
    //
    // cell that originall had values
    //
    expected = MAX_FLUID_VALUE - FLUID_PRESSURECELL_DIFFUSION_CONSTANT * 6 * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(4,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (4,4,4)!  expected: %f    actual: %f  \n");
    }


    //
    // neighbors
    //
    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(3,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (3,4,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(5,4,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (5,4,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(4,3,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (4,3,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(4,5,4)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (4,5,4)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(4,4,3)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (4,4,3)!  expected: %f    actual: %f  \n");
    }

    expected = FLUID_PRESSURECELL_DIFFUSION_CONSTANT * MAX_FLUID_VALUE * env->consts.dt;
    actual = currentChunk->uTempCache[IX(4,4,5)];
    if(fabs(expected - actual) > FLUID_PRESSURE_CELL_ERROR_MARGIN){
        rVal += assertEqualsFloat(expected,actual,"Failed to diffuse velocity correctly (4,4,5)!  expected: %f    actual: %f  \n");
    }

    return rVal;
}

/**
 * Testing adding source values
 */
int fluid_sim_pressurecell_diffuse_tests(int argc, char **argv){
    int rVal = 0;

    // rVal += fluid_sim_pressurecell_diffuse_test1();
    // rVal += fluid_sim_pressurecell_diffuse_test2();

    return rVal;
}