#include <math.h>
#include <stdlib.h>

#include "plane_cloud.h"
#include "random.h"

#define NUM_PLANES 1000

#define BOX_RADIUS 40.0

static float *_matrices;

static inline float _random_posneg_fraction(void)
{
    return 2.0 * random_fraction() - 1.0;
}

static inline float _random_box_position(void)
{
    return _random_posneg_fraction() * BOX_RADIUS;
}

static void _fill_random_matrix(float *matrix)
{
    float axis_x = _random_posneg_fraction();
    float axis_y = _random_posneg_fraction();
    float axis_z = _random_posneg_fraction();
    
    float inv_length = 1.0 / sqrtf(
        (axis_x * axis_x) + (axis_y * axis_y) + (axis_z * axis_z));
    
    axis_x *= inv_length;
    axis_y *= inv_length;
    axis_z *= inv_length;
    
    double angle = 2.0 * M_PI * random_fraction();
    
    matrix[ 0] = axis_x * axis_x * (1.0 - cos(angle)) +          cos(angle);
    matrix[ 1] = axis_x * axis_y * (1.0 - cos(angle)) - axis_z * sin(angle);
    matrix[ 2] = axis_x * axis_z * (1.0 - cos(angle)) + axis_y * sin(angle);
    
    matrix[ 4] = axis_x * axis_y * (1.0 - cos(angle)) + axis_z * sin(angle);
    matrix[ 5] = axis_y * axis_y * (1.0 - cos(angle)) +          cos(angle);
    matrix[ 6] = axis_y * axis_z * (1.0 - cos(angle)) - axis_x * sin(angle);
    
    matrix[ 8] = axis_x * axis_z * (1.0 - cos(angle)) - axis_y * sin(angle);
    matrix[ 9] = axis_y * axis_z * (1.0 - cos(angle)) + axis_x * sin(angle);
    matrix[10] = axis_z * axis_z * (1.0 - cos(angle)) +          cos(angle);

    matrix[12] = _random_box_position();
    matrix[13] = _random_box_position();
    matrix[14] = _random_box_position();
    
    matrix[ 3] = 0.0f;
    matrix[ 7] = 0.0f;
    matrix[11] = 0.0f;
    matrix[15] = 1.0f;
}

void initialize_plane_cloud(void)
{
    _matrices = malloc(16 * NUM_PLANES * sizeof(float));
    float *matrix_pointer = _matrices;
    unsigned i;
    for (i = 0; i < NUM_PLANES; ++i)
    {
        _fill_random_matrix(matrix_pointer);
        matrix_pointer += 16;
    }
}

void update_plane_cloud(double dt)
{
}

unsigned plane_count(void)
{
    return NUM_PLANES;
}

const float *plane_matrices(void)
{
    return _matrices;
}