#include "definitions.hpp"
#include "Particle.hpp"
#include "Map.hpp"

#include <cmath>

Particle::Particle(fpoint_t position, fpoint_t direction):
                   m_position(position), m_direction(direction)
{
}

fpoint_t fit_map(fpoint_t point, numeric_t width, numeric_t height){
    if(point.x < 0){
        point.x += width;
    }
    if(point.y < 0){
        point.y += height;
    }
    if(point.x >= width){
        point.x -= width;
    }
    if(point.y >= height){
        point.y -= height;
    }
    return point;
}

fpoint_t ahead(fpoint_t point, fpoint_t direction, numeric_t distance){
    point.x += direction.x * distance;
    point.y += direction.y * distance;
    return point;
}

fpoint_t rotate(fpoint_t vec, numeric_t angle_deg){
    numeric_t C = cos(angle_deg * M_PI / 180.0);
    numeric_t S = sin(angle_deg * M_PI / 180.0);
    return {C * vec.x - S * vec.y, S * vec.x + C * vec.y};
}


size_t map_coord(fpoint_t point, icoord_t width, icoord_t height){
    return (size_t) floor(point.y) * (size_t) width + (size_t) floor(point.x);
}

size_t Particle::tile_index(Map const &map)
{
    return map_coord(m_position, map.width(), map.height());
}


void Particle::update(Map const &map, 
                      numeric_t delta_t, 
                      numeric_t speed, 
                      numeric_t steering, 
                      numeric_t sampling_angle, 
                      numeric_t sampling_distance)
{
    m_position.x += m_direction.x * speed * delta_t;
    m_position.y += m_direction.y * speed * delta_t;
    
    m_position = fit_map(m_position, map.width(), map.height());

    fpoint_t bit_ahead = fit_map(ahead(m_position, m_direction, sampling_distance), map.width(), map.height());
    fpoint_t bit_right = fit_map(ahead(m_position, rotate(m_direction, -sampling_angle), sampling_distance), map.width(), map.height());
    fpoint_t bit_left  = fit_map(ahead(m_position, rotate(m_direction,  sampling_angle), sampling_distance), map.width(), map.height());
    numeric_t pheromone_ahead = map.tiles()[map_coord(bit_ahead, map.width(), map.height())].pheromone;
    numeric_t pheromone_right = map.tiles()[map_coord(bit_right, map.width(), map.height())].pheromone;
    numeric_t pheromone_left  = map.tiles()[map_coord(bit_left , map.width(), map.height())].pheromone;
    if (pheromone_right > pheromone_ahead && pheromone_right > pheromone_left)
        m_direction = rotate(m_direction, -steering * delta_t);
    else if(pheromone_left > pheromone_ahead && pheromone_left > pheromone_right)
        m_direction = rotate(m_direction,  steering * delta_t);

}