#include "definitions.hpp"
#include "Map.hpp"
#include "Particle.hpp"
#include <sys/types.h>
#include <omp.h>

Map::Map(size_t width, size_t height): m_width(width), m_height(height)
{
    m_surface = (size_t) width * (size_t) height;
    m_tiles.resize(m_surface);
    for(auto &tile : m_tiles){
        tile.pheromone = 0;
        tile.variation = 0;
    }
    m_tiles_swap.resize(m_surface);
    for(auto &tile : m_tiles_swap){
        tile.pheromone = 0;
        tile.variation = 0;
    }
}

void Map::populate(std::vector<Particle> && particles){
    this->m_particles = std::move(particles);
}

inline size_t index(ssize_t i, ssize_t j, size_t height, size_t width){
    if(i < 0)
        i += height;
    if(j < 0)
        j += width;
    if(i >= height)
        i -= height;
    if(j >= width)
        j -= width;
    return i * width + j;
}

void Map::update(numeric_t delta_t)
{
    #pragma omp parallel for
    for(size_t i = 0 ; i < m_particles.size(); ++i){
        m_particles[i].update(*this, delta_t, speed, steering, sampling_angle, sampling_distance);
    }

    for(size_t i = 0 ; i < m_particles.size(); ++i){
        m_tiles[m_particles[i].tile_index(*this)].variation += increase * delta_t;
        m_tiles_swap[m_particles[i].tile_index(*this)].population ++;
    }

    #pragma omp parallel for
    for(size_t i = 0 ; i < m_tiles.size(); ++i){
        m_tiles[i].variation -= decay * delta_t;
        m_tiles[i].update();
    }


    #pragma omp parallel for
    for(ssize_t i = 0 ; i < m_height; ++i){
        for(ssize_t j = 0 ; j < m_width; ++j){
            m_tiles_swap[index(i, j, m_height, m_width)].pheromone = 
                m_tiles[index(i, j, m_height, m_width)].pheromone * (1 - 4 * diffusion * delta_t)
                + m_tiles[index(i - 1, j, m_height, m_width)].pheromone * diffusion * delta_t
                + m_tiles[index(i + 1, j, m_height, m_width)].pheromone * diffusion * delta_t
                + m_tiles[index(i, j - 1, m_height, m_width)].pheromone * diffusion * delta_t
                + m_tiles[index(i, j + 1, m_height, m_width)].pheromone * diffusion * delta_t;
        }
    }

    std::swap(m_tiles, m_tiles_swap);
}