#define _USE_MATH_DEFINES
/// code to make agents
#include <vector>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <random>

#include <boost/foreach.hpp>

#include <Rcpp.h>
#include <RcppParallel.h>

#include "network.h"
#include "landscape.h"
#include "agents.h"

// to shuffle pop id
void Population::shufflePop() {
    if (order[0] == order[nAgents - 1])
    {
        for (size_t i = 0; i < static_cast<size_t>(nAgents); i++)
        {
            order[i] = i;
        }
        std::random_shuffle ( order.begin(), order.end() );
    }
    else {
        std::random_shuffle ( order.begin(), order.end() );
    }

}

// to update agent Rtree
void Population::updateRtree () {
    // initialise rtree
    bgi::rtree< value, bgi::quadratic<16> > tmpRtree;
    for (int i = 0; i < nAgents; ++i)
    {
        point p = point(coordX[i], coordY[i]);
        tmpRtree.insert(std::make_pair(p, i));
    }
    std::swap(agentRtree, tmpRtree);
    tmpRtree.clear();
}

// uniform distribution for agent position
std::uniform_real_distribution<float> agent_ran_pos(0.0f, 1.f);

// function for initial positions
void Population::initPos(Resources food) {
    for (size_t i = 0; i < static_cast<size_t>(nAgents); i++) {
        coordX[i] = agent_ran_pos(rng) * food.dSize;
        initX[i] = coordX[i];
        coordY[i] = agent_ran_pos(rng) * food.dSize;
        initY[i] = coordY[i];
    }
    updateRtree();
}

// set agent trait
void Population::setTrait(const float mSize) {

    // create a cauchy distribution, mSize is the scale
    std::cauchy_distribution<float> agent_ran_trait(0.f, mSize);

    for(int i = 0; i < nAgents; i++) {
        sF[i] = agent_ran_trait(rng);
        sH[i] = agent_ran_trait(rng);
        sN[i] = agent_ran_trait(rng);
    }
}

float get_distance(float x1, float x2, float y1, float y2) {
    return std::sqrt(std::pow((x1 - x2), 2) + std::pow((y1 - y2), 2));
}

// general function for agents within distance
std::pair<int, int> Population::countAgents (
    const float xloc, const float yloc) {

    int handlers = 0;
    int nonhandlers = 0;
    std::vector<value> near_agents;
    // query for a simple box
    agentRtree.query(bgi::satisfies([&](value const& v) {
        return bg::distance(v.first, point(xloc, yloc)) < range_agents;}),
        std::back_inserter(near_agents));

    BOOST_FOREACH(value const& v, near_agents) {

        if(counter[v.second] > 0) handlers ++; else nonhandlers ++;
    }
    near_agents.clear();
    // first element is number of near entities
    // second is the identity of entities
    return std::pair<int, int> {handlers, nonhandlers};
}

// function for near agent ids
std::vector<int> Population::getNeighbourId (
    const float xloc, const float yloc) {

    std::vector<int> agent_id;
    std::vector<value> near_agents;
    // query for a simple box
    // neighbours for associations are counted over the MOVEMENT RANGE
    agentRtree.query(bgi::satisfies([&](value const& v) {
        return bg::distance(v.first, point(xloc, yloc)) < range_move;}),
        std::back_inserter(near_agents));

    BOOST_FOREACH(value const& v, near_agents) {
        agent_id.push_back(v.second);
    }
    near_agents.clear();
    // first element is number of near entities
    // second is the identity of entities
    return agent_id;
}

// general function for items within distance
int Population::countFood (
    const Resources &food,
    const float xloc, const float yloc) {

    int nFood = 0;
    std::vector<value> near_food;

    // check any available
    if (food.nAvailable > 0) {
        // query for a simple box
        food.rtree.query(bgi::satisfies([&](value const& v) {
            return bg::distance(v.first, point(xloc, yloc)) < range_food;}),
            std::back_inserter(near_food));

        BOOST_FOREACH(value const& v, near_food) {
            // count only which are available!
            if (food.available[v.second]) {
                nFood++;
            }
        }
        near_food.clear();
    }

    return nFood;
}

// function for the nearest available food item
std::vector<int> Population::getFoodId (
    const Resources &food,
    const float xloc, const float yloc) {

    std::vector<int> food_id;
    std::vector<value> near_food;
    // check any available
    if (food.nAvailable > 0) {
        // query for a simple box
        // food is accessed over the MOVEMENT RANGE
        food.rtree.query(bgi::satisfies([&](value const& v) {
            return bg::distance(v.first, point(xloc, yloc)) < range_move;}),
            std::back_inserter(near_food));

        BOOST_FOREACH(value const& v, near_food) {
            // count only which are available!
            if (food.available[v.second]) {
                food_id.push_back(v.second);
            }
        }
        near_food.clear();
    }

    // first element is number of near entities
    // second is the identity of entities
    return food_id;
}

/// rng for suitability
std::normal_distribution<float> noise(0.f, 0.01f);
std::cauchy_distribution<float> noise_cauchy(0.f, 0.001f);

/// population movement function
void Population::move(const Resources &food, const int nThreads) {

    float twopi = 2.f * M_PI;

    // what increment for 3 samples in a circle around the agent
    float increment = twopi / n_samples;
    float angle = 0.f;
    // for this increment what angles to sample at
    std::vector<float> sample_angles (static_cast<int>(n_samples), 0.f);
    for (int i_ = 0; i_ < static_cast<int>(n_samples); i_++)
    {
        sample_angles[i_] = angle;
        angle += increment;
    }

    // make random noise for each individual and each sample
    std::vector<std::vector<float> > noise_v (nAgents, std::vector<float>(static_cast<int>(n_samples), 0.f));
    for (size_t i_ = 0; i_ < noise_v.size(); i_++)
    {
        for (size_t j_ = 0; j_ < static_cast<size_t>(n_samples); j_++)
        {
            noise_v[i_][j_] = noise(rng);
        }
    }

    shufflePop();
    // loop over agents --- randomise
    if (nThreads > 1) {
        // any number above 1 will allow automatic n threads
        tbb::task_scheduler_init _tbb(tbb::task_scheduler_init::automatic); // automatic for now
        // try parallel
        tbb::parallel_for(
            tbb::blocked_range<unsigned>(1, order.size()),
            [&](const tbb::blocked_range<unsigned>& r) {
                for (unsigned i = r.begin(); i < r.end(); ++i) {
                    int id = order[i];
                    if (counter[id] > 0) {
                        counter[id] --;
                    }
                    else {
                        // first assess current location
                        float sampleX = coordX[id];
                        float sampleY = coordY[id];

                        float foodHere = 0.f;
                        // count local food only if items are available
                        if(food.nAvailable > 0) {
                            foodHere = static_cast<float>(countFood(
                                food, sampleX, sampleY
                            ));
                        }
                        // count local handlers and non-handlers
                        std::pair<int, int> agentCounts = countAgents(sampleX, sampleY);

                        // get suitability current
                        float suit_origin = (
                            (sF[id] * foodHere) + (sH[id] * agentCounts.first) +
                            (sN[id] * agentCounts.second)
                        );

                        float newX = sampleX;
                        float newY = sampleY;
                        // now sample at three locations around
                        for(size_t j = 0; j < sample_angles.size(); j++) {
                            float t1_ = static_cast<float>(cos(sample_angles[j]));
                            float t2_ = static_cast<float>(sin(sample_angles[j]));

                            // use range for agents to determine sample locs
                            sampleX = coordX[id] + (range_agents * t1_);
                            sampleY = coordY[id] + (range_agents * t2_);

                            // crudely wrap sampling location
                            if((sampleX > food.dSize) | (sampleX < 0.f)) {
                                sampleX = std::fabs(std::fmod(sampleX, food.dSize));
                            }
                            if((sampleY > food.dSize) | (sampleY < 0.f)) {
                                sampleY = std::fabs(std::fmod(sampleY, food.dSize));
                            }

                            // count food at sample locations if any available
                            if(food.nAvailable > 0) {
                                foodHere = static_cast<float>(countFood(
                                    food, sampleX, sampleY
                                ));
                            }

                            // count local handlers and non-handlers
                            std::pair<int, int> agentCounts = countAgents(sampleX, sampleY);

                            float suit_dest = (
                                (sF[id] * foodHere) + (sH[id] * agentCounts.first) +
                                (sN[id] * agentCounts.second) +
                                noise_v[id][j] // add same very very small noise to all
                            );

                            if (suit_dest > suit_origin) {
                                // where does the individual really go
                                newX = coordX[id] + (range_move * t1_);
                                newY = coordY[id] + (range_move * t2_);

                                // crudely wrap MOVEMENT location
                                if((newX > food.dSize) | (newX < 0.f)) {
                                    newX = std::fabs(std::fmod(newX, food.dSize));
                                }
                                if((newY > food.dSize) | (newY < 0.f)) {
                                    newY = std::fabs(std::fmod(newY, food.dSize));
                                }

                                assert(newX < food.dSize && newX > 0.f);
                                assert(newY < food.dSize && newY > 0.f);
                                suit_origin = suit_dest;
                            }
                        }
                        // distance to be moved
                        moved[id] += range_move;

                        // set locations
                        coordX[id] = newX; coordY[id] = newY;
                    }
                }
            }
        );
    } else if (nThreads == 1) {
        for (int i = 0; i < nAgents; ++i) {
            int id = order[i];
            if (counter[id] > 0) {
                counter[id] --;
            }
            else {
                // first assess current location
                float sampleX = coordX[id];
                float sampleY = coordY[id];

                float foodHere = 0.f;
                // count local food only if items are available
                if(food.nAvailable > 0) {
                    foodHere = static_cast<float>(countFood(
                        food, sampleX, sampleY
                    ));
                }
                // count local handlers and non-handlers
                std::pair<int, int> agentCounts = countAgents(sampleX, sampleY);

                // get suitability current
                float suit_origin = (
                    (sF[id] * foodHere) + (sH[id] * agentCounts.first) +
                    (sN[id] * agentCounts.second)
                );

                float newX = sampleX;
                float newY = sampleY;
                // now sample at three locations around
                for(size_t j = 0; j < sample_angles.size(); j++) {
                    float t1_ = static_cast<float>(cos(sample_angles[j]));
                    float t2_ = static_cast<float>(sin(sample_angles[j]));

                    // use range for agents to determine sample locs
                    sampleX = coordX[id] + (range_agents * t1_);
                    sampleY = coordY[id] + (range_agents * t2_);

                    // crudely wrap sampling location
                    if((sampleX > food.dSize) | (sampleX < 0.f)) {
                        sampleX = std::fabs(std::fmod(sampleX, food.dSize));
                    }
                    if((sampleY > food.dSize) | (sampleY < 0.f)) {
                        sampleY = std::fabs(std::fmod(sampleY, food.dSize));
                    }

                    // count food at sample locations if any available
                    if(food.nAvailable > 0) {
                        foodHere = static_cast<float>(countFood(
                            food, sampleX, sampleY
                        ));
                    }

                    // count local handlers and non-handlers
                    std::pair<int, int> agentCounts = countAgents(sampleX, sampleY);

                    float suit_dest = (
                        (sF[id] * foodHere) + (sH[id] * agentCounts.first) +
                        (sN[id] * agentCounts.second) +
                        noise_v[id][j] // add same very very small noise to all
                    );

                    if (suit_dest > suit_origin) {
                        // where does the individual really go
                        newX = coordX[id] + (range_move * t1_);
                        newY = coordY[id] + (range_move * t2_);

                        // crudely wrap MOVEMENT location
                        if((newX > food.dSize) | (newX < 0.f)) {
                            newX = std::fabs(std::fmod(newX, food.dSize));
                        }
                        if((newY > food.dSize) | (newY < 0.f)) {
                            newY = std::fabs(std::fmod(newY, food.dSize));
                        }

                        assert(newX < food.dSize && newX > 0.f);
                        assert(newY < food.dSize && newY > 0.f);
                        suit_origin = suit_dest;
                    }
                }
                // distance to be moved
                moved[id] += range_move;

                // set locations
                coordX[id] = newX; coordY[id] = newY;
            }
        }

    }

}

// function to paralellise choice of forage item
void Population::pickForageItem(const Resources &food, const int nThreads){
    shufflePop();
    // nearest food
    std::vector<int> idTargetFood (nAgents, -1);

    if (nThreads > 1)
    {
        // loop over agents --- no shuffling required here
        tbb::task_scheduler_init _tbb(tbb::task_scheduler_init::automatic); // automatic for now
        // try parallel foraging --- agents pick a target item
        tbb::parallel_for(
            tbb::blocked_range<unsigned>(1, order.size()),
                [&](const tbb::blocked_range<unsigned>& r) {
                for (unsigned i = r.begin(); i < r.end(); ++i) {
                    if ((counter[i] > 0) | (food.nAvailable == 0)) {
                        // nothing -- agent cannot forage or there is no food
                    }
                    else {
                        // find nearest item ids
                        std::vector<int> theseItems = getFoodId(food, coordX[i], coordY[i]);
                        int thisItem = -1;

                        // check near items count
                        if(theseItems.size() > 0) {
                            // take first item by default
                            thisItem = theseItems[0];
                            idTargetFood[i] = thisItem;
                        }
                    }
                }
            }
        );
    } else if (nThreads == 1)
    {
        for (int i = 0; i < nAgents; ++i) {
            if ((counter[i] > 0) | (food.nAvailable == 0)) {
                // nothing -- agent cannot forage or there is no food
            }
            else {
                // find nearest item ids
                std::vector<int> theseItems = getFoodId(food, coordX[i], coordY[i]);
                int thisItem = -1;

                // check near items count
                if(theseItems.size() > 0) {
                    // take first item by default
                    thisItem = theseItems[0];
                    idTargetFood[i] = thisItem;
                }
            }
        }
    }

    forageItem = idTargetFood;
}

// function to exploitatively forage on picked forage items
void Population::doForage(Resources &food) {
    // all agents have picked a food item if they can forage
    // now forage in a serial loop --- this cannot be parallelised
    // this order is randomised
    for (size_t i = 0; i < static_cast<size_t>(nAgents); i++)
    {
        int id = order[i];
        if ((counter[id] > 0) | (food.nAvailable == 0)) {
            // nothing
        } else {
            int thisItem = forageItem[id]; //the item picked by this agent
            // check selected item is available
            if (thisItem != -1)
            {
                counter[id] = handling_time;
                intake[id] += 1.0; // increased here --- not as described.

                // reset food availability
                food.available[thisItem] = false;
                food.counter[thisItem] = food.regen_time;
                food.nAvailable --;
            }
        }
    }
}

void Population::countAssoc(const int nThreads) {
    for (int i = 0; i < nAgents; ++i) {
        // count nearby agents and update raw associations
        std::vector<int> nearby_agents = getNeighbourId(coordX[i], coordY[i]);
        associations[i] += nearby_agents.size();

        // loop over nearby agents and update association matrix
        for (size_t j = 0; j < nearby_agents.size(); j++)
        {
            int target_agent = nearby_agents[j];
            pbsn.adjMat (i, target_agent) += 1;
        }
    }
}

/// minor function to normalise vector
std::vector<float> Population::handleFitness() {
    // sort vec fitness
    std::vector<float> vecFitness = energy;
    std::sort(vecFitness.begin(), vecFitness.end()); // sort to to get min-max
    // scale to max fitness
    float maxFitness = vecFitness[vecFitness.size()-1];
    float minFitness = vecFitness[0];

    // reset to energy
    vecFitness = energy;
    // rescale copied energy vector by min anx max fitness
    for(size_t i = 0; i < static_cast<size_t>(nAgents); i++) {
        vecFitness[i] = ((vecFitness[i]  - minFitness) / (maxFitness - minFitness)) +
         noise(rng);
    }

    return vecFitness;
}

// fun for replication
void Population::Reproduce(const Resources food, const bool infect_percent,
    const float dispersal, const float mProb, const float mSize)
{
    // std::bernoulli_distribution verticalInfect(0.01f);

    // mutation probability and size distribution --- inefficient but oh well
    std::bernoulli_distribution mutation_happens(mProb);
    std::cauchy_distribution<float> mutation_size(0.0, mSize);

    // choose the range over which individuals are dispersed
    std::normal_distribution<float> sprout(0.f, dispersal);
    std::vector<float> vecFitness;
    //normalise intake if percent infect is not true
    if (infect_percent) {
         vecFitness = energy;
    } else {
        vecFitness = handleFitness();
    }

    // set up weighted lottery
    std::discrete_distribution<> weightedLottery(vecFitness.begin(), vecFitness.end());

    // get parent trait based on weighted lottery
    std::vector<float> tmp_sF (nAgents, 0.f);
    std::vector<float> tmp_sH (nAgents, 0.f);
    std::vector<float> tmp_sN (nAgents, 0.f);

    // infected or not for vertical transmission
    std::vector<bool> infected_2 (nAgents, false);

    // reset infection source
    srcInfect = std::vector<int> (nAgents, 0);

    // reset associations
    associations = std::vector<int> (nAgents, 0);

    // reset distance moved
    moved = std::vector<float> (nAgents, 0.f);

    // reset adjacency matrix
    pbsn.adjMat = Rcpp::NumericMatrix(nAgents, nAgents);

    // positions
    std::vector<float> coord_x_2 (nAgents, 0.f);
    std::vector<float> coord_y_2 (nAgents, 0.f);

    for (int a = 0; a < nAgents; a++) {
        size_t parent_id = static_cast<size_t>(weightedLottery(rng));

        tmp_sF[a] = sF[parent_id];
        tmp_sH[a] = sH[parent_id];
        tmp_sN[a] = sN[parent_id];

        // inherit positions from parent
        coord_x_2[a] = coordX[parent_id] + sprout(rng);
        coord_y_2[a] = coordY[parent_id] + sprout(rng);

        // robustly wrap positions
        if(coord_x_2[a] < 0.f) coord_x_2[a] = food.dSize + coord_x_2[a];
        if(coord_x_2[a] > food.dSize) coord_x_2[a] = coord_x_2[a] - food.dSize;

        if(coord_y_2[a] < 0.f) coord_y_2[a] = food.dSize + coord_y_2[a];
        if(coord_y_2[a] > food.dSize) coord_y_2[a] = coord_y_2[a] - food.dSize;

        // // vertical transmission of infection.
        // if(infected[parent_id]) {
        //     if(verticalInfect(rng)) {
        //         infected_2[a] = true;
        //         srcInfect[a] = 1;
        //     }
        // }
    }

    // swap infected and infected_2
    std::swap(infected, infected_2);
    infected_2.clear();

    // swap coords --- this initialises individuals near their parent's position
    std::swap(coordX, coord_x_2);
    std::swap(coordY, coord_y_2);
    coord_x_2.clear(); coord_y_2.clear();

    // update initial positions!
    initX = coordX;
    initY = coordY;

    // reset counter and time infected
    counter = std::vector<int> (nAgents, 0);
    timeInfected = std::vector<int> (nAgents, 0);
    assert(static_cast<int>(counter.size()) == nAgents && "counter size wrong");

    // mutate trait: trait shifts up or down with an equal prob
    // trait mutation prob is mProb, in a two step process
    for (int a = 0; a < nAgents; a++) {
        if(mutation_happens(rng)) {
            tmp_sF[a] = tmp_sF[a] + mutation_size(rng);
        }
        if(mutation_happens(rng)) {
            tmp_sH[a] = tmp_sH[a] + mutation_size(rng);
        }
        if(mutation_happens(rng)) {
            tmp_sN[a] = tmp_sN[a] + mutation_size(rng);
        }
    }

    // reset nInfected and count natal infections
    // from vertical transmission
    countInfected();

    // swap trait matrices
    std::swap(sF, tmp_sF);
    std::swap(sH, tmp_sH);
    std::swap(sN, tmp_sN);

    tmp_sF.clear(); tmp_sH.clear(); tmp_sN.clear();

    // swap energy
    std::vector<float> tmpEnergy (nAgents, 0.001);
    std::swap(energy, tmpEnergy);
    tmpEnergy.clear();

    // swap intake
    std::vector<float> tmpIntake (nAgents, 0.001);
    std::swap(intake, tmpIntake);
    tmpIntake.clear();
}


#ifndef PARMCB_SVA_SIGNED_TBB_HPP_
#define PARMCB_SVA_SIGNED_TBB_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>
#include <boost/timer/timer.hpp>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <tbb/concurrent_vector.h>
#include <tbb/task_group.h>

#include <parmcb/detail/signed_dijkstra.hpp>
#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph, class WeightMap>
        struct OddCycleFinder {
            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef typename boost::property_traits<WeightMap>::value_type WeightType;

            OddCycleFinder(const Graph &g, const WeightMap &weight_map, const ForestIndex<Graph> &forest_index,
                    const std::vector<Vertex> &vertices) :
                    g(g), weight_map(weight_map), forest_index(forest_index), vertices(vertices), compare(
                            std::less<WeightType>()) {
            }

            std::tuple<std::set<Edge>, WeightType, bool> find(const SpVecGF2<std::size_t> &support) {
                std::set<Edge> signed_edges;
                convert_edges(support, std::inserter(signed_edges, signed_edges.end()), forest_index);
                if (signed_edges.size() == 1) {
                    return find_single_edge(*signed_edges.begin());
                } else if (signed_edges.size() >= boost::num_vertices(g)) {
                    return find_all_vertices(signed_edges);
                } else {
                    return find_less_than_vertices(signed_edges);
                }
            }

            std::tuple<std::set<Edge>, WeightType, bool> find_single_edge(const Edge &se) {
                std::tuple<std::set<Edge>, WeightType, bool> best;

                auto se_v = boost::source(se, g);
                auto se_u = boost::target(se, g);
                auto res = bidirectional_signed_dijkstra(g, weight_map, std::set<Edge> { }, std::set<Edge> { se }, true,
                        se_v, true, se_u, true, std::get<2>(best), std::get<1>(best));
                if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                    std::get<1>(res) += boost::get(weight_map, se);
                    if (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best))) {
                        std::get<0>(res).insert(se);
                        best = res;
                    }
                }
                return best;
            }

            std::tuple<std::set<Edge>, WeightType, bool> find_all_vertices(const std::set<Edge> &signed_edges) {
                typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
                auto cycle_min = [&](const cycle_t &c1, const cycle_t &c2) {
                    if (!std::get<2>(c1) || !std::get<2>(c2)) {
                        if (std::get<2>(c1)) {
                            return c1;
                        } else {
                            return c2;
                        }
                    }
                    // both valid, compare
                    if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                        return c1;
                    }
                    return c2;
                };

                return tbb::parallel_reduce(tbb::blocked_range<std::size_t>(0, boost::num_vertices(g)),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto v = vertices[i];
                                const bool use_hidden_edges = false;
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges,
                                        std::set<Edge> { }, use_hidden_edges, v, true, v, false,
                                        std::get<2>(running_min), std::get<1>(running_min));
                                if (std::get<2>(res)
                                        && (!std::get<2>(running_min)
                                                || compare(std::get<1>(res), std::get<1>(running_min)))) {
                                    running_min = res;
                                }
                            }
                            return running_min;
                        },
                        cycle_min);

            }

            std::tuple<std::set<Edge>, WeightType, bool> find_less_than_vertices(const std::set<Edge> &signed_edges) {
                /*
                 * Heuristic in case number of signed edges is small compared to the number of vertices.
                 */
                typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
                auto cycle_min = [&](const cycle_t &c1, const cycle_t &c2) {
                    if (!std::get<2>(c1) || !std::get<2>(c2)) {
                        if (std::get<2>(c1)) {
                            return c1;
                        } else {
                            return c2;
                        }
                    }
                    // both valid, compare
                    if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                        return c1;
                    }
                    return c2;
                };

                std::map<Edge, std::set<Edge>> hidden_edges_per_edge;
                std::vector<Edge> signed_edges_as_vector;
                std::set<Edge> tmp_signed_edges = signed_edges;
                while (!tmp_signed_edges.empty()) {
                    auto bit = tmp_signed_edges.begin();
                    hidden_edges_per_edge.insert(std::make_pair(*bit, tmp_signed_edges));
                    signed_edges_as_vector.push_back(*bit);
                    tmp_signed_edges.erase(bit);
                }
                return tbb::parallel_reduce(tbb::blocked_range<std::size_t>(0, signed_edges_as_vector.size()),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto se = signed_edges_as_vector.at(i);
                                auto se_v = boost::source(se, g);
                                auto se_u = boost::target(se, g);
                                auto hidden_edges = hidden_edges_per_edge.at(se);
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, hidden_edges,
                                        true, se_v, true, se_u, true, std::get<2>(running_min),
                                        std::get<1>(running_min));
                                if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                                    std::get<1>(res) += boost::get(weight_map, se);
                                    if (!std::get<2>(running_min)
                                            || compare(std::get<1>(res), std::get<1>(running_min))) {
                                        std::get<0>(res).insert(se);
                                        running_min = res;
                                    }
                                }
                            }
                            return running_min;
                        },
                        cycle_min);
            }

            const Graph &g;
            const WeightMap &weight_map;
            const ForestIndex<Graph> &forest_index;
            const std::vector<Vertex> &vertices;
            const std::less<WeightType> compare;
        };

    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_signed_tbb(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, const std::size_t hardware_concurrency_hint = 0) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        std::cout << "Cycle dimension " << csd << std::endl;
        std::vector<Vertex> vertices;
        {
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
                vertices.push_back(*vi);
            }
        }

        /*
         * Initialize support vectors
         */
        tbb::concurrent_vector<SpVecGF2<std::size_t>> support;
        tbb::parallel_for(tbb::blocked_range<std::size_t>(0, csd), [&](const tbb::blocked_range<std::size_t> &r) {
            for (std::size_t i = r.begin(); i != r.end(); ++i) {
                support.push_back(SpVecGF2<std::size_t> { i });
            }
        });

        boost::timer::cpu_timer cycle_timer;
        cycle_timer.stop();
        boost::timer::cpu_timer support_timer;
        support_timer.stop();

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        parmcb::detail::OddCycleFinder<Graph, WeightMap> odd_cycle_finder(g, weight_map, forest_index, vertices);
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << "iteration = " << k << std::endl;
            }

            /*
             * Choose the sparsest support heuristic
             */
            auto min_support = k;
            for (auto r = k + 1; r < csd; ++r) {
                if (support[r].size() < support[min_support].size())
                    min_support = r;
            }
            if (min_support != k) {  // swap
                std::swap(support[k], support[min_support]);
            }

            /*
             * Compute shortest odd cycle
             */
            cycle_timer.resume();
            auto cycle = odd_cycle_finder.find(support[k]);
            cycle_timer.stop();

            /*
             * Update support vectors
             */
            support_timer.resume();
            std::set<std::size_t> cyclek;
            convert_edges(std::get<0>(cycle), std::inserter(cyclek, cyclek.end()), forest_index);
            tbb::parallel_for(tbb::blocked_range<std::size_t>(k + 1, csd),
                    [&](const tbb::blocked_range<std::size_t> &r) {
                        auto e = r.end();
                        for (std::size_t i = r.begin(); i != e; ++i) {
                            if (support[i] * cyclek == 1) {
                                support[i] += support[k];
                            }
                        }
                    });
            support_timer.stop();

            /*
             * Report cycle
             */
            std::list<Edge> cyclek_edgelist;
            std::copy(std::get<0>(cycle).begin(), std::get<0>(cycle).end(), std::back_inserter(cyclek_edgelist));
            *out++ = cyclek_edgelist;
            mcb_weight += std::get<1>(cycle);
        }

        std::cout << "cycle   timer" << cycle_timer.format();
        std::cout << "support timer" << support_timer.format();

        return mcb_weight;
    }

} // namespace parmcb

#endif


//MIT License
//
//Copyright(c) 2020 Zheng Jiaqi @NUSComputing
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files(the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions :
//
//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.

#include <CGAL/Surface_mesh_parameterization/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Square_border_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Two_vertices_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/internal/Containers_filler.h>
#include <CGAL/Surface_mesh_parameterization/ARAP_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/LSCM_parameterizer_3.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>
#include <CGAL/Surface_mesh_parameterization/IO/File_off.h>
#include <CGAL/Polygon_mesh_processing/detect_features.h>
#include <CGAL/Polygon_mesh_processing/smooth_mesh.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/Polygon_mesh_processing/locate.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <boost/property_map/property_map.hpp>
#include <boost/function_output_iterator.hpp>
#include <CGAL/boost/graph/Seam_mesh.h>
#include <CGAL/boost/graph/iterator.h>
#include <CGAL/disable_warnings.h>
#include <CGAL/Simple_cartesian.h>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <CGAL/Unique_hash_map.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/circulator.h>
#include <CGAL/AABB_tree.h>
#include <iostream>
#include <fstream>
#include <cstddef>
#include <vector>

#include "gDel2D/gDel2D/GpuDelaunay.h"
#include "gDel2D/gDel2D/PerfTimer.h"
#include "gDel2D/gDel2D/CPU/PredWrapper.h"
#include "gDel2D/DelaunayChecker.h"
#include "delaunay.h"

#if defined(_WIN32)
#include <Windows.h>
#endif

//#define OUTPUT_PARAMETERIZATION
//#define VISUALIZE_TRIANGULATION
#ifdef VISUALIZE_TRIANGULATION
#include "gDel2D/Visualizer.h"
#endif

typedef CGAL::Simple_cartesian<double>	Kernel;

#include "split.h"
#include "weighting.h"
#include "triangulation.h"
#include "discretization.h"
#include "constrains.h"
#include "gcvt.h"
#include "recover.h"

typedef Kernel::Point_2					Point_2;
typedef Kernel::Point_3					Point_3;
typedef CGAL::Surface_mesh<Point_3>		SurfMesh;
typedef CGAL::Surface_mesh<Point_2>		Mesh2D;

typedef boost::graph_traits<SurfMesh>::edge_descriptor			SM_edge_descriptor;
typedef boost::graph_traits<SurfMesh>::halfedge_descriptor		SM_halfedge_descriptor;
typedef boost::graph_traits<SurfMesh>::vertex_descriptor		SM_vertex_descriptor;

typedef boost::graph_traits<Mesh2D>::edge_descriptor			edge_2d;
typedef boost::graph_traits<Mesh2D>::halfedge_descriptor		halfedge_2d;
typedef boost::graph_traits<Mesh2D>::vertex_descriptor			vertex_2d;

typedef CGAL::Unique_hash_map<SM_halfedge_descriptor, Point_2>	UV_uhm;
typedef CGAL::Unique_hash_map<SM_edge_descriptor, bool>			Seam_edge_uhm;
typedef CGAL::Unique_hash_map<SM_vertex_descriptor, bool>		Seam_vertex_uhm;

typedef boost::associative_property_map<UV_uhm>					UV_pmap;
typedef boost::associative_property_map<Seam_edge_uhm>			Seam_edge_pmap;
typedef boost::associative_property_map<Seam_vertex_uhm>		Seam_vertex_pmap;

typedef CGAL::Seam_mesh<SurfMesh, Seam_edge_pmap, Seam_vertex_pmap>	SeamMesh;

typedef boost::graph_traits<SeamMesh>::vertex_descriptor		vertex_descriptor;
typedef boost::graph_traits<SeamMesh>::edge_descriptor			edge_descriptor;
typedef boost::graph_traits<SeamMesh>::halfedge_descriptor		halfedge_descriptor;
typedef boost::graph_traits<SeamMesh>::face_descriptor			face_descriptor;

typedef boost::unordered_map<vertex_descriptor, vertex_2d>		_3d_to_2d_uhm;
typedef boost::unordered_map<vertex_2d, vertex_descriptor>		_2d_to_3d_uhm;
typedef boost::unordered_map<vertex_2d, double>					weight_uhm;

typedef boost::associative_property_map<_3d_to_2d_uhm>			_3d_to_2d_pmap;
typedef boost::associative_property_map<_2d_to_3d_uhm>			_2d_to_3d_pmap;
typedef boost::associative_property_map<weight_uhm>				weight_pmap;

namespace PMP = CGAL::Polygon_mesh_processing;
namespace SMP = CGAL::Surface_mesh_parameterization;

typedef PMP::Face_location<SurfMesh, Kernel::FT>				Face_Location;

int imageSize	= 2048;
int depth		= 1;
int maxIter		= 1000;
int vertices    = 20000;
double scale, leftbound, lowerbound;
float *densityMap;
short *Voronoi;
bool *constrainMask;

GDel2DInput		DelInput;
GDel2DOutput	DelOutput;

int main(int argc, char** argv)
{
	// input surface mesh
	const char* filename = argc > 1 ? argv[1] : "data/horse.off";
	std::ifstream input(filename);
	SurfMesh surface;
	if (!input || !(input >> surface) || surface.is_empty()) {
		std::cerr << "Not a valid .off file." << std::endl;
		return EXIT_FAILURE;
	}
	input.close();

	double edge_length_limit = 0.72;

	// split long edges
	std::vector<SM_edge_descriptor> constrain_vec;
	detectConstrains(surface, constrain_vec);
	PMP::split_long_edges(constrain_vec, edge_length_limit, surface);

	// create seam mesh
	const char* seamfile = "data/horse.selection.txt";
	Seam_edge_uhm seam_edge_uhm(false);
	Seam_edge_pmap seam_edges(seam_edge_uhm);
	Seam_vertex_uhm seam_vertex_uhm(false);
	Seam_vertex_pmap seam_vertices(seam_vertex_uhm);
	std::vector<SM_edge_descriptor> seam_vec;
	addSeams(surface, seam_edges, seam_vec, seamfile);
	PMP::split_long_edges(seam_vec, edge_length_limit, surface, PMP::parameters::edge_is_constrained_map(seam_edges));
	getSeamVertices(surface, seam_edges, seam_vertices);

	SeamMesh seam_mesh(surface, seam_edges, seam_vertices);

	// the property map stores the 2D points in the uv parameter space
	UV_uhm uv_uhm;
	UV_pmap uv_coord(uv_uhm);

	// a halfedge on the border
	halfedge_descriptor border_halfedge = PMP::longest_border(seam_mesh).first;

	// planar parameterization
	//typedef SMP::Square_border_uniform_parameterizer_3<SeamMesh> Border_parameterizer;
	typedef SMP::ARAP_parameterizer_3<SeamMesh> Parameterizer;
	SMP::parameterize(seam_mesh, Parameterizer(), border_halfedge, uv_coord);
	printf("Parameterization done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);
#ifdef OUTPUT_PARAMETERIZATION
	std::ofstream out("parameterization.off");
	SMP::IO::output_uvmap_to_off(seam_mesh, border_halfedge, uv_coord, out);
#endif

	// create 2d triangulation
	Mesh2D mesh_2d;
	_3d_to_2d_uhm _32_uhm;
	_3d_to_2d_pmap vertex_3d_to_2d(_32_uhm);
	_2d_to_3d_uhm _23_uhm;
	_2d_to_3d_pmap vertex_2d_to_3d(_23_uhm);
	build_2d_triangulation(seam_mesh, border_halfedge, uv_coord, mesh_2d, vertex_3d_to_2d, vertex_2d_to_3d);
	printf("Triangulation done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// weighting vertices
	weight_uhm wgt_uhm;
	weight_pmap vertex_weight(wgt_uhm);
	weighting_vertices_with_area(seam_mesh, mesh_2d, vertex_3d_to_2d, vertex_weight);
	printf("Weighting done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// discretization
	densityMap = (float *)malloc(imageSize * imageSize * sizeof(float));
	discretization(mesh_2d, vertex_weight, densityMap, imageSize, scale, leftbound, lowerbound);
	printf("Discretization done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// detect constrains
	std::vector<std::pair<vertex_2d, vertex_2d> > constrain_edge;
	std::unordered_set<vertex_2d> constrain_point;
	detect_constrains(seam_mesh, vertex_3d_to_2d, constrain_edge, constrain_point);
	printf("Detect constrains done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// construct centroidal Voronoi diagram
	Voronoi = (short *)malloc(sizeof(short) * imageSize * imageSize * 2);
	constrainMask = (bool *)malloc(sizeof(bool) * imageSize * imageSize);
	generateMask(mesh_2d, constrain_point, constrainMask, imageSize, scale, leftbound, lowerbound);
	centroidalVoronoi(Voronoi, densityMap, constrainMask, vertices, imageSize, depth, maxIter);
	printf("Centroidal Voronoi tessellation done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// construct constraint Delaunay triangulation
#ifdef VISUALIZE_TRIANGULATION
	Visualizer *vis = Visualizer::instance();
	if (vis->isEnable()) {
		vis->init(argc, argv, "gDel2D");
		vis->printHelp();
	}
	Visualizer::instance()->pause();
#endif
	GpuDel gpuDel;
	Point2HVec().swap(DelInput.pointVec);
	SegmentHVec().swap(DelInput.constraintVec);
	TriHVec().swap(DelOutput.triVec);
	TriOppHVec().swap(DelOutput.triOppVec);
	delaunayInput(mesh_2d, Voronoi, constrainMask, constrain_point, constrain_edge, DelInput.pointVec, DelInput.constraintVec, imageSize, scale, leftbound, lowerbound);
	//printf("%d points, %d segments", DelInput.pointVec.size(), DelInput.constraintVec.size());
	gpuDel.compute(DelInput, &DelOutput);
	printf("Constraint Delaunay triangulation done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// Parameter space -> surface
	SurfMesh resultMesh;
	recover(mesh_2d, seam_mesh, vertex_2d_to_3d, DelInput.pointVec, DelOutput.triVec, constrain_point, resultMesh);
	printf("Recover done. %f s\n", clock()*1.0 / CLOCKS_PER_SEC);

	// compute average edge length
	double edge_sum = 0, edge_avg = 0;
	for (auto e : resultMesh.edges()) {
		edge_sum += PMP::edge_length(e, resultMesh);
	}
	edge_avg = edge_sum / resultMesh.number_of_edges();
	printf("Average edge length: %f\n", edge_sum / resultMesh.number_of_halfedges());

	// Output to off file
	std::ofstream ostream("result.off");
	ostream << resultMesh;

	free(densityMap);
	free(Voronoi);
	free(constrainMask);

#ifdef VISUALIZE_TRIANGULATION
	// Visualize the constraint Delaunay triangualtion
	Visualizer::instance()->resume();
	Visualizer::instance()->addFrame(DelInput.pointVec, DelInput.constraintVec, DelOutput.triVec);
	vis->run();
#endif

	return EXIT_SUCCESS;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/units/units_fwd.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/unit.hpp>

namespace boost {

namespace units {

struct length_base_dimension : boost::units::base_dimension<length_base_dimension,1> { };       ///> base dimension of length
struct time_base_dimension : boost::units::base_dimension<time_base_dimension,3> { };           ///> base dimension of time

typedef length_base_dimension::dimension_type    length_dimension;
typedef time_base_dimension::dimension_type      time_dimension;

struct length1_base_unit : base_unit<length1_base_unit,length_dimension,1>
{
    static std::string name()               { return "length 1"; }
    static std::string symbol()             { return "l1"; }
};

struct length2_base_unit : base_unit<length2_base_unit,length_dimension,2>
{
    static std::string name()               { return "length2"; }
    static std::string symbol()             { return "l2"; }
};

struct time1_base_unit : base_unit<time1_base_unit,time_dimension,3>
{
    static std::string name()               { return "time1"; }
    static std::string symbol()             { return "t1"; }
};

struct time2_base_unit : base_unit<time2_base_unit,time_dimension,4>
{
    static std::string name()               { return "time2"; }
    static std::string symbol()             { return "t2"; }
};

namespace s1 {

typedef make_system<length1_base_unit,time1_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(length1,length);
BOOST_UNITS_STATIC_CONSTANT(time1,time);

} // namespace s1

namespace s2 {

typedef make_system<length2_base_unit,time2_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(length2,length);
BOOST_UNITS_STATIC_CONSTANT(time2,time);

} // namespace s2

template<class X,class Y>
struct conversion_helper< quantity<s1::length,X>,quantity<s2::length,Y> >
{
    static quantity<s2::length,Y> convert(const quantity<s1::length,X>& source)
    {
        return quantity<s2::length,Y>::from_value(2.5*source.value());
    }
};

template<class X,class Y>
struct conversion_helper< quantity<s2::length,X>,quantity<s1::length,Y> >
{
    static quantity<s1::length,Y> convert(const quantity<s2::length,X>& source)
    {
        return quantity<s1::length,Y>::from_value((1.0/2.5)*source.value());
    }
};

template<class X,class Y>
struct conversion_helper< quantity<s1::time,X>,quantity<s2::time,Y> >
{
    static quantity<s2::time,Y> convert(const quantity<s1::time,X>& source)
    {
        return quantity<s2::time,Y>::from_value(0.5*source.value());
    }
};

} // namespace units

} // namespace boost

int main(void)
{
    using namespace boost::units;

    quantity<s1::length,float>  l1(1.0*s1::length1);
    quantity<s2::length,double> l2(1.5*l1);
    quantity<s1::length,float>  l3(2.0*l2/3.0);

    quantity<s1::time,float>    t1(1.0*s1::time1);
    quantity<s2::time,double>   t2(1.5*t1);
//    quantity<s1::time,float>    t3(2.0*t2/3.0);

    return 0;
}

/*
// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/units/units_fwd.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/unit.hpp>

namespace boost {

namespace units {

struct length_base_dimension : boost::units::base_dimension<length_base_dimension,1> { };       ///> base dimension of length
struct mass_base_dimension : boost::units::base_dimension<mass_base_dimension,2> { };           ///> base dimension of mass
struct time_base_dimension : boost::units::base_dimension<time_base_dimension,3> { };           ///> base dimension of time

typedef length_base_dimension::dimension_type    length_dimension;
typedef mass_base_dimension::dimension_type      mass_dimension;
typedef time_base_dimension::dimension_type      time_dimension;

struct centimeter_base_unit : base_unit<centimeter_base_unit,length_dimension,1>
{
    static std::string name()               { return "centimeter"; }
    static std::string symbol()             { return "cm"; }
};

struct gram_base_unit : base_unit<gram_base_unit,mass_dimension,2>
{
    static std::string name()               { return "gram"; }
    static std::string symbol()             { return "g"; }
};

struct second_base_unit : base_unit<second_base_unit,time_dimension,3>
{
    static std::string name()               { return "second"; }
    static std::string symbol()             { return "s"; }
};

namespace CG {

typedef make_system<centimeter_base_unit,gram_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);

} // namespace CG

namespace cgs {

typedef make_system<centimeter_base_unit,gram_base_unit,second_base_unit>::type system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);

} // namespace cgs

namespace esu {

typedef make_system<centimeter_base_unit,
                    gram_base_unit,
                    second_base_unit>::type system;

/// derived dimension for force in electrostatic units : L M T^-2
typedef derived_dimension<length_base_dimension,1,
                          mass_base_dimension,1,
                          time_base_dimension,-2>::type                                             force_dimension;

/// derived dimension for charge in electrostatic units : L^3/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<3,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     charge_dimension;

/// derived dimension for current in electrostatic units : L^3/2 M^1/2 T^-2
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<3,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-2> > > >::type     current_dimension;

/// derived dimension for electric potential in electrostatic units : L^1/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<1,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     electric_potential_dimension;

/// derived dimension for electric field in electrostatic units : L^-1/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<-1,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     electric_field_dimension;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;
typedef unit<time_dimension,system>         time;

typedef unit<force_dimension,system>        force;

typedef unit<charge_dimension,system>               charge;
typedef unit<current_dimension,system>              current;
typedef unit<electric_potential_dimension,system>   electric_potential;
typedef unit<electric_field_dimension,system>       electric_field;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);

BOOST_UNITS_STATIC_CONSTANT(dyne,force);

BOOST_UNITS_STATIC_CONSTANT(esu,charge);
BOOST_UNITS_STATIC_CONSTANT(statvolt,electric_potential);

} // namespace esu

template<class Y>
quantity<esu::force,Y> coulombLaw(const quantity<esu::charge,Y>& q1,
                                  const quantity<esu::charge,Y>& q2,
                                  const quantity<esu::length,Y>& r)
{
    return q1*q2/(r*r);
}

} // namespace units

} // namespace boost

int main(void)
{
    using namespace boost::units;

    quantity<CG::length>    cg_length(1.0*CG::centimeter);
    quantity<cgs::length>   cgs_length(1.0*cgs::centimeter);

    std::cout << cg_length/cgs_length << std::endl;

    std::cout << esu::gram*pow<2>(esu::centimeter/esu::second)/esu::esu << std::endl;
    std::cout << esu::statvolt/esu::centimeter << std::endl;

    quantity<esu::charge>   q1 = 1.0*esu::esu,
                            q2 = 2.0*esu::esu;
    quantity<esu::length>   r = 1.0*esu::centimeter;

    std::cout << coulombLaw(q1,q2,r) << std::endl;
    std::cout << coulombLaw(q1,q2,cgs_length) << std::endl;

    return 0;
}
*/


#pragma once

// deal.II includes
#include <deal.II/base/types.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe.h>
#include <deal.II/fe/fe_face.h>
#include <deal.II/fe/fe_values.h>

#include <Eigen/Core>

// system includes
#include <map>
#include <vector>

namespace boltzmann {

class FaceOrientation2D
{
 protected:
  static const int dimX = 2;
  typedef dealii::DoFHandler<dimX> dof_handler_t;
  typedef typename dof_handler_t::active_cell_iterator cell_it;
  typedef dealii::types::global_dof_index size_type;

  /// key: <cell iterator, face_idx>
  typedef std::tuple<cell_it, int> key_t;
  typedef struct
  {
    Eigen::Vector2d normal_vector;
    /// DoF (vertex) indices right
    size_type phys_idx_r;
    /// DoF (vertex) indices left
    size_type phys_idx_l;
    double length;
  } face_data_t;

 public:
  typedef std::map<key_t, face_data_t> face_data_map_t;

 public:
  void init(const dof_handler_t& dof_handler);

  const face_data_map_t& get_faces_map() const { return face_data_map_; }

  const std::vector<face_data_t>& get_faces() const { return faces_; }

 protected:
  face_data_map_t face_data_map_;
  std::vector<face_data_t> faces_;
};

void
FaceOrientation2D::init(const dof_handler_t& dof_handler)
{
  // faces -> local fe dofs
  std::array<std::array<size_type, 2>, 4> faces2localdofs;
  faces2localdofs[0] = {0, 2};
  faces2localdofs[1] = {1, 3};
  faces2localdofs[2] = {0, 1};
  faces2localdofs[3] = {2, 3};

  const auto& fe = dof_handler.get_fe();
  dealii::UpdateFlags update_flags = dealii::update_values | dealii::update_JxW_values |
                                     dealii::update_quadrature_points |
                                     dealii::update_normal_vectors;
  dealii::QGauss<1> quad(1);
  dealii::FEFaceValues<dimX, dimX> fe_face_values(fe, quad, update_flags);

  const unsigned int dofs_per_cell = fe_face_values.dofs_per_cell;
  std::vector<size_type> local_dof_indices(dofs_per_cell);

  for (auto cell = dof_handler.begin_active(); cell != dof_handler.end(); ++cell) {
    cell->get_dof_indices(local_dof_indices);
    for (unsigned int face_idx = 0; face_idx < dealii::GeometryInfo<dimX>::faces_per_cell;
         ++face_idx) {
      if (cell->face(face_idx)->at_boundary()) {
        // *** Face is located at boundary ***
        fe_face_values.reinit(cell, face_idx);

        // relevant FE dofs
        const double nx = fe_face_values.normal_vector(0)[0];
        const double ny = fe_face_values.normal_vector(0)[1];
        face_data_t face_data;
        face_data.normal_vector << nx, ny;

        size_type phys_idx_l = local_dof_indices[faces2localdofs[face_idx][0]];
        size_type phys_idx_r = local_dof_indices[faces2localdofs[face_idx][1]];
        face_data.phys_idx_l = phys_idx_l;
        face_data.phys_idx_r = phys_idx_r;
        face_data.length = cell->face(face_idx)->measure();

        auto key = std::make_tuple(cell, face_idx);
        face_data_map_[key] = face_data;
      }
    }
  }

  for (auto it = face_data_map_.begin(); it != face_data_map_.end(); ++it) {
    faces_.push_back(it->second);
  }
}

}  // end namespace boltzmann


// SNCOperators.cpp - Code for the SNC operator equations.
// See SNCOperators.hpp for details.
//
// Copyright (c) 2016 Timothy Zhu.
// Licensed under the MIT License. See LICENSE file for details.
//

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <stdint.h>
#include <set>
#include <vector>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include "ProcessedTrace.hpp"
#include "time.hpp"
#include "search.hpp"
#include "NC.hpp"
#include "MGF.hpp"
#include "SNCOperators.hpp"

using namespace std;

const uint64_t MMBPArrival::intervalWidth = ConvertSecondsToTime(1);

// Check if there is a dependency between bound and this. A dependency exists if both bound and this share the same flow id in their dependency sets.
bool DependencyParams::checkDependence(const DependencyParams* bound) const
{
    const set<FlowId>& A = getDependencies();
    const set<FlowId>& B = bound->getDependencies();
    set<FlowId>::const_iterator itA = A.begin();
    set<FlowId>::const_iterator itB = B.begin();
    while ((itA != A.end()) && (itB != B.end())) {
        if (*itA == *itB) {
            return true;
        } else if (*itA < *itB) {
            itA++;
        } else {
            itB++;
        }
    }
    return false;
}

// Top-level function for analyzing a trace and initializing the MMBP parameters based on the analysis.
void MMBPArrival::init(ProcessedTrace* pTrace)
{
    // Split trace into intervals of length intervalWidth
    vector<double> intervals;
    countArrivalIntervals(pTrace, intervals);
    // Assign a state to each interval using the LAMBDA algorithm
    vector<unsigned int> states;
    vector<double> lambdas;
    unsigned int numStates = determineStatesLAMBDA(intervals, states, lambdas, 2.0);
    // Initialize the transition matrix between states
    initTransitionMatrix(numStates, states);
    // Initialize MGFs for each state
    initMGFs(pTrace, numStates, states, lambdas);
}

// Count the number of arrivals in each interval in the trace pTrace and store the results in intervals.
void MMBPArrival::countArrivalIntervals(ProcessedTrace* pTrace, vector<double>& intervals) const
{
    intervals.clear();
    int count = 0;
    uint64_t nextIntervalTime = intervalWidth;
    ProcessedTraceEntry traceEntry;
    pTrace->reset();
    while (pTrace->nextEntry(traceEntry)) {
        while (traceEntry.arrivalTime >= nextIntervalTime) {
            intervals.push_back(count);
            count = 0;
            nextIntervalTime += intervalWidth;
        }
        count++;
    }
    intervals.push_back(count);
}

// Helper function for performing the LAMBDA algorithm.
double MMBPArrival::LAMBDAAlgorithm(double a, void* params)
{
    LAMBDAParams_t* p = static_cast<LAMBDAParams_t*>(params);
    double low = p->low;
    double high = p->high;
    vector<double>* pLambdas = p->pLambdas;
    double lambda = sqrt(high + a * a / 4.0) - (a / 2.0);
    lambda = lambda * lambda;
    for (unsigned int state = pLambdas->size() - 1; state > 0; state--) {
        if (lambda < low) {
            return -numeric_limits<double>::infinity();
        }
        (*pLambdas)[state] = lambda;
        lambda = sqrt(lambda) - a;
        lambda = lambda * lambda;
    }
    (*pLambdas)[0] = lambda;
    return lambda - a * sqrt(lambda);
}

// Assign a state to each interval in the trace using the LAMBDA algorithm. Returns number of states used, up to maxNumStates.
unsigned int MMBPArrival::determineStatesLAMBDA(const vector<double>& intervals, vector<unsigned int>& states, vector<double>& lambdas, double a) const
{
    // Find largest and smallest interval size
    LAMBDAParams_t params;
    double& low = params.low;
    double& high = params.high;
    params.pLambdas = &lambdas;
    low = intervals[0];
    high = intervals[0];
    for (unsigned int intervalIndex = 1; intervalIndex < intervals.size(); intervalIndex++) {
        if (intervals[intervalIndex] < low) {
            low = intervals[intervalIndex];
        }
        if (intervals[intervalIndex] > high) {
            high = intervals[intervalIndex];
        }
    }
    // Determine lambdas with LAMBDA algorithm
    unsigned int numStates = maxNumStates;
    lambdas.resize(numStates, 0);
    if (isfinite(LAMBDAAlgorithm(a, &params))) {
        // Maximum number of states used, search for best confidence interval for the max number of states to cover the range
        a = binarySearchReverse(0, high, low, 0.01, LAMBDAAlgorithm, &params);
        LAMBDAAlgorithm(a, &params);
    } else {
        // Less than the max number of states needed for a given confidence interval, remove unnecessary states
        unsigned int unusedStates = 0;
        for (unsigned int state = 0; state < lambdas.size(); state++) {
            if (lambdas[state] > 0) {
                lambdas[state - unusedStates] = lambdas[state];
            } else {
                unusedStates++;
            }
        }
        numStates -= unusedStates;
        lambdas.resize(numStates);
    }
    // Assign states to intervals
    states.resize(intervals.size(), 0);
    for (unsigned int intervalIndex = 0; intervalIndex < intervals.size(); intervalIndex++) {
        states[intervalIndex] = 0;
        for (unsigned int state = lambdas.size() - 1; state > 0; state--) {
            double lambda = lambdas[state];
            if (intervals[intervalIndex] > (lambda - a * sqrt(lambda))) {
                states[intervalIndex] = state;
                break;
            }
        }
    }
    return numStates;
}

// Initialize the transition matrix between states.
void MMBPArrival::initTransitionMatrix(unsigned int numStates, const vector<unsigned int>& states)
{
    // Initialize transition matrix size
    _transitionMatrix.resize(numStates);
    for (unsigned int state = 0; state < numStates; state++) {
        _transitionMatrix[state].resize(numStates);
    }
    // Calculate transition matrix
    vector<uint64_t> stateDurations(numStates, 0);
    unsigned int fromState = states[0];
    stateDurations[fromState] += intervalWidth;
    for (unsigned int stateIndex = 1; stateIndex < states.size(); stateIndex++) {
        unsigned int toState = states[stateIndex];
        stateDurations[toState] += intervalWidth;
        _transitionMatrix[fromState][toState] += 1;
        fromState = toState;
    }
    for (unsigned int fromState = 0; fromState < numStates; fromState++) {
        double stateSteps = floor(ConvertTimeToSeconds(stateDurations[fromState]) / stepSize);
        if (stateSteps == 0) {
            stateSteps = 1;
        }
        double probTransition = 0;
        _transitionMatrix[fromState][fromState] = 0;
        for (unsigned int toState = 0; toState < numStates; toState++) {
            _transitionMatrix[fromState][toState] /= stateSteps;
            probTransition += _transitionMatrix[fromState][toState];
        }
        _transitionMatrix[fromState][fromState] = 1.0 - probTransition;
    }
}

// Helper function for creating the MGF for a MMBP state.
MGF* MMBPArrival::createMMBPStateMGF()
{
    return new MGFExponential();
}

// Initialize each MMBP state with its associated arrival rate and request size distribution, which is represented by a moment generating function (MGF).
void MMBPArrival::initMGFs(ProcessedTrace* pTrace, unsigned int numStates, const vector<unsigned int>& states, const vector<double>& lambdas)
{
    // Initialize MGFs
    _MGFs.resize(numStates);
    for (unsigned int state = 0; state < numStates; state++) {
        _MGFs[state] = createMMBPStateMGF();
    }
    // Estimate MGFs based on trace
    uint64_t nextIntervalTime = intervalWidth;
    unsigned int stateIndex = 0;
    ProcessedTraceEntry traceEntry;
    pTrace->reset();
    while (pTrace->nextEntry(traceEntry)) {
        while (traceEntry.arrivalTime >= nextIntervalTime) {
            stateIndex++;
            nextIntervalTime += intervalWidth;
        }
        unsigned int state = states[stateIndex];
        _MGFs[state]->addSampleRequest(traceEntry);
    }
    // Set prob of generating a request for each MGF
    for (unsigned int state = 0; state < numStates; state++) {
        _MGFs[state]->setProbRequest(lambdas[state] * stepSize / ConvertTimeToSeconds(intervalWidth));
    }
}

// Calculate the spectral radius of the matrix: Diag(_MGFs(theta)) * _transitionMatrix,
// where Diag(_MGFs(theta)) is the diagonal matrix from evaluating each entry of _MGFs for a given theta value.
double MMBPArrival::calcSpectralRadius(double theta) const
{
    if (_MGFs.size() == 2) {
        // Hand-solved solution for 2 states
        double MGF0 = _MGFs[0]->calcMGF(theta);
        double MGF1 = _MGFs[1]->calcMGF(theta);
        double L1 = (_transitionMatrix[0][0] * MGF0 + _transitionMatrix[1][1] * MGF1 + sqrt((_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) * (_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) + 4.0 * _transitionMatrix[0][1] * _transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
        double L2 = (_transitionMatrix[0][0] * MGF0 + _transitionMatrix[1][1] * MGF1 - sqrt((_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) * (_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) + 4.0 * _transitionMatrix[0][1] * _transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
        return max(fabs(L1), fabs(L2));
    } else {
        // Generic solution for n states
        Eigen::MatrixXd m(_MGFs.size(), _MGFs.size());
        for (unsigned int fromState = 0; fromState < _transitionMatrix.size(); fromState++) {
            double stateMGF = _MGFs[fromState]->calcMGF(theta);
            if (!isfinite(stateMGF)) {
                return numeric_limits<double>::infinity();
            }
            for (unsigned int toState = 0; toState < _transitionMatrix[fromState].size(); toState++) {
                m(fromState, toState) = stateMGF * _transitionMatrix[fromState][toState];
            }
        }
        return m.eigenvalues().cwiseAbs().maxCoeff();
    }
}

// Equations for the MMBPArrival operator, representing an arrival process of a flow as analyzed by its trace.
void MMBPArrival::calcBound(double theta, double* sigma, double* rho) const
{
    *sigma = 0;
    *rho = log(calcSpectralRadius(theta)) / theta;
}

// Equations for the ConstantService operator, representing a constant service process with rate c.
void ConstantService::calcBound(double theta, double* sigma, double* rho) const
{
    *sigma = 0;
    *rho = -_c;
}

// Equations for the AggregateArrival operator, representing the aggregation of two arrival processes A and B.
void AggregateArrival::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaB, rhoB;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _B->calcBound(getQ() * theta, &sigmaB, &rhoB);
    *sigma = sigmaA + sigmaB;
    *rho = rhoA + rhoB;
}

// Equations for the ConvolutionService operator, representing the convolution of two service processes S and T.
void ConvolutionService::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaS, rhoS;
    double sigmaT, rhoT;
    _S->calcBound(getP() * theta, &sigmaS, &rhoS);
    _T->calcBound(getQ() * theta, &sigmaT, &rhoT);
    // Handle the rhoS == rhoT case
    if (rhoS == rhoT) {
        rhoS *= 0.99;
    }
    *sigma = sigmaS + sigmaT - log(1.0 - exp(-theta * abs(rhoS - rhoT))) / theta;
    *rho = max(rhoS, rhoT);
}

// Equations for the OutputArrival operator, representing the departure process of an arrival process A after leaving a queue with service process S.
void OutputArrival::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    *sigma = sigmaA + sigmaS - log(1.0 - exp(theta * (rhoA + rhoS))) / theta;
    *rho = rhoA;
}

// Equations for the LeftoverService operator, representing the remaining service process that is leftover once a queue with service process S has accounted for the behavior of an arrival process A.
void LeftoverService::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    *sigma = sigmaA + sigmaS;
    *rho = rhoA + rhoS;
}

// Calculate the latency bound for a given theta value. Each invocation with a positive theta produces a valid (but possibly sub-optimal) upper bound on latency.
double LatencyBound::calcLatency(double theta) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    double latency = (log(_epsilon * (1.0 - exp(theta * (rhoA + rhoS)))) / theta - (sigmaA + sigmaS)) / rhoS;
    return latency * stepSize;
}

// Optimize over the space of positive theta values to search for the theta value that produces the best (i.e., tightest) latency bound.
double LatencyBound::calcTheta() const
{
    const double MIN_THETA = 1e-9;
    const double INITIAL_THETA = 1000.0;
    const double STEP_SIZE_DECREASE_FACTOR = 10.0;
    const double INITIAL_STEP_SIZE = INITIAL_THETA / STEP_SIZE_DECREASE_FACTOR;
    void* params = const_cast<LatencyBound*>(this);
    double theta = INITIAL_THETA;
    for (double stepSize = INITIAL_STEP_SIZE; stepSize >= MIN_THETA; stepSize /= STEP_SIZE_DECREASE_FACTOR) {
        theta = minSearch(max(theta - (STEP_SIZE_DECREASE_FACTOR * stepSize), MIN_THETA), theta + (STEP_SIZE_DECREASE_FACTOR * stepSize), stepSize, calcLatency, params);
    }
    return theta;
}

// Static helper function used by calcTheta.
double LatencyBound::calcLatency(double theta, void* params)
{
    const LatencyBound* p = static_cast<const LatencyBound*>(params);
    return p->calcLatency(theta);
}

// Calculate the latency bound using an optimized theta value.
double LatencyBound::calcLatency() const
{
    return calcLatency(calcTheta());
}

// Optimize the Hoelder dependency parameters. More research is needed to improve speed and accuracy.
double LatencyBound::dependencyOptimization() const
{
    const vector<DependencyParams*>& bounds = getDependentBounds();
    const unsigned int SEARCH_RANGE_DECREASE_COUNT = 25;
    const double SEARCH_RANGE_DECREASE_FACTOR = 1.2;
    const unsigned int ITERATION_COUNT = bounds.size() * 10;
    double minLatency = calcLatency();
    // Early exit if no dependencyParams to optimize
    if (bounds.size() == 0) {
        return minLatency;
    }
    // Optimize for the best Hoelder p value
    vector<double> bestP(bounds.size());
    for (unsigned int index = 0; index < bounds.size(); index++) {
        bestP[index] = bounds[index]->getP();
    }
    srand(1); // use fixed seed to avoid different latency calculations across multiple calls
    for (unsigned int i = 0; i < SEARCH_RANGE_DECREASE_COUNT; i++) {
        // Randomly search within search space
        for (unsigned int iter = 0; iter < ITERATION_COUNT; iter++) {
            // Set p/q
            for (unsigned int index = 0; index < bounds.size(); index++) {
                DependencyParams* bound = bounds[index];
                double searchRangeP = bound->getUpperP() - bound->getLowerP();
                double searchRangeQ = bound->getUpperQ() - bound->getLowerQ();
                double r = static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
                r *= searchRangeP + searchRangeQ;
                if (r <= searchRangeP) {
                    bound->setP(bound->getLowerP() + r);
                } else {
                    r -= searchRangeP;
                    bound->setQ(bound->getLowerQ() + r);
                }
            }
            // Check for better latency
            double latency = calcLatency();
            if (latency < minLatency) {
                minLatency = latency;
                for (unsigned int index = 0; index < bounds.size(); index++) {
                    bestP[index] = bounds[index]->getP();
                }
            }
        }
        // Update search space
        for (unsigned int index = 0; index < bounds.size(); index++) {
            DependencyParams* bound = bounds[index];
            bound->setP(bestP[index]);
            double searchRangeP = bound->getUpperP() - bound->getLowerP();
            searchRangeP /= SEARCH_RANGE_DECREASE_FACTOR;
            double searchRangeQ = bound->getUpperQ() - bound->getLowerQ();
            searchRangeQ /= SEARCH_RANGE_DECREASE_FACTOR;
            bound->setLowerP(max(bound->getP() - (searchRangeP / 2.0), 1.001));
            bound->setUpperP(bound->getLowerP() + searchRangeP);
            bound->setLowerQ(max(bound->getQ() - (searchRangeQ / 2.0), 1.001));
            bound->setUpperQ(bound->getLowerQ() + searchRangeQ);
        }
    }
    return minLatency;
}


/* The MIT License

Copyright (c) 2011 Sahab Yazdani

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "stippler_impl.h"

#include <fstream>
#include <limits>
#include <cstring>

#include <boost/random.hpp>

#include "VoronoiDiagramGenerator.h"

Stippler::Stippler( const StipplingParameters &parameters )
: IStippler(),
vertsX(new float[parameters.points]), vertsY(new float[parameters.points]), radii(new float[parameters.points]),
displacement(std::numeric_limits<float>::max()),
image(parameters.inputFile),
parameters(parameters) {
	createInitialDistribution();
}

Stippler::~Stippler() {
	delete[] radii;
	delete[] vertsX;
	delete[] vertsY;
}

void Stippler::distribute() {
	createVoronoiDiagram();
	redistributeStipples();
}

float Stippler::getAverageDisplacement() {
	return displacement;
}

void Stippler::createInitialDistribution() {
	using std::ceil;

	// find initial distribution
	boost::mt19937 rng;
	boost::uniform_01<boost::mt19937, float> generator( rng );

	float w = (float)(image.getWidth() - 1), h = (float)(image.getHeight() - 1);
	float xC, yC;

	for ( unsigned int i = 0; i < parameters.points; ) {
		xC = generator() * w;
		yC = generator() * h;

		// do a nearest neighbour search on the vertices
		if ( ceil(generator() * 255.0f) <= image.getIntensity( xC, yC ) ) {
			vertsX[i] = xC;
			vertsY[i] = yC;
			radii[i] = 0.0f;

			i++;
		}
	}
}

void Stippler::getStipples( StipplePoint *dst ) {
	StipplePoint *workingPtr;

	for (unsigned int i = 0; i < parameters.points; i++ ) {
		workingPtr = &(dst[i]);

		workingPtr->x = vertsX[i];
		workingPtr->y = vertsY[i];
		workingPtr->radius = radii[i];

		image.getColour(vertsX[i], vertsY[i], workingPtr->r, workingPtr->g, workingPtr->b);
	}
}

void Stippler::createVoronoiDiagram() {
	VoronoiDiagramGenerator generator;

	generator.generateVoronoi( vertsX, vertsY, parameters.points,
		0.0f, (float)(image.getWidth() - 1), 0.0f, (float)(image.getHeight() - 1) );

	edges.clear();

	Point< float > p1, p2;
	Edge< float > edge;

	generator.resetIterator();
	while ( generator.getNext(
		edge.begin.x, edge.begin.y, edge.end.x, edge.end.y,
		p1.x, p1.y, p2.x, p2.y ) ) {

		if ( edge.begin == edge.end ) {
			continue;
		}

		if ( edges.find( p1 ) == edges.end() ) {
			edges[p1] = EdgeList();
		}
		if ( edges.find( p2 ) == edges.end() ) {
			edges[p2] = EdgeList();
		}
		edges[p1].push_back( edge );
		edges[p2].push_back( edge );
	}
}

void Stippler::redistributeStipples() {
	using std::pow;
	using std::sqrt;
	using std::pair;
	using std::make_pair;
	using std::vector;

	vector< pair< Point< float >, EdgeList > > vectorized;
	for ( EdgeMap::iterator key_iter = edges.begin(); key_iter != edges.end(); ++key_iter ) {
		vectorized.push_back(make_pair(key_iter->first, key_iter->second));
	}

	float local_displacement = 0.0f;

	#pragma omp parallel for reduction(+:local_displacement)
	for (int i = 0; i < (int)vectorized.size(); i++) {
		pair< Point< float >, EdgeList > item = vectorized[i];
		pair< Point<float>, float > centroid = calculateCellCentroid( item.first, item.second );

		radii[i] = centroid.second;
		vertsX[i] = centroid.first.x;
		vertsY[i] = centroid.first.y;

		local_displacement += sqrt( pow( item.first.x - centroid.first.x, 2.0f ) + pow( item.first.y - centroid.first.y, 2.0f ) );
	}

	displacement = local_displacement / vectorized.size(); // average out the displacement
}

inline Line<float> Stippler::createClipLine( float insideX, float insideY, float x1, float y1, float x2, float y2 ) {
	using std::abs;
	using std::numeric_limits;

	Line<float> l;

	// if the floating point version of the line collapsed down to one
	// point, then just ignore it all
	if (abs(x1 - x2) < numeric_limits<float>::epsilon() && abs(y1 - y2) < numeric_limits<float>::epsilon()) {
		l.a = .0f;
		l.b = .0f;
		l.c = .0f;

		return l;
	}

	l.a = -(y1 - y2);
	l.b = x1 - x2;
	l.c = (y1 - y2) * x1 - (x1 - x2) * y1;

	// make sure the known inside point falls on the correct side of the clipping plane
	if ( insideX * l.a + insideY * l.b + l.c > 0.0f ) {
		l.a *= -1;
		l.b *= -1;
		l.c *= -1;
	}

	return l;
}

std::pair< Point<float>, float > Stippler::calculateCellCentroid( Point<float> &inside, EdgeList &edgeList ) {
	using std::make_pair;
	using std::numeric_limits;
	using std::vector;
	using std::floor;
	using std::ceil;
	using std::abs;
	using std::sqrt;
	using std::pow;

	vector< Line<float> > clipLines;
	Extents<float> extent = getCellExtents(edgeList);

	unsigned int x, y;

	float xDiff = ( extent.maxX - extent.minX );
	float yDiff = ( extent.maxY - extent.minY );

	unsigned int tileWidth = (unsigned int)ceil(xDiff) * parameters.subpixels;
	unsigned int tileHeight = (unsigned int)ceil(yDiff) * parameters.subpixels;

	float xStep = xDiff / (float)tileWidth;
	float yStep = yDiff / (float)tileHeight;

	float spotDensity, areaDensity = 0.0f, maxAreaDensity = 0.0f;
	float xSum = 0.0f;
	float ySum = 0.0f;

	float xCurrent;
	float yCurrent;

	// compute the clip lines
	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		Line<float> l = createClipLine( inside.x, inside.y,
			value_iter->begin.x, value_iter->begin.y,
			value_iter->end.x, value_iter->end.y );

		if (l.a < numeric_limits<float>::epsilon() && abs(l.b) < numeric_limits<float>::epsilon()) {
			continue;
		}

		clipLines.push_back(l);
	}

	for ( y = 0, yCurrent = extent.minY; y < tileHeight; ++y, yCurrent += yStep ) {
		for ( x = 0, xCurrent = extent.minX; x < tileWidth; ++x, xCurrent += xStep ) {
			// a point is outside of the polygon if it is outside of all clipping planes
			bool outside = false;
			for ( vector< Line<float> >::iterator iter = clipLines.begin(); iter != clipLines.end(); iter++ ) {
				if ( xCurrent * iter->a + yCurrent * iter->b + iter->c >= 0.0f ) {
					outside = true;
					break;
				}
			}

			if (!outside) {
				spotDensity = image.getIntensity(xCurrent, yCurrent);

				areaDensity += spotDensity;
				maxAreaDensity += 255.0f;
				xSum += spotDensity * xCurrent;
				ySum += spotDensity * yCurrent;
			}
		}
	}

	float area = areaDensity * xStep * yStep / 255.0f;
	float maxArea = maxAreaDensity * xStep * yStep / 255.0f;

	Point<float> pt;
	if (areaDensity > numeric_limits<float>::epsilon()) {
		pt.x = xSum / areaDensity;
		pt.y = ySum / areaDensity;
	} else {
		// if for some reason, the cell is completely white, then the centroid does not move
		pt.x = inside.x;
		pt.y = inside.y;
	}

	float closest = numeric_limits<float>::max(),
		  farthest = numeric_limits<float>::min(),
		  distance;
	float x0 = pt.x, y0 = pt.y,
	      x1, x2, y1, y2;

	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		x1 = value_iter->begin.x; x2 = value_iter->end.x;
		y1 = value_iter->begin.y; y2 = value_iter->end.y;

		distance = abs( ( x2 - x1 ) * ( y1 - y0 ) - ( x1 - x0 ) * ( y2 - y1 ) ) / sqrt( pow( x2 - x1, 2.0f ) + pow( y2 - y1, 2.0f ) );
		if ( closest > distance ) {
			closest = distance;
		}
		if ( farthest < distance ) {
			farthest = distance;
		}
	}

	float radius;
	if ( parameters.noOverlap ) {
		radius = closest;
	} else {
		radius = farthest;
	}
	radius *= area / maxArea;

	return make_pair( pt, radius );
}

Extents<float> Stippler::getCellExtents( Stippler::EdgeList &edgeList ) {
	using std::numeric_limits;

	Extents<float> extent;

	extent.minX = extent.minY = numeric_limits<float>::max();
	extent.maxX = extent.maxY = numeric_limits<float>::min();

	for ( EdgeList::iterator value_iter = edgeList.begin(); value_iter != edgeList.end(); ++value_iter ) {
		if ( value_iter->begin.x < extent.minX ) extent.minX = value_iter->begin.x;
		if ( value_iter->end.x < extent.minX ) extent.minX = value_iter->end.x;
		if ( value_iter->begin.y < extent.minY ) extent.minY = value_iter->begin.y;
		if ( value_iter->end.y < extent.minY ) extent.minY = value_iter->end.y;

		if ( value_iter->begin.x > extent.maxX ) extent.maxX = value_iter->begin.x;
		if ( value_iter->end.x > extent.maxX ) extent.maxX = value_iter->end.x;
		if ( value_iter->begin.y > extent.maxY ) extent.maxY = value_iter->begin.y;
		if ( value_iter->end.y > extent.maxY ) extent.maxY = value_iter->end.y;
	}

	return extent;
}

bool operator==(Point<float> const& p1, Point<float> const& p2)
{
	using std::abs;
	using std::numeric_limits;

	return abs( p1.x - p2.x ) < numeric_limits<float>::epsilon() &&
		abs( p1.y - p2.y ) < numeric_limits<float>::epsilon();
}

std::size_t hash_value(Point<float> const& p) {
	using std::size_t;
	using boost::hash_combine;

    size_t seed = 0;

    hash_combine(seed, p.x);
    hash_combine(seed, p.y);

    return seed;
}