#include <unistd.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <omp.h>
#include <time.h>
#include <math.h>
#ifndef M_PI
#define M_PI 3.14
#endif

#include "SDL2/SDL.h"
#include <SDL2/SDL_image.h>
#include <SDL2/SDL_timer.h>

#include "particles.h"
#include "terrain.h"

#define ARG_POP 1
#define ARG_W 2
#define ARG_H 3
#define ARG_V 4
#define ARG_INCREASE 5
#define ARG_DECAY 6
#define ARG_SAMPLING_D 7
#define ARG_SAMPLING_A 8
#define ARG_STEERING 9

Uint8 clamp_Uint8(float x){
    if(x >= 255.0f)
        return 255;
    if(x <= 0.0f)
        return 0;
    return (Uint8) x;
}

int main(int argc, char *argv[]){
    if(argc < 5){
        fprintf(stderr, "./particles [n_particles] [width] [height] [speed]\n");
        return -1;
    }
    
    size_t n_particules;
    size_t width;
    size_t height;
    coord_t speed;
    float increase;
    float decay;
    coord_t sampling_distance;
    coord_t sampling_angle;
    coord_t steering_angle;
    size_t surface;
    particle_t *particles;
    terrain_t *terrain;

    n_particules = strtol(argv[ARG_POP], NULL, 0);
    width = strtol(argv[ARG_W], NULL, 0);
    height = strtol(argv[ARG_H], NULL, 0);
    speed = strtod(argv[ARG_V], NULL);
    increase = strtof(argv[ARG_INCREASE], NULL);
    decay = strtof(argv[ARG_DECAY], NULL);
    sampling_angle = strtof(argv[ARG_SAMPLING_A], NULL);
    sampling_distance = strtof(argv[ARG_SAMPLING_D], NULL);
    steering_angle = strtof(argv[ARG_STEERING], NULL);
    surface = width*height;

    particles = (particle_t *) malloc(n_particules * sizeof(particle_t));
    terrain = (terrain_t *) malloc(surface * sizeof(terrain_t));
    srand(time(NULL));

    float average_pop = (float)n_particules / (float)surface;
    float average_pheromone = (float)n_particules * (float)(increase) / (float)surface;

    /* Intialization */
    for(size_t i = 0 ; i < height ; ++i){
        for(size_t j = 0 ; j < width ; ++j){
            float d_i = (float)(2.0f * i - height)/(float)height;
            float d_j = (float)(2.0f * j - width)/(float)width;
            float d_center = sqrt((d_i * d_i + d_j * d_j) / 2.0f);
            terrain[i * width + j].pheromone = 0;
            // terrain[i * width + j].decay = decay * (5 - d_center + (float) rand() / (float)(RAND_MAX)) / 7.0f;
            terrain[i * width + j].decay = decay * (5 + 2 * (float) rand() / (float)(RAND_MAX)) / 7.0f;
        }
    }

    for(size_t n = 0 ; n < n_particules ; ++n){
        particles[n].pos.x = width * (float)(rand())/(float)(RAND_MAX);
        particles[n].pos.y = height * (float)(rand())/(float)(RAND_MAX);
        if(particles[n].pos.x == width)
            particles[n].pos.x = 0;
        if(particles[n].pos.y == height)
            particles[n].pos.y = 0;

        coord_t angle = 2 * M_PI * (float)(rand())/(float)(RAND_MAX);
        particles[n].dir.x = cosf(angle);
        particles[n].dir.y = sinf(angle);
    }


    if (SDL_Init(SDL_INIT_VIDEO) != 0) {
        printf("error initializing SDL: %s\n", SDL_GetError());
    }
    SDL_Window* win;
    SDL_Renderer* renderer;
    Uint64 time_last;
    Uint64 time_now;
    coord_t time_delta;
    SDL_CreateWindowAndRenderer(width, height, 0, &win, &renderer);
    SDL_SetRenderDrawColor(renderer, 0, 0, 0, 0);
    SDL_RenderClear(renderer);

    time_now = SDL_GetTicks64();
    int close = 0;
    while(!close){
        SDL_Event event;
    
        // Events management
        while (SDL_PollEvent(&event)) {
            if(event.type == SDL_QUIT) {
                close = 1;
                break;
            }
        }

        time_last = time_now;
        time_now = SDL_GetTicks64();
        time_delta = (time_now - time_last) / 1000.0f;

        // Clear pop count in world
        for(size_t k = 0 ; k < surface ; ++k){
            terrain[k].pop = 0;
            terrain[k].increase = 0;
        }

        // Update particles
        for(size_t n = 0 ; n < n_particules ; ++n){
            particle_update(&particles[n], terrain, time_delta , speed, width, height, sampling_distance, sampling_angle, steering_angle);
        }

        for(size_t n = 0 ; n < n_particules ; ++n){
            terrain[(size_t)floor(particles[n].pos.y) * width + (size_t)floor(particles[n].pos.x)].pop ++;
            terrain[(size_t)floor(particles[n].pos.y) * width + (size_t)floor(particles[n].pos.x)].increase += increase;
        }

        // Update world pheromone level
        terrain_update(terrain, width, height, time_delta);

        SDL_SetRenderDrawColor(renderer, 0, 0, 0, 0);
        SDL_RenderClear(renderer);
        for(size_t i = 0 ; i < height ; ++i){
            for(size_t j = 0 ; j < width ; ++j){
                float value_1 = clamp_Uint8( terrain[i * width + j].pop * (20.0f/average_pop) );
                float value_2 = clamp_Uint8( terrain[i * width + j].pheromone * (255.0f/average_pheromone) );
                // SDL_SetRenderDrawColor(renderer, value_1, value_2, value_2, 1);
                SDL_SetRenderDrawColor(renderer, value_2, value_1, value_2, 1);
                SDL_RenderDrawPoint(renderer, j, i);
            }
        }
        SDL_RenderPresent(renderer);

    }

    // destroy my things
    free(particles);
    free(terrain);

    // destroy SDL things
    SDL_DestroyWindow(win);
    SDL_DestroyRenderer(renderer);
    SDL_Quit();

    return 0;
}