#ifndef PARTICLES
#define PARTICLES

#include "terrain.h"
#include "math.h"
#include <omp.h>

typedef double coord_t; 

typedef struct particule_coord_t {
    coord_t x;
    coord_t y;
} particule_coord_t;

typedef struct particle_t {
    particule_coord_t pos;
    particule_coord_t dir;
} particle_t;

particule_coord_t bit_ahead(particule_coord_t at, particule_coord_t dir, coord_t distance){
    return (particule_coord_t){at.x + distance * dir.x, at.y + distance * dir.y};
}

particule_coord_t rotate(particule_coord_t dir, coord_t angle){
    coord_t C = cos(angle);
    coord_t S = sin(angle);
    return (particule_coord_t){C * dir.x - S * dir.y, S * dir.x + C * dir.y};
}

size_t particle_coord_to_index(particule_coord_t particle_coord, size_t width, size_t height){
    world_coord_t world_coord = {floor(particle_coord.y), floor(particle_coord.x)};
    if(world_coord.i < 0)
        world_coord.i = 0;
    if(world_coord.j < 0)
        world_coord.j = 0;
    if(world_coord.i >= height)
        world_coord.i = height - 1;
    if(world_coord.j >= width)
        world_coord.j = width - 1;
    return world_coord.i * width + world_coord.j;
}

void particle_update(particle_t *p, terrain_t const * const terrain, 
                     coord_t time_delta, coord_t speed, size_t width, size_t heigth, 
                     coord_t sampling_distance, coord_t sampling_angle, coord_t steering_angle){
    p->pos.x += time_delta * speed * p->dir.x;
    p->pos.y += time_delta * speed * p->dir.y;
    // Remain in [0, width[
    if(p->pos.x < 0){
        p->pos.x = - p->pos.x;
        p->dir.x = - p->dir.x;
    }
    if(p->pos.y < 0){
        p->pos.y = - p->pos.y;
        p->dir.y = - p->dir.y;
    }
    if(p->pos.x >= width){
        p->pos.x = 2 * width - p->pos.x;
        p->dir.x = - p->dir.x;
    }
    if(p->pos.y >= heigth){
        p->pos.y = 2 * heigth - p->pos.y;
        p->dir.y = - p->dir.y;
    }

    particule_coord_t dir_left  = rotate(p->dir,  sampling_angle * M_PI / 180.0f);
    particule_coord_t dir_right = rotate(p->dir, -sampling_angle * M_PI / 180.0f);
    float pheromone_ahead = terrain[particle_coord_to_index(bit_ahead(p->pos, p->dir, sampling_distance), width, heigth)].pheromone;
    float pheromone_left = terrain[particle_coord_to_index(bit_ahead(p->pos, dir_left, sampling_distance), width, heigth)].pheromone;
    float pheromone_rigth = terrain[particle_coord_to_index(bit_ahead(p->pos, dir_right, sampling_distance), width, heigth)].pheromone;
    if(pheromone_left > pheromone_ahead && pheromone_left > pheromone_rigth)
        p->dir = rotate(p->dir,  steering_angle * M_PI / 180.0f);
    else if(pheromone_rigth > pheromone_ahead)
        p->dir = rotate(p->dir, -steering_angle * M_PI / 180.0f);
    p->dir = rotate(p->dir, steering_angle * 0.2 * (2*rand()/RAND_MAX-1) * time_delta * M_PI / 180.0f);
}

#endif //PARTICLES