/* Connected components
 * developed and maintained by Srinivas C. Turaga <sturaga@mit.edu>
 * do not distribute without permission.
 */
#include "main2.h"
//#pragma once
#include "agglomeration.hpp"
#include "region_graph.hpp"
#include "basic_watershed.hpp"
#include "limit_functions.hpp"
#include "types.hpp"
#include "utils.hpp"

#include <memory>
#include <type_traits>

#include <iostream>
#include <fstream>
#include <cstdio>
#include <cstddef>
#include <cstdint>
#include <queue>
#include <vector>
#include <algorithm>
#include <tuple>
#include <map>
#include <list>
#include <set>
#include <vector>
#include <chrono>
#include <fstream>
#include <string>
#include <boost/make_shared.hpp>
using namespace std;

template< typename ID, typename F >
inline region_graph_ptr<ID,F>
get_merge_tree( const region_graph<ID,F>& rg, std::size_t max_segid )
{
    zi::disjoint_sets<ID>      sets(max_segid+1);
    std::vector<std::list<ID>> edges(max_segid+1);
    region_graph_ptr<ID,F>     mt_ptr( new region_graph<ID,F> );

    for ( const auto& e: rg )
    {
        ID v1 = std::get<1>(e);
        ID v2 = std::get<2>(e);

        ID s1 = sets.find_set(v1);
        ID s2 = sets.find_set(v2);

        if ( s1 != s2 && s1 && s2 )
        {
            mt_ptr->push_back(e);
            sets.join(s1, s2);

            edges[v1].push_back(v2);
            edges[v2].push_back(v1);

        }
    }

    std::vector<ID> order(max_segid+1);
    ID curr = 0;

    for ( ID i = 0; i <= max_segid; ++i )
    {
        if ( order[i] == 0 )
        {
            std::deque<ID> queue;
            queue.push_back(i);
            order[i] = ++curr;

            while ( queue.size() )
            {
                ID x = queue.front();
                queue.pop_front();

                for ( auto& y: edges[x] )
                {
                    if ( order[y] == 0 )
                    {
                        order[y] = ++curr;
                        queue.push_back(y);
                    }
                }
            }
        }
    }

    for ( auto& e: *mt_ptr )
    {
        if ( order[std::get<2>(e)] < order[std::get<1>(e)] )
        {
            std::swap(std::get<2>(e), std::get<1>(e));
        }
    }

    return mt_ptr;
}

template< typename ID,
          typename F,
          typename L,
          class = typename std::enable_if<is_numeric<F>::value>::type,
          class = typename std::enable_if<std::is_integral<ID>::value>::type,
          class = typename std::enable_if<std::is_convertible<L,F>::value>::type >
inline void yet_another_watershed( const volume_ptr<ID>& seg_ptr,
                                   const region_graph_ptr<ID,F> rg_ptr,
                                   std::vector<std::size_t>& counts,
                                   const L& lowl)
{
    F low = static_cast<F>(lowl);

    std::vector<std::size_t> new_counts({0});
    std::vector<ID>          remaps(counts.size());

    zi::disjoint_sets<ID>    sets(counts.size());
    std::vector<F>           maxs(counts.size());

    region_graph<ID,F>& rg  = *rg_ptr;

    ID next_id = 1;
    ID merged = 0;

    for ( auto& it: rg )
    {
        if ( std::get<0>(it) <= low )
        {
            break;
        }

        ID s1 = std::get<1>(it);
        ID s2 = std::get<2>(it);

        F f = std::get<0>(it);

        if ( s1 && s2 )
        {
            if ( (remaps[s1] == 0) || (remaps[s2] == 0) )
            {
                if ( remaps[s1] == 0 )
                {
                    std::swap(s1,s2);
                }

                if ( remaps[s1] == 0 )
                {
                    maxs[next_id] = f;
                    remaps[s1] = remaps[s2] = next_id;
                    new_counts.push_back(counts[s1]+counts[s2]);
                    ++next_id;
                }
                else
                {
                    ID actual = sets.find_set(remaps[s1]);
                    remaps[s2] = remaps[s1];
                    new_counts[actual] += counts[s2];
                }
            }
            else
            {
                ID a1 = sets.find_set(remaps[s1]);
                ID a2 = sets.find_set(remaps[s2]);

                if ( 0 && a1 != a2 && ((maxs[a1]==f)||(maxs[a2]==f)) )
                {
                    ++merged;
                    new_counts[a1] += new_counts[a2];
                    new_counts[a2] = 0;
                    maxs[a1] = std::max(maxs[a1],maxs[a2]);
                    maxs[a2] = 0;
                    ID a = sets.join(a1,a2);
                    std::swap(new_counts[a], new_counts[a1]);
                    std::swap(maxs[a], maxs[a1]);
                }
            }
        }
    }

    next_id -= merged;

    std::vector<ID> remaps2(counts.size());

    next_id = 1;

    for ( ID id = 0; id < counts.size(); ++id )
    {
        ID s = sets.find_set(remaps[id]);
        if ( s && (remaps2[s]==0) )
        {
            remaps2[s] = next_id;
            new_counts[next_id] = new_counts[s];
            ++next_id;
        }
    }

    new_counts.resize(next_id);

    std::ptrdiff_t xdim = seg_ptr->shape()[0];
    std::ptrdiff_t ydim = seg_ptr->shape()[1];
    std::ptrdiff_t zdim = seg_ptr->shape()[2];

    std::ptrdiff_t total = xdim * ydim * zdim;

    ID* seg_raw = seg_ptr->data();

    for ( std::ptrdiff_t idx = 0; idx < total; ++idx )
    {
        seg_raw[idx] = remaps2[remaps[seg_raw[idx]]];
    }

    region_graph<ID,F> new_rg;

    for ( auto& it: rg )
    {
        ID s1 = remaps2[remaps[std::get<1>(it)]];
        ID s2 = remaps2[remaps[std::get<2>(it)]];

        if ( s1 != s2 && s1 && s2 )
        {
            auto mm = std::minmax(s1,s2);
            new_rg.emplace_back(std::get<0>(it), mm.first, mm.second);
        }
    }

    rg.swap(new_rg);

    counts.swap(new_counts);

    std::cout << "New count: " << counts.size() << std::endl;

    std::cout << "Done with updating the region graph, size: "
              << rg.size() << std::endl;
}

std::size_t get_rand_idx( std::vector<uint32_t>& v)
{
    if ( v.size() <= 1 )
    {
        return 0;
    }

    std::size_t r = 0;

    for ( int i = 0; i < v.size() - 1; ++i )
    {
        for ( int j = i+1; j < v.size(); ++j )
            r += v[i]*v[j];
    }

    return r;
}

double square_sum( std::vector<uint32_t>& v)
{
    double r = 0;

    for ( const auto& x: v )
    {
        r += x*x;
    }

    return r;
}

template< typename ID >
void fill_void( ID* arr, std::size_t len )
{
    ID maxi = 0;
    for ( std::size_t i = 0; i < len; ++i )
    {
        maxi = std::max(maxi, arr[i]);
    }

    for ( std::size_t i = 0; i < len; ++i )
    {
        if ( arr[i] == 0 ) arr[i] = ++maxi;
    }
}

std::pair<double,double>
compare_volumes( volume<uint32_t>& gt,
                 volume<uint32_t>& ws,
                 std::size_t size )
{
    std::map<uint32_t, std::map<uint32_t, uint32_t>> map;
    std::map<uint32_t, std::map<uint32_t, uint32_t>> invmap;
    std::map<uint32_t, uint32_t> setg, setw;

    double rand_split = 0;
    double rand_merge = 0;

    double t_sq = 0;
    double s_sq = 0;

    double total = 0;
    std::map<uint32_t, std::map<uint32_t, std::size_t>> p_ij;

    std::map<uint32_t, std::size_t> s_i, t_j;

    for ( std::ptrdiff_t z = 28; z < size-28; ++z )
        for ( std::ptrdiff_t y = 28; y < size-28; ++y )
            for ( std::ptrdiff_t x = 16; x < size-16; ++x )
            {
                uint32_t wsv = ws[x][y][z];
                uint32_t gtv = gt[x][y][z];

                if ( gtv )
                {
                    ++total;

                    ++p_ij[gtv][wsv];
                    ++s_i[wsv];
                    ++t_j[gtv];
                }
            }

    double sum_p_ij = 0;
    for ( auto& a: p_ij )
    {
        for ( auto& b: a.second )
        {
            sum_p_ij += b.second * b.second;
        }
    }

    double sum_t_k = 0;
    for ( auto& a: t_j )
    {
        sum_t_k += a.second * a.second;
    }

    double sum_s_k = 0;
    for ( auto& a: s_i )
    {
        sum_s_k += a.second * a.second;
    }

    //std::cout << sum_p_ij << "\n";
    std::cout << "Rand Split: " << (sum_p_ij/sum_t_k) << "\n";
    std::cout << "Rand Merge: " << (sum_p_ij/sum_s_k) << "\n";
    std::cout << "Rand alpha: " << (sum_p_ij*2/(sum_t_k+sum_s_k)) << "\n";

    return std::make_pair(sum_p_ij/sum_t_k,
                          sum_p_ij/sum_s_k);
}

std::vector<double> reduce( const std::vector<double>& v )
{
    std::vector<double> ret;
    ret.push_back(v[0]);
    ret.push_back(v[1]);

    for ( std::size_t i = 2; i + 4 < v.size(); i += 2 )
    {
        double oldx = v[i-2];
        double oldy = v[i-1];

        double x = v[i];
        double y = v[i+1];

        double nextx = v[i+2];
        double nexty = v[i+3];

        if ( std::abs(oldx-x) > 0.0001 ||
             std::abs(oldy-y) > 0.0001 ||
             std::abs(nextx-x) > 0.0001 ||
             std::abs(nexty-y) > 0.0001 )
        {
            ret.push_back(x);
            ret.push_back(y);
        }
    }

    ret.push_back(v[v.size()-2]);
    ret.push_back(v[v.size()-1]);

    return ret;
}

std::pair<double,double>
compare_volumes_arb(
                 volume<uint32_t>& gt,
                 volume<uint32_t>& ws, int dimX, int dimY, int dimZ )
{
    //ws is seg
    //std::map<uint32_t, std::map<uint32_t, uint32_t>> map;
    //std::map<uint32_t, std::map<uint32_t, uint32_t>> invmap;
    //std::map<uint32_t, uint32_t> setg, setw;

    double rand_split = 0;
    double rand_merge = 0;

    double t_sq = 0;
    double s_sq = 0;

    double total = 0;
    std::map<uint32_t, std::map<uint32_t, std::size_t>> p_ij;

    std::map<uint32_t, std::size_t> s_i, t_j;

    for ( std::ptrdiff_t z = 0; z < dimX; ++z )
        for ( std::ptrdiff_t y = 0; y < dimY; ++y )
            for ( std::ptrdiff_t x = 0; x < dimZ; ++x )
            {
                uint32_t wsv = ws[x][y][z];
                uint32_t gtv = gt[x][y][z];

                if ( gtv )
                {
                    ++total;

                    ++p_ij[gtv][wsv];
                    ++s_i[wsv];
                    ++t_j[gtv];
                }
            }

    double sum_p_ij = 0;
    for ( auto& a: p_ij )
    {
        for ( auto& b: a.second )
        {
            sum_p_ij += b.second * b.second;
        }
    }

    double sum_t_k = 0;
    for ( auto& a: t_j )
    {
        sum_t_k += a.second * a.second;
    }

    double sum_s_k = 0;
    for ( auto& a: s_i )
    {
        sum_s_k += a.second * a.second;
    }

    //std::cout << sum_p_ij << "\n";
    std::cout << "Rand Split: " << (sum_p_ij/sum_t_k) << "\n";
    std::cout << "Rand Merge: " << (sum_p_ij/sum_s_k) << "\n";
    std::cout << "Rand alpha: " << (sum_p_ij*2/(sum_t_k+sum_s_k)) << "\n";

    return std::make_pair(sum_p_ij/sum_t_k,
                          sum_p_ij/sum_s_k);
}

using namespace std;

struct Vertex
{
    uint32_t first, second;
    float value;
};

typedef vector<Vertex> VertexList;

std::map<std::string,std::vector<double>> eval_c(int dimX, int dimY, int dimZ, int dcons, uint32_t* gt, float* affs,std::list<int> * threshes, std::list<std::string> * funcs, int save_seg, std::string* out_ptr)
{
/////////////////////////////////////////// LOAD DATA ///////////////////////////////////////////////////////////////
    bool write_dats = save_seg!=0;
    bool recreate_rg = false;
    bool debug = 1;
    // these values based on 5% at iter = 10000
    double LOW=  .0001;// 0.003785; //.00001; //default = .3
    double HIGH= .9999;// 0.999971; //.99988; //default = .99

    std::string out = *out_ptr;
    std::cout << "evaluating..." << std::endl;
    std::cout << "output: "<< out << std::endl;
    std::string experiment_result_folder = ".";

    volume_ptr<uint32_t> gt_ptr = read_volumes<uint32_t>("", dimX, dimY, dimZ);

    int totalDim = dimX*dimY*dimZ;
    for(int i=0;i<totalDim;i++){
        gt_ptr->data()[i] = gt[i];
    }

    std::cout << std::endl;

    affinity_graph_ptr<float> aff = read_affinity_graphe<float>("", dimX, dimY, dimZ, dcons);

    totalDim*=dcons;
    for(int i=0;i<totalDim;i++){
        aff->data()[i] = affs[i];
    }

    std::cout << "0_dim of affinity " << aff->shape()[0] << "\n";
    std::cout << "1_dim of affinity " << aff->shape()[1] << "\n";
    std::cout << "2_dim of affinity " << aff->shape()[2] << "\n";
    std::cout << "3_dim of affinity " << aff->shape()[3] << "\n";

    std::list<int>::const_iterator iterator;
    std::list<int> thresh_list = *threshes;

    /* loop over thresholds
    for (iterator = thresh_list.begin(); iterator != thresh_list.end(); ++iterator) {
        int thold = *iterator;
    */

    std::map<std::string,std::vector<double>> returnMap;
    volume_ptr<uint32_t>     seg_ref   ;
    std::vector<std::size_t> counts_ref;
    std::tie(seg_ref , counts_ref) = watershed<uint32_t>(aff, LOW, HIGH);
    auto rg = get_region_graph(aff, seg_ref , counts_ref.size()-1);
    volume_ptr<uint32_t>     seg   ;

/////////////////////////////////////////// SQUARE ///////////////////////////////////////////////////////////////

     if ( std::find(funcs->begin(), funcs->end(), "square") != funcs->end() )
     {
         std::cout << "\n\n\nsquare" << "\n";
         std::vector<double> r;

         for (iterator = thresh_list.begin(); iterator != thresh_list.end(); ++iterator) {
                 int thold = *iterator;

             std::cout << "THOLD: " << thold << "\n";
             {
                 seg.reset(new volume<uint32_t>(*seg_ref));
                 std::vector<std::size_t> counts(counts_ref);
                 merge_segments_with_function(seg, rg, counts, square(thold), 10,recreate_rg);
                 if(write_dats)
                    write_volume(out+"square/" + std::to_string(thold) + ".dat", seg);
                 auto x = compare_volumes_arb(*gt_ptr, *seg, dimX,dimY,dimZ);
                 r.push_back(x.first);
                 r.push_back(x.second);
             }
             write_to_file(out+"square.dat", r.data(), r.size());
         }

        returnMap["square"] = r;
     }

/////////////////////////////////////////// LINEAR ///////////////////////////////////////////////////////////////

    if ( std::find(funcs->begin(), funcs->end(), "linear") != funcs->end() )
    {
     std::cout << "\n\n\nlinear" << "\n";
      std::vector<double> r;
      for (iterator = thresh_list.begin(); iterator != thresh_list.end(); ++iterator) {
              int thold = *iterator;

          std::cout << "THOLD: " << thold << "\n";
          {
              seg.reset(new volume<uint32_t>(*seg_ref));
              std::vector<std::size_t> counts(counts_ref);

              merge_segments_with_function(seg, rg, counts,linear(thold), 10,recreate_rg);
              if(write_dats)
                 write_volume(out+"linear/"+ std::to_string(thold) + ".dat", seg);

              auto x = compare_volumes_arb(*gt_ptr, *seg, dimX,dimY,dimZ);
              r.push_back(x.first);
              r.push_back(x.second);
          }
          write_to_file(out+"linear.dat", r.data(), r.size());
      }
      returnMap["linear"] = r;
    }

/////////////////////////////////////////// SIMPLE THRESHOLD ///////////////////////////////////////////////////////////////

     if ( std::find(funcs->begin(), funcs->end(), "threshold") != funcs->end() ){
         std::cout << "\n\n\nsimple thold" << "\n";
         std::vector<double> r;
         for (iterator = thresh_list.begin(); iterator != thresh_list.end(); ++iterator) {
                 int thold = *iterator;

             std::cout << "THOLD: " << thold << "\n";

             {
                 seg.reset(new volume<uint32_t>(*seg_ref));
                 std::vector<std::size_t> counts(counts_ref);

                 merge_segments_with_function(seg, rg, counts,const_above_threshold(0.3, thold), 100,recreate_rg);
                 if(write_dats)
                    write_volume(out+"threshold/"+ std::to_string(thold) + ".dat", seg);

                 auto x = compare_volumes_arb(*gt_ptr, *seg, dimX,dimY,dimZ);
                 r.push_back(x.first);
                 r.push_back(x.second);
             }
         }
         write_to_file(out+"threshold.dat", r.data(), r.size());
         returnMap["threshold"] = r;
     }

/////////////////////////////////////////// LOWHIGH ///////////////////////////////////////////////////////////////

     if ( std::find(funcs->begin(), funcs->end(), "lowhigh") != funcs->end() ) {
         volume_ptr<uint32_t>     segg  ;
         std::vector<std::size_t> counts;

         std::tie(segg, counts) = watershed<uint32_t>(aff, -1, 2);

         //write_volume(out+"voutraw.out", segg);
         //
         // low high tholds
         //
         std::tie(segg, counts) = watershed<uint32_t>(aff, -1, 2);
         write_volume(out+"lowhigh/vout." + std::to_string(-1) + "." + std::to_string(2) + ".out", segg);

         for ( double low = .00001; low <= 0.00001; low += 0.00001 ) //default low = 0.01; low < 0.051; low += 0.01
         {
             for ( double sub = .1; sub >= 1e-8; sub*=.1 ) //default high = 0.998; high > 0.989; high -= 0.002
             {
                 double high = 1-sub;
                 std::cout << std::to_string(low) + "." + std::to_string(high) + ".out" << std::endl;
                 std::tie(segg, counts) = watershed<uint32_t>(aff, low, high);
                 write_volume(out+"lowhigh/vout." + std::to_string(low) + "." + std::to_string(high) + ".out", segg);
             }
         }

         std::tie(segg, counts) = watershed<uint32_t>(aff, 0.5, 2);

         //write_volume(out+"voutmax.out", segg);

         std::tie(segg, counts) = watershed<uint32_t>(aff, 0.3, 0.99);

         //write_volume(out+"voutminmax.out", segg);

    }

/////////////////////////////////////////// WATERSHED SEGS ///////////////////////////////////////////////////////////////

     if ( std::find(funcs->begin(), funcs->end(), "watershed") != funcs->end() )
         {
             std::cout << "\n\n\nwatershed" << "\n";
             volume_ptr<uint32_t>     seg   ;
             std::vector<std::size_t> counts;

             std::tie(seg, counts) = watershed<uint32_t>(aff, -1, 2);
             write_volume(out+"watershed/basic.out", seg);

             std::tie(seg, counts) = watershed<uint32_t>(aff, 0.1, 0.99);
             write_volume(out+"watershed/minmax.out", seg);

             if(debug){
                 std::tie(seg , counts) = watershed<uint32_t>(aff, LOW, HIGH);
                 write_volume(out+"watershed/3_99.out", seg);
             }
             // {
             //     std::tie(segg, counts) = watershed<uint32_t>(aff, 0.3, 0.99);

             //     auto rg = get_region_graph(aff, segg, counts.size()-1);
             //     merge_segments_with_function(segg, rg,
             //                                  counts, limit_fn2, 100,recreate_rg);

             //     write_volume(out+"voutall.out", segg);
             // }

         }
     // auto rg = get_region_graph(aff, segg, counts.size()-1);

     // //yet_another_watershed(segg, rg, counts, 0.3);

     // //write_volume("voutanouther.out", segg);

     // merge_segments_with_function(segg, rg, counts, limit_fn3, 100,recreate_rg);

     // write_volume("voutdo.out", segg);

     //auto rg = get_region_graph(aff, segg, counts.size()-1);

     //yet_another_watershed(segg, rg, counts, 0.3);

     //write_volume("voutanouther.out", segg);

     //auto r = merge_segments_with_function_err(segg, gt_ptr, rg,
     //                                          counts, limit_fn2, 100,recreate_rg);

     //write_to_file(out+"/precision_recall.dat",
     //              r.data(), r.size());

     //write_volume(out+"voutall4x.out", segg);

     //return 0;

     //write_region_graph(out+"voutall.rg", *rg);

     //auto mt = get_merge_tree(*rg, counts.size()-1);

     //write_region_graph(out+"voutall.mt", *mt);

     return returnMap;

 }


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_POPCNT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_POPCNT_HPP_INCLUDED

#ifdef BOOST_MSVC
#include <intrin.h>
#endif
#include <boost/simd/function/hi.hpp>
#include <boost/simd/function/lo.hpp>
#include <boost/simd/function/sbits.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( popcnt_
                          ,(typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0, unsigned> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return simd::popcnt(sbits(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD  ( popcnt_
                           ,(typename A0)
                           , bd::cpu_
                           , bd::scalar_< bd::ints8_<A0> >
                           )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0, unsigned> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
#ifdef BOOST_MSVC
      return __popcnt16(int16_t(a0) & 0xFF);
#else
      return __builtin_popcount(int32_t(a0) & 0xFF);
#endif
    }
  };

  BOOST_DISPATCH_OVERLOAD  ( popcnt_
                          ,(typename A0)
                           , bd::cpu_
                           , bd::scalar_< bd::ints16_<A0> >
                           )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0, unsigned> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_MSVC
      return __popcnt16(a0);
    #else
      return __builtin_popcount(int32_t(a0) & 0xFFFF);
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( popcnt_
                          ,(typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::ints32_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0, unsigned> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_MSVC
      return __popcnt(a0);
    #else
      return __builtin_popcount(a0);
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( popcnt_
                          ,(typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::ints64_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0, unsigned> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
    #if defined BOOST_MSVC && defined _WIN64
      return __popcnt64(a0);
    #elif defined BOOST_MSVC
      return  __popcnt( hi(a0) )
            + __popcnt( lo(a0) );
    #else
      return  __builtin_popcount( hi(a0) )
            + __builtin_popcount( lo(a0) );
    #endif
    }
  };
} } }

#endif


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "arithmetic.hpp"

#include <boost/range/counting_range.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/* move? */
bdd ite( const bdd& cond, const bdd& _then, const bdd& _else )
{
  return ( !cond && _else ) || ( cond && _then );
}

std::tuple<bdd, bdd> full_adder( const bdd& x, const bdd& y, const bdd& cin )
{
  const auto sum  = x ^ y ^ cin;
  const auto cout = ( x && y ) || ( x && cin ) || ( y && cin );

  return std::make_tuple( sum, cout );
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

std::vector<bdd> bdd_subtract( const std::vector<bdd>& minuend, const std::vector<bdd>& subtrahend )
{
  assert( minuend.size() == subtrahend.size() );
  assert( !minuend.empty() );

  std::vector<bdd> diff( minuend.size() );
  bdd carry = minuend.front().manager->bdd_top();

  for ( const auto& i : boost::counting_range( 0ul, static_cast<unsigned long>( minuend.size() ) ) )
  {
    std::tie( diff[i], carry ) = full_adder( minuend[i], !subtrahend[i], carry );
  }

  return diff;
}

std::vector<bdd> bdd_abs( const std::vector<bdd>& n )
{
  std::vector<bdd> mask( n.size(), n.back() );
  std::vector<bdd> result;

  for ( const auto& i : boost::counting_range( 0ul, static_cast<unsigned long>( n.size() ) ) )
  {
    result.push_back( n[i] ^ mask[i] );
  }

  auto diff = bdd_subtract( result, mask );
  std::copy( diff.begin(), diff.begin() + n.size(), result.begin() );

  return result;
}

std::vector<bdd> zero_extend( const std::vector<bdd>& n, unsigned to )
{
  assert( to >= n.size() );
  auto ze = n;
  ze.resize( to, n.front().manager->bdd_bot() );
  return ze;
}

std::vector<bdd> sign_extend( const std::vector<bdd>& n, unsigned to )
{
  assert( to >= n.size() );
  auto se = n;
  se.resize( to, n.back() );
  return se;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FLOOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FLOOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the greatest integral representable value of
    its parameter type which is less or equal to it.

    @par Header <boost/simd/function/floor.hpp>

    @par Notes

     - @c floor is also used as parameter to pass to @ref div or @ref rem

    @par Decorators

    - std_ for floating entries call std::floor

    @see  ceil, round, nearbyint, trunc, ifloor

    @par Example:

      @snippet floor.cpp floor

    @par Possible output:

      @snippet floor.txt floor

  **/
  Value floor(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/simd/floor.hpp>

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NORMAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NORMAL_HPP_INCLUDED

#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_not_denormal.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( is_normal_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE bs::as_logical_t<A0> operator() (const A0& a0) const BOOST_NOEXCEPT
    {
      return is_nez(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( is_normal_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE  bs::as_logical_t<A0> operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return   logical_and(logical_and(is_finite(a0), is_nez(a0)), is_not_denormal(a0));
    }
  };
} } }

#endif


#include "vtkArduPilotDataFlashLogReader.h"

#include <stdio.h>

#include <vtkObjectFactory.h>
#include <vtkInformationVector.h>

#include <boost/algorithm/string.hpp>

#include <Eigen/Geometry>

#include "GPSProjectionUtils.h"

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkArduPilotDataFlashLogReader)

//-----------------------------------------------------------------------------
vtkArduPilotDataFlashLogReader::vtkArduPilotDataFlashLogReader()
{
  this->GPSTrajectory = vtkSmartPointer<vtkTemporalTransforms>::New();
  this->SetNumberOfInputPorts(0);
  this->SetNumberOfOutputPorts(1);
}

//-----------------------------------------------------------------------------
int vtkArduPilotDataFlashLogReader::RequestData(vtkInformation *, vtkInformationVector **, vtkInformationVector *outputVector)
{
  std::ifstream f(this->FileName);
  if (!f.good())
  {
    vtkErrorMacro("Failed to open input file \"" << this->FileName << "\"");
    return VTK_ERROR;
  }

  bool offsetFound = false;
  Eigen::Vector3d offset;
  std::string line;
  while (std::getline(f, line))
  {
    boost::algorithm::trim(line);
    if (line.empty())
    {
      continue;
    }

    if (boost::starts_with(line, "GPS2"))
    {
      // GPS2 lines contain fields:
      // TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U
      // Useful: https://groups.google.com/forum/#!topic/swiftnav-discuss/XOr7WQto9ZI
      // quoting from: https://discuss.ardupilot.org/t/correct-gps-time-stamp/14329
      // "GMS and GWK would be the best way to get the current gps time yes.
      // the tick with that though is there is latency vs the measurement data.
      // so correlating that data could be interesting."
      // GMS = GPS ms since beginning of week
      // GWk = None # GPS week (a GPS week is 7*24h afaik)
      // Status: 0 = no GPS, 1 = GPS but no fix, 2 = GPS with 2D fix, 3 = GPS with 3D fix

      std::vector<std::string> elements;
      boost::algorithm::split(
        elements, line, boost::is_any_of(", "), boost::token_compress_on);

      double GMS = std::stod(elements[3]); // ms since beginning of GPS week
      double lat = std::stod(elements[7]); // latitude in degrees
      double lng = std::stod(elements[8]); // longitude in degrees
      // I beleive "Alt" is altitude over ellipsoid (WGS84, standard for GPS)
      // but it could be height above geoid.
      double alt = std::stod(elements[9]); // in meters

      double Time = GMS / 1e3;

      double z = alt;
      double easting, northing;
      UTMProjector proj;
      proj.Project(lat, lng, easting, northing);
      Eigen::Vector3d  position;
      position << easting, northing, z; // ENU referential (right hand oriented)
      if (offsetFound)
      {
        position = position - offset;
      }
      else
      {
        this->SignedUTMZone = proj.SignedUTMZone;
        offset = position;
        position = Eigen::Vector3d::Zero();
        this->Offset[0] = offset[0];
        this->Offset[1] = offset[1];
        this->Offset[2] = offset[2];
        offsetFound = true;
      }

      GPSTrajectory->PushBack(Time + TimeOffset, Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()), position);
    }
  }

  auto *output = vtkPolyData::GetData(outputVector->GetInformationObject(0));
  output->ShallowCopy(this->GPSTrajectory);
  return VTK_OK;
}


/**
 *  filename : Mesh.hpp
 *  author   : Do Won Cha
 *  content  : Mesh class
 */

#pragma once
#ifndef _RAY_MESH_
#define _RAY_MESH_

#include <cstdio>
#include <cstring>
#include <string>
#include <vector>
#include <array>
#include <queue>
#include <fstream>
#include <float.h>
#include <Eigen/Core>

using namespace Eigen;

namespace raytracer
{

class KdMesh: public Surface
{
public:
	KdMesh(std::string meshfile, std::string kdfile, std::string material_name) :
		Surface(material_name)
	{
    // Load the mesh and the kd tree from file
    load_mesh(meshfile);
    load_kd_tree(kdfile);
	}

	~KdMesh()
	{

	}

	bool Intersect(const Ray& ray, HitData& hit) const
	{
		//Intersect(kd_tree_.at(0), ray, );
	}

	bool Intersect(const KdNode& node, const Ray& ray, float t, float& tmin) const
  {
		/*
    // If the node's bounding box is hit
		if (node->boundingBox.Intersect(ray))
		{
      Vector3f normal;
      bool bTriHit = false;
      Vector3f hit_point, local_hit_point;

      const KdNode& left = kd_tree_.at(node.leftChildId);
      const KdNode& right = kd_tree_.at(node.rightChildId);

      // Recursive case: if children still have triangles go deeper
      if (left.triangles_.size() > 0 || right.triangles_.size() > 0)
      {
        bool hitLeft = Intersect(left, ray, t, tmin);
        bool hitRight = Intersect(right, ray, t ,tmin);
        return hitLeft || hitRight;
      }
      else  // Leaf of kd-tree reached
      {
        // For each triangle intersect
        for (const Triangle& tri: node.triangles)
        {
          if (tri.Intersect(ray, t, tmin))
          {
            bTriHit = true;
            tmin = t;
          }
        }

        if (bTriHit)
        {

        }
      }
		}

		*/
	}

private:
  // Load the mesh from the filename
	void load_mesh(const std::string& filename)
	{
		std::ifstream fin(filename);
		if (!fin.is_open())
		{
			printf("ERROR: Unable to load mesh from %s!\n", filename);
			exit(0);
		}

		float xmin = FLT_MAX;
		float xmax = -FLT_MAX;
		float ymin = FLT_MAX;
		float ymax = -FLT_MAX;
		float zmin = FLT_MAX;
		float zmax = -FLT_MAX;

		while (true)
		{
			char line[1024] = {0};
			fin.getline(line, 1024);

			if (fin.eof())
				break;

			if (strlen(line) <= 1)
				continue;

			std::vector<std::string> tokens;
			tokenize(line, tokens, " ");

			if (tokens[0] == "v")
			{
				float x = atof(tokens[1].c_str());
				float y = atof(tokens[2].c_str());
				float z = atof(tokens[3].c_str());

				xmin = std::min(x, xmin);
				xmax = std::max(x, xmax);
				ymin = std::min(y, ymin);
				ymax = std::max(y, ymax);
				zmin = std::min(z, zmin);
				zmax = std::max(z, zmax);

				gPositions.emplace_back(x, y, z);
			}
			else if (tokens[0] == "vn")
			{
				float x = atof(tokens[1].c_str());
				float y = atof(tokens[2].c_str());
				float z = atof(tokens[3].c_str());
				Vector3 normal = {x, y, z};
				gNormals.push_back(normal);
			}
			else if (tokens[0] == "f")
			{
				unsigned int a = face_index(tokens[1].c_str());
				unsigned int b = face_index(tokens[2].c_str());
				unsigned int c = face_index(tokens[3].c_str());
				Triangle triangle;
				triangle.indices[0] = a - 1;
				triangle.indices[1] = b - 1;
				triangle.indices[2] = c - 1;
				gTriangles.push_back(triangle);
			}
		}

		fin.close();

		printf("Loaded mesh from %s. (%lu vertices, %lu normals, %lu triangles)\n", fileName.c_str(), gPositions.size(), gNormals.size(), gTriangles.size());
		printf("Mesh bounding box is: (%0.4f, %0.4f, %0.4f) to (%0.4f, %0.4f, %0.4f)\n", xmin, ymin, zmin, xmax, ymax, zmax);
	}

	bool load_kd_tree(const std::string& filename)
	{
		FILE* fp;
		char temp[256];
		fp = fopen(filename, "r+");
		if (!fp)
			return false;
		int nodeId = -1;
		while (true)
		{
			nodeId++;
			*temp = NULL;
			fscanf(fp, "%s{ ", temp);
			if (strcmp(temp, "inner{") == 0)
			{
				KdNode kd;
				kd.nodeId = nodeId;
				kd.isLeaf = false;
				fscanf(fp, "%f %f %f %f %f %f ; %d %d %d %f }", &kd.boundingBox.min.x, &kd.boundingBox.min.y, &kd.boundingBox.min.z, &kd.boundingBox.max.x, &kd.boundingBox.max.y, &kd.boundingBox.max.z, &kd.leftChildId, &kd.rightChildId, &kd.splitAxis, &kd.splitPosition);
				kdTree.push_back(kd);
			}
			else if (strcmp(temp, "leaf{") == 0)
			{
				KdNode kd;
				kd.nodeId = nodeId;
				kd.isLeaf = true;
				fscanf(fp, "%f %f %f %f %f %f ;", &kd.boundingBox.min.x, &kd.boundingBox.min.y, &kd.boundingBox.min.z, &kd.boundingBox.max.x, &kd.boundingBox.max.y, &kd.boundingBox.max.z);
				char token[256];
				while (true)
				{
					fscanf(fp, " %s", token);
					if (strcmp(token, "}") == 0)
						break;
					int triIndex = atoi(token);
					kd.triIndex.push_back(triIndex);
				}
				kdTree.push_back(kd);
			}
			else
				break;
		}
		printf("Kd file parsed succesfully.\n");
		return true;
	}

	static void tokenize(char* string, std::vector<std::string>& tokens, const char* delimiter)
	{
		char* token = strtok(string, delimiter);
		while (token != NULL)
		{
			tokens.push_back(std::string(token));
			token = strtok(NULL, delimiter);
		}
	}

	static int face_index(const char* string)
	{
		int length = strlen(string);
		char* copy = new char[length + 1];
		memset(copy, 0, length+1);
		strcpy(copy, string);

		std::vector<std::string> tokens;
		tokenize(copy, tokens, "/");
		delete[] copy;
		if (tokens.front().length() > 0 && tokens.back().length() > 0 && atoi(tokens.front().c_str()) == atoi(tokens.back().c_str()))
		{
			return atoi(tokens.front().c_str());
		}
		else
		{
			printf("ERROR: Bad face specifier!\n");
			exit(0);
		}
	}

private:
  // Bounding box
  struct AABB
  {
    Vector3f min;
    Vector3f max;

    // Bounding box & ray hit test
    bool hit(const Ray& ray)
    {
      float tx1 = (min(0) - ray.pos(0)) * ray.direction.inverse();
      float tx2 = (max(0) - ray.pos(0)) * ray.direction.inverse();

      double tmin = min(tx1, tx2);
      double tmax = max(tx1, tx2);

      double ty1 = (b.min.y - r.x0.y)*r.n_inv.y;
      double ty2 = (b.max.y - r.x0.y)*r.n_inv.y;

      tmin = max(tmin, min(ty1, ty2));
      tmax = min(tmax, max(ty1, ty2));

      return tmax >= tmin;
    }
  };

  // kd-tree nodes struct
  struct KdNode
  {
    int nodeId;
    AABB boundingBox;
    int leftChildId;
    int rightChildId;
    int splitAxis;
    float splitPosition;
    bool isLeaf;
    std::vector<int> triIndex;
  };

  std::vector<KdNode>	  kd_tree_;
  std::vector<Vector3f>	vertices_;
  std::vector<Vector3f>	vertex_normals_;
  std::vector<Triangle>	triangles_;
};

#endif // _RAY_MESH_


//  Copyright John Maddock 2007.
//  Use, modification and distribution are subject to 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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip10

#include <boost/math/special_functions/gamma.hpp>

using namespace boost::math;
using namespace boost::math::policies;

double t = tgamma(12, policy<digits10<5> >());

//]

#include <iostream>

int main()
{
   std::cout << t << std::endl;
}


//==================================================================================================
/*!
  @file

  @copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ILOGB_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ILOGB_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <cmath>
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF(ilogb_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return if_else(is_inf(a0), Valmax<result_t>(), exponent(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD(ilogb_
                         , (typename A0, typename X)
                         , bd::cpu_
                         , bs::pedantic_tag
                         , bs::pack_<bd::floating_<A0>, X>
                         )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator()(const pedantic_tag &
                                         ,  const A0& a0) const BOOST_NOEXCEPT
    {
      result_t fp_ilogbnan(FP_ILOGBNAN);
      result_t fp_ilogb0(FP_ILOGB0);
      return if_else(is_nan(a0), fp_ilogbnan,
                            if_else(is_eqz(a0), fp_ilogb0
                                   , bs::ilogb(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(ilogb_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::integer_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return bitwise_cast<A0>(ilogb(tofloat(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( ilogb_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_<bd::ints16_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0)
    {
      auto s0 = split(a0);
      return bitwise_cast<A0>(group(ilogb(s0[0]), ilogb(s0[1])));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( ilogb_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_<bd::ints8_<A0>, X>
                             )
  {
    using result = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0)
    {
      auto s0 = split(a0);
      return bitwise_cast<A0>(group(ilogb(s0[0]), ilogb(s0[1])));
    }
  };
} } }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.
//
// Use, modification and distribution is subject to 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)
//
// Projection example 5 (reworked from 4), using small factory

#include <fstream>

#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/goode.hpp>
#include <boost/geometry/extensions/gis/projections/proj/moll.hpp>
#include <boost/geometry/extensions/gis/projections/proj/natearth.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>

#include <boost/geometry/extensions/gis/projections/new_projection.hpp>

// Define a specific projection transformer
// (NOTE: this might become part of the library - copied from p04)
template <typename Projection>
struct projection_transformer
{
    Projection const& m_prj;

    inline projection_transformer(Projection const& prj)
        : m_prj(prj)
    {}

    inline bool apply(typename Projection::geographic_point_type const& p1,
                typename Projection::cartesian_point_type& p2) const
    {
        return m_prj.forward(p1, p2);
    }
};

void p05_example(int projection_id,
        std::string const& wkt_filename,
        std::string const& svg_filename)
{
    using namespace boost::geometry;
    using namespace boost::geometry::projections;

    typedef model::ll::point<degree> pll;
    typedef model::d2::point_xy<double> pxy;

    // Idea and headerfile "new_projection" submitted by Krzysztof Czainski:
    // They are useful, when:
    // - you have a small set of types of projections you'll use,
    // - you know the type of projection during it's creation, and later
    //   you want to use it through an abstract base pointer,
    // - you want to avoid the overhead of factory: generating code for
    //   creating a projection of every type you don't use, and selecting
    //   the type from a string.

    projection<pll, pxy>* prj = NULL;

    parameters pars = projections::init("+ellps=WGS84");
    switch(projection_id)
    {
        case 1 : prj = new_projection<robin_spheroid<pll, pxy> >(pars); break;
        case 2 : prj = new_projection<moll_spheroid<pll, pxy> >(pars); break;
        case 3 : prj = new_projection<goode_spheroid<pll, pxy> >(pars); break;
        case 4 : prj = new_projection<natearth_spheroid<pll, pxy> >(pars); break;
        default : return;
    }

    typedef model::multi_polygon<model::polygon<pll> > mp_ll;
    typedef model::multi_polygon<model::polygon<pxy> > mp_xy;

    std::vector<mp_ll> countries_in_ll;

    // Read polygons from WKT
    std::ifstream cpp_file(wkt_filename.c_str());
    if (! cpp_file.is_open())
    {
        throw std::string("File not found: ") + wkt_filename;
    }

    while (! cpp_file.eof() )
    {
        std::string line;
        std::getline(cpp_file, line);
        if (boost::starts_with(line, "MULTIPOLYGON"))
        {
            countries_in_ll.resize(countries_in_ll.size() + 1);
            boost::geometry::read_wkt(line, countries_in_ll.back());
        }
    }

    projection_transformer<projection<pll, pxy> > strategy(*prj);

    // Project the polygons, and at the same time get the bounding box (in xy)
    std::vector<mp_xy> countries_in_xy;
    model::box<pxy> bbox;
    assign_inverse(bbox);
    BOOST_FOREACH(mp_ll const& country_ll, countries_in_ll)
    {
        mp_xy country_xy;
        if (transform(country_ll, country_xy, strategy))
        {
            expand(bbox, return_envelope<model::box<pxy> >(country_xy));
            countries_in_xy.push_back(country_xy);
        }
    }

    // Create an SVG image
    std::ofstream svg(svg_filename.c_str());
    boost::geometry::svg_mapper<pxy> mapper(svg, 1000, 800);
    mapper.add(bbox);

    BOOST_FOREACH(mp_xy const& country, countries_in_xy)
    {
        mapper.map(country, "fill-opacity:0.6;fill:rgb(153,204,0);stroke:rgb(0,128,0);stroke-width:0.2");
    }

    delete prj;
}

int main(int argc, char** argv)
{
    // Note, file location: trunk/libs/geometry/example/data
    // update path below if necessary
    std::string const data = "../../../../example/data/world.wkt";
    try
    {
        p05_example(1, data, "p05_world_1.svg");
        p05_example(2, data, "p05_world_2.svg");
        p05_example(3, data, "p05_world_3.svg");
        p05_example(4, data, "p05_world_4.svg");
    }
    catch(std::exception const& e)
    {
        std::cerr << "Exception: " << e.what() << std::endl;
        return 1;
    }
    catch(std::string const& s)
    {
        std::cerr << "Exception: " << s << std::endl;
        return 1;
    }

    return 0;
}


#include "dynamicvoronoi/dynamicvoronoi.h"

#include <math.h>
#include <iostream>

#include <boost/smart_ptr/make_shared.hpp>

DynamicVoronoi::DynamicVoronoi() {
  sqrt2 = sqrt(2.0);
  data = NULL;
  gridMap = NULL;
  vertices = NULL;
}

DynamicVoronoi::~DynamicVoronoi() {
  if (data) {
    for (int x=0; x<sizeX; x++) delete[] data[x];
    delete[] data;
  }
  if (gridMap) {
    for (int x=0; x<sizeX; x++) delete[] gridMap[x];
    delete[] gridMap;
  }
  if (vertices) {
    for (int x=0; x<sizeX; x++) delete[] vertices[x];
    delete[] vertices;
  }
  graph.clear();
  xy_list.clear();
}

void DynamicVoronoi::setMapInfo(double resolution, double x, double y)
{
  resolution_ = resolution;
  origin_x = x;
  origin_y = y;
}

void DynamicVoronoi::initializeEmpty(int _sizeX, int _sizeY, bool initGridMap) {
  sizeX = _sizeX;
  sizeY = _sizeY;
  if (data) {
    for (int x=0; x<sizeX; x++) delete[] data[x];
    delete[] data;
  }
  data = new dataCell*[sizeX];
  for (int x=0; x<sizeX; x++) data[x] = new dataCell[sizeY];

  if (initGridMap) {
    if (gridMap) {
      for (int x=0; x<sizeX; x++) delete[] gridMap[x];
      delete[] gridMap;
    }
    gridMap = new bool*[sizeX];
    for (int x=0; x<sizeX; x++) gridMap[x] = new bool[sizeY];

    if (vertices)
    {
      for (int x=0; x<sizeX; x++) delete[] vertices[x];
      delete[] vertices;
    }
    vertices = new teb_local_planner::HcGraphVertexType*[sizeX];
    for (int x=0; x<sizeX; x++) vertices[x] = new teb_local_planner::HcGraphVertexType[sizeY];

  }

  dataCell c;
  c.dist = INFINITY;
  c.sqdist = INT_MAX;
  c.obstX = invalidObstData;
  c.obstY = invalidObstData;
  c.voronoi = free;
  c.queueing = fwNotQueued;
  c.needsRaise = false;

  for (int x=0; x<sizeX; x++)
    for (int y=0; y<sizeY; y++) data[x][y] = c;

  if (initGridMap) {
    for (int x=0; x<sizeX; x++)
      for (int y=0; y<sizeY; y++) gridMap[x][y] = 0;
  }
}

void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
  gridMap = _gridMap;
  initializeEmpty(_sizeX, _sizeY, false);

  for (int x=0; x<sizeX; x++) {
    for (int y=0; y<sizeY; y++) {
      if (gridMap[x][y]) {
        dataCell c = data[x][y];
        if (!isOccupied(x,y,c)) {

          bool isSurrounded = true;
          for (int dx=-1; dx<=1; dx++) {
            int nx = x+dx;
            if (nx<=0 || nx>=sizeX-1) continue;
            for (int dy=-1; dy<=1; dy++) {
              if (dx==0 && dy==0) continue;
              int ny = y+dy;
              if (ny<=0 || ny>=sizeY-1) continue;

              if (!gridMap[nx][ny]) {
                isSurrounded = false;
                break;
              }
            }
          }
          if (isSurrounded) {
            c.obstX = x;
            c.obstY = y;
            c.sqdist = 0;
            c.dist=0;
            c.voronoi=occupied;
            c.queueing = fwProcessed;
            data[x][y] = c;
          } else setObstacle(x,y);
        }
      }
    }
  }
}

void DynamicVoronoi::occupyCell(int x, int y) {
  gridMap[x][y] = 1;
  setObstacle(x,y);
}
void DynamicVoronoi::clearCell(int x, int y) {
  gridMap[x][y] = 0;
  removeObstacle(x,y);
}

void DynamicVoronoi::setObstacle(int x, int y) {
  dataCell c = data[x][y];
  if(isOccupied(x,y,c)) return;

  addList.push_back(INTPOINT(x,y));
  c.obstX = x;
  c.obstY = y;
  data[x][y] = c;
}

void DynamicVoronoi::removeObstacle(int x, int y) {
  dataCell c = data[x][y];
  if(isOccupied(x,y,c) == false) return;

  removeList.push_back(INTPOINT(x,y));
  c.obstX = invalidObstData;
  c.obstY  = invalidObstData;
  c.queueing = bwQueued;
  data[x][y] = c;
}

void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT> points) {

  for (unsigned int i=0; i<lastObstacles.size(); i++) {
    int x = lastObstacles[i].x;
    int y = lastObstacles[i].y;

    bool v = gridMap[x][y];
    if (v) continue;
    removeObstacle(x,y);
  }

  lastObstacles.clear();

  for (unsigned int i=0; i<points.size(); i++) {
    int x = points[i].x;
    int y = points[i].y;
    bool v = gridMap[x][y];
    if (v) continue;
    setObstacle(x,y);
    lastObstacles.push_back(points[i]);
  }
}

void DynamicVoronoi::update(bool updateRealDist) {

  commitAndColorize(updateRealDist);

  while (!open.empty()) {
    INTPOINT p = open.pop();
    int x = p.x;
    int y = p.y;
    dataCell c = data[x][y];

    if(c.queueing==fwProcessed) continue;

    if (c.needsRaise) {
      // RAISE
      for (int dx=-1; dx<=1; dx++) {
        int nx = x+dx;
        if (nx<=0 || nx>=sizeX-1) continue;
        for (int dy=-1; dy<=1; dy++) {
          if (dx==0 && dy==0) continue;
          int ny = y+dy;
          if (ny<=0 || ny>=sizeY-1) continue;
          dataCell nc = data[nx][ny];
          if (nc.obstX!=invalidObstData && !nc.needsRaise) {
            if(!isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])) {
              open.push(nc.sqdist, INTPOINT(nx,ny));
              nc.queueing = fwQueued;
              nc.needsRaise = true;
              nc.obstX = invalidObstData;
              nc.obstY = invalidObstData;
              if (updateRealDist) nc.dist = INFINITY;
              nc.sqdist = INT_MAX;
              data[nx][ny] = nc;
            } else {
              if(nc.queueing != fwQueued){
                open.push(nc.sqdist, INTPOINT(nx,ny));
                nc.queueing = fwQueued;
                data[nx][ny] = nc;
              }
            }
          }
        }
      }
      c.needsRaise = false;
      c.queueing = bwProcessed;
      data[x][y] = c;
    }
    else if (c.obstX != invalidObstData && isOccupied(c.obstX,c.obstY,data[c.obstX][c.obstY])) {

      // LOWER
      c.queueing = fwProcessed;
      c.voronoi = occupied;

      for (int dx=-1; dx<=1; dx++) {
        int nx = x+dx;
        if (nx<=0 || nx>=sizeX-1) continue;
        for (int dy=-1; dy<=1; dy++) {
          if (dx==0 && dy==0) continue;
          int ny = y+dy;
          if (ny<=0 || ny>=sizeY-1) continue;
          dataCell nc = data[nx][ny];
          if(!nc.needsRaise) {
            int distx = nx-c.obstX;
            int disty = ny-c.obstY;
            int newSqDistance = distx*distx + disty*disty;
            bool overwrite =  (newSqDistance < nc.sqdist);
            if(!overwrite && newSqDistance==nc.sqdist) {
              if (nc.obstX == invalidObstData || isOccupied(nc.obstX,nc.obstY,data[nc.obstX][nc.obstY])==false) overwrite = true;
            }
            if (overwrite) {
              open.push(newSqDistance, INTPOINT(nx,ny));
              nc.queueing = fwQueued;
              if (updateRealDist) {
                nc.dist = sqrt((double) newSqDistance);
              }
              nc.sqdist = newSqDistance;
              nc.obstX = c.obstX;
              nc.obstY = c.obstY;
            } else {
              checkVoro(x,y,nx,ny,c,nc);
            }
            data[nx][ny] = nc;
          }
        }
      }
    }
    data[x][y] = c;
  }
}

float DynamicVoronoi::getDistance( int x, int y ) {
  if( (x>0) && (x<sizeX) && (y>0) && (y<sizeY)) return data[x][y].dist;
  else return -INFINITY;
}

bool DynamicVoronoi::isVoronoi( int x, int y ) {
  dataCell c = data[x][y];
  return (c.voronoi==free || c.voronoi==voronoiKeep);
}

void DynamicVoronoi::commitAndColorize(bool updateRealDist) {
  // ADD NEW OBSTACLES
  for (unsigned int i=0; i<addList.size(); i++) {
    INTPOINT p = addList[i];
    int x = p.x;
    int y = p.y;
    dataCell c = data[x][y];

    if(c.queueing != fwQueued){
      if (updateRealDist) c.dist = 0;
      c.sqdist = 0;
      c.obstX = x;
      c.obstY = y;
      c.queueing = fwQueued;
      c.voronoi = occupied;
      data[x][y] = c;
      open.push(0, INTPOINT(x,y));
    }
  }

  // REMOVE OLD OBSTACLES
  for (unsigned int i=0; i<removeList.size(); i++) {
    INTPOINT p = removeList[i];
    int x = p.x;
    int y = p.y;
    dataCell c = data[x][y];

    if (isOccupied(x,y,c)==true) continue; // obstacle was removed and reinserted
    open.push(0, INTPOINT(x,y));
    if (updateRealDist) c.dist  = INFINITY;
    c.sqdist = INT_MAX;
    c.needsRaise = true;
    data[x][y] = c;
  }
  removeList.clear();
  addList.clear();
}

void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc) {

  if ((c.sqdist>1 || nc.sqdist>1) && nc.obstX!=invalidObstData) {
    if (abs(c.obstX-nc.obstX) > 1 || abs(c.obstY-nc.obstY) > 1) {
      //compute dist from x,y to obstacle of nx,ny
      int dxy_x = x-nc.obstX;
      int dxy_y = y-nc.obstY;
      int sqdxy = dxy_x*dxy_x + dxy_y*dxy_y;
      int stability_xy = sqdxy - c.sqdist;
      if (sqdxy - c.sqdist<0) return;

      //compute dist from nx,ny to obstacle of x,y
      int dnxy_x = nx - c.obstX;
      int dnxy_y = ny - c.obstY;
      int sqdnxy = dnxy_x*dnxy_x + dnxy_y*dnxy_y;
      int stability_nxy = sqdnxy - nc.sqdist;
      if (sqdnxy - nc.sqdist <0) return;

      //which cell is added to the Voronoi diagram?
      if(stability_xy <= stability_nxy && c.sqdist>2) {
        if (c.voronoi != free) {
          c.voronoi = free;
          reviveVoroNeighbors(x,y);
          pruneQueue.push(INTPOINT(x,y));
        }
      }
      if(stability_nxy <= stability_xy && nc.sqdist>2) {
        if (nc.voronoi != free) {
          nc.voronoi = free;
          reviveVoroNeighbors(nx,ny);
          pruneQueue.push(INTPOINT(nx,ny));
        }
      }
    }
  }
}

void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) {
  for (int dx=-1; dx<=1; dx++) {
    int nx = x+dx;
    if (nx<=0 || nx>=sizeX-1) continue;
    for (int dy=-1; dy<=1; dy++) {
      if (dx==0 && dy==0) continue;
      int ny = y+dy;
      if (ny<=0 || ny>=sizeY-1) continue;
      dataCell nc = data[nx][ny];
      if (nc.sqdist != INT_MAX && !nc.needsRaise && (nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
        nc.voronoi = free;
        data[nx][ny] = nc;
        pruneQueue.push(INTPOINT(nx,ny));
      }
    }
  }
}

bool DynamicVoronoi::isOccupied(int x, int y) {
  dataCell c = data[x][y];
  return (c.obstX==x && c.obstY==y);
}

bool DynamicVoronoi::isOccupied(int &x, int &y, dataCell &c) {
  return (c.obstX==x && c.obstY==y);
}

void DynamicVoronoi::visualize(const char *filename) {
  // write pgm files

  FILE* F = fopen(filename, "w");
  if (!F) {
    std::cerr << "could not open 'result.pgm' for writing!\n";
    return;
  }
  fprintf(F, "P6\n");
  fprintf(F, "%d %d 255\n", sizeX, sizeY);

  for(int y = sizeY-1; y >=0; y--){
    for(int x = 0; x<sizeX; x++){
      unsigned char c = 0;
      if (isVoronoi(x,y)) {
        fputc( 255, F );
        fputc( 0, F );
        fputc( 0, F );
      } else if (data[x][y].sqdist==0) {
        fputc( 0, F );
        fputc( 0, F );
        fputc( 0, F );
      } else {
        float f = 80+(data[x][y].dist*5);
        if (f>255) f=255;
        if (f<0) f=0;
        c = (unsigned char)f;
        fputc( c, F );
        fputc( c, F );
        fputc( c, F );
      }
    }
  }
  fclose(F);
}

void DynamicVoronoi::prune() {
  // filler
  while(!pruneQueue.empty()) {
    INTPOINT p = pruneQueue.front();
    pruneQueue.pop();
    int x = p.x;
    int y = p.y;

    if (data[x][y].voronoi==occupied) continue;
    if (data[x][y].voronoi==freeQueued) continue;

    data[x][y].voronoi = freeQueued;
    open.push(data[x][y].sqdist, p);

    /* tl t tr
       l c r
       bl b br */

    dataCell tr,tl,br,bl;
    tr = data[x+1][y+1];
    tl = data[x-1][y+1];
    br = data[x+1][y-1];
    bl = data[x-1][y-1];

    dataCell r,b,t,l;
    r = data[x+1][y];
    l = data[x-1][y];
    t = data[x][y+1];
    b = data[x][y-1];

    if (x+2<sizeX && r.voronoi==occupied) {
      // fill to the right
      if (tr.voronoi!=occupied && br.voronoi!=occupied && data[x+2][y].voronoi!=occupied) {
        r.voronoi = freeQueued;
        open.push(r.sqdist, INTPOINT(x+1,y));
        data[x+1][y] = r;
      }
    }
    if (x-2>=0 && l.voronoi==occupied) {
      // fill to the left
      if (tl.voronoi!=occupied && bl.voronoi!=occupied && data[x-2][y].voronoi!=occupied) {
        l.voronoi = freeQueued;
        open.push(l.sqdist, INTPOINT(x-1,y));
        data[x-1][y] = l;
      }
    }
    if (y+2<sizeY && t.voronoi==occupied) {
      // fill to the top
      if (tr.voronoi!=occupied && tl.voronoi!=occupied && data[x][y+2].voronoi!=occupied) {
        t.voronoi = freeQueued;
        open.push(t.sqdist, INTPOINT(x,y+1));
        data[x][y+1] = t;
      }
    }
    if (y-2>=0 && b.voronoi==occupied) {
      // fill to the bottom
      if (br.voronoi!=occupied && bl.voronoi!=occupied && data[x][y-2].voronoi!=occupied) {
        b.voronoi = freeQueued;
        open.push(b.sqdist, INTPOINT(x,y-1));
        data[x][y-1] = b;
      }
    }
  }

  while(!open.empty()) {
    INTPOINT p = open.pop();
    dataCell c = data[p.x][p.y];
    int v = c.voronoi;
    if (v!=freeQueued && v!=voronoiRetry) { // || v>free || v==voronoiPrune || v==voronoiKeep) {
      //      assert(v!=retry);
      continue;
    }

    markerMatchResult r = markerMatch(p.x,p.y);
    if (r==pruned) c.voronoi = voronoiPrune;
    else if (r==keep) c.voronoi = voronoiKeep;
    else { // r==retry
      c.voronoi = voronoiRetry;
      //      printf("RETRY %d %d\n", x, sizeY-1-y);
      pruneQueue.push(p);
    }
    data[p.x][p.y] = c;

    if (open.empty()) {
      while (!pruneQueue.empty()) {
        INTPOINT p = pruneQueue.front();
        pruneQueue.pop();
        open.push(data[p.x][p.y].sqdist, p);
      }
    }
  }
  //  printf("match: %d\nnomat: %d\n", matchCount, noMatchCount);
}

DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) {
  // implementation of connectivity patterns
  bool f[8];

  int nx, ny;
  int dx, dy;

  int i=0;
  int count=0;
  //  int obstacleCount=0;
  int voroCount=0;
  int voroCountFour=0;

  for (dy=1; dy>=-1; dy--) {
    ny = y+dy;
    for (dx=-1; dx<=1; dx++) {
      if (dx || dy) {
        nx = x+dx;
        dataCell nc = data[nx][ny];
        int v = nc.voronoi;
        bool b = (v<=free && v!=voronoiPrune);
        //	if (v==occupied) obstacleCount++;
        f[i] = b;
        if (b) {
          voroCount++;
          if (!(dx && dy)) voroCountFour++;
        }
        if (b && !(dx && dy) ) count++;
        //	if (v<=free && !(dx && dy)) voroCount++;
        i++;
      }
    }
  }
  if (voroCount<3 && voroCountFour==1 && (f[1] || f[3] || f[4] || f[6])) {
    //    assert(voroCount<2);
    //    if (voroCount>=2) printf("voro>2 %d %d\n", x, y);
    return keep;
  }

  // 4-connected
  if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) || (!f[7] && f[6] && f[4])) return keep;
  if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4])) return keep;

  // keep voro cells inside of blocks and retry later
  if (voroCount>=5 && voroCountFour>=3 && data[x][y].voronoi!=voronoiRetry) {
    return retry;
  }

  return pruned;
}

void DynamicVoronoi::move(int minX_s_, int minY_s_, int minX_t_, int minY_t_, int sizeX_, int sizeY_)
{

  assert(minX_s_ + sizeX_ <= sizeX);
  assert(minY_s_ + sizeY_ <= sizeY);
  assert(minX_t_ + sizeX_ <= sizeX);
  assert(minY_t_ + sizeY_ <= sizeY);

  // mem alloc
  dataCell** data_temp = new dataCell*[sizeX_];
  for (int x=0; x<sizeX_; x++) data_temp[x] = new dataCell[sizeY_];

  bool** gridMap_temp = new bool*[sizeX];
  for (int x=0; x<sizeX_; x++) gridMap_temp[x] = new bool[sizeY_];

  // assignment from source
  for (int x=0; x<sizeX_; x++)
    for (int y=0; y<sizeY_; y++) data_temp[x][y] = data[minX_s_+x][minY_s_+y];

  for (int x=0; x<sizeX_; x++)
    for (int y=0; y<sizeY_; y++) gridMap_temp[x][y] = gridMap[minX_s_+x][minY_s_+y];

  // assignment to target
  for (int x=0; x<sizeX_; x++)
    for (int y=0; y<sizeY_; y++) data[minX_t_+x][minY_t_+y] = data_temp[x][y];

  for (int x=0; x<sizeX_; x++)
    for (int y=0; y<sizeY_; y++) gridMap[minX_t_+x][minY_t_+y] = gridMap_temp[x][y];

}

void DynamicVoronoi::createGraph()
{

  graph.clear();
  xy_list.clear();

  for(int y = sizeY-1; y >=0; y--)
  {
      for(int x = 0; x<sizeX; x++)
      {
          if (isVoronoi(x,y))
          {
            teb_local_planner::HcGraphVertexType v = boost::add_vertex(graph);
            graph[v].pos.x() = x * resolution_ + origin_x;
            graph[v].pos.y() = y * resolution_ + origin_y;
            vertices[x][y] = v;

            xy_list.push_back( std::pair<int, int>(x, y) );

          }
      }
  }

  teb_local_planner::HcGraphVertexIterator it_i, end_i;
  for (boost::tie(it_i,end_i) = boost::vertices(graph); it_i!=(end_i); ++it_i)
  {
    int x = xy_list[*it_i].first;
    int y = xy_list[*it_i].second;

    int x_left = xy_list[*it_i].first-1;
    int x_right = xy_list[*it_i].first+1;
    int y_up = xy_list[*it_i].second+1;
    int y_down = xy_list[*it_i].second-1;

    bool isVoronoi_left = false;
    bool isVoronoi_right = false;
    bool isVoronoi_up = false;
    bool isVoronoi_down = false;
    bool isVoronoi_left_up = false;
    bool isVoronoi_left_down = false;
    bool isVoronoi_right_up = false;
    bool isVoronoi_right_down = false;

    if (x_left>=0)
    {
      if (isVoronoi(x_left,y))
      {
        isVoronoi_left = true;
      }
    }
    if (x_right<sizeX)
    {
      if (isVoronoi(x_right,y))
      {
        isVoronoi_right = true;
      }
    }
    if (y_up<sizeY)
    {
      if (isVoronoi(x,y_up))
      {
        isVoronoi_up = true;
      }
    }

    if (y_down>=0)
    {
      if (isVoronoi(x,y_down))
      {
        isVoronoi_down = true;
      }
    }

    if (isVoronoi_left && isVoronoi_up)
    {
      if (isVoronoi(x_left,y_up))
      {
        isVoronoi_left_up = true;
      }
    }

    if (isVoronoi_left && isVoronoi_down)
    {
      if (isVoronoi(x_left,y_down))
      {
        isVoronoi_left_down = true;
      }
    }

    if (isVoronoi_right && isVoronoi_up)
    {
      if (isVoronoi(x_right,y_up))
      {
        isVoronoi_right_up = true;
      }
    }

    if (isVoronoi_right && isVoronoi_down)
    {
      if (isVoronoi(x_right,y_down))
      {
        isVoronoi_right_down = true;
      }
    }

    if (isVoronoi_left && !isVoronoi_left_down)
    {
      boost::add_edge(*it_i, vertices[x_left][y], graph);
    }

    if (isVoronoi_right && !isVoronoi_right_down)
    {
      boost::add_edge(*it_i, vertices[x_right][y], graph);
    }

    if (isVoronoi_up && !isVoronoi_left_up)
    {
      boost::add_edge(*it_i, vertices[x][y_up], graph);
    }

    if (isVoronoi_down && !isVoronoi_left_down)
    {
      boost::add_edge(*it_i, vertices[x][y_down], graph);
    }

    if (isVoronoi_left_down)
    {
      boost::add_edge(*it_i, vertices[x_left][y_down], graph);
    }
    if (isVoronoi_right_up)
    {
      boost::add_edge(*it_i, vertices[x_right][y_up], graph);
    }

  }

}

boost::optional<std::pair<int,int>> DynamicVoronoi::spiralIsVoronoi( int q_x, int q_y){

    int X = (sizeX-1 - q_x)*2+1;
    int Y = (sizeY-1 - q_y)*2+1;

    int x,y,dx,dy;
    x = y = dx =0;
    dy = -1;
    int t = std::max(X,Y);
    int maxI = t*t;
    for(int i =0; i < maxI; i++){

        // check if is in range
        if ( q_x+x >=0 && q_x+x < sizeX && q_y+y >= 0 && q_y+y < sizeY ){
          if (isVoronoi(q_x+x, q_y+y))
          {
            return std::pair<int,int> (q_x+x, q_y+y);
          }
        }

        if( (x == y) || ((x < 0) && (x == -y)) || ((x > 0) && (x == 1-y))){
            t = dx;
            dx = -dy;
            dy = t;
        }
        x += dx;
        y += dy;
    }
  return boost::none;
}

teb_local_planner::HcGraph DynamicVoronoi::getGraph(int start_x, int start_y, int goal_x, int goal_y)
{

  teb_local_planner::HcGraphVertexType v_start = boost::add_vertex(graph);
  graph[v_start].pos.x() = start_x*resolution_+origin_x;
  graph[v_start].pos.y() = start_y*resolution_+origin_y;
  xy_list.push_back(std::pair<int, int>(start_x, start_y));

  boost::optional<std::pair<int,int>> start_near_xy = spiralIsVoronoi(start_x, start_y);

  if (start_near_xy)
  {
    boost::add_edge(v_start, vertices[(*start_near_xy).first][(*start_near_xy).second], graph);
  }

  teb_local_planner::HcGraphVertexType v_goal = boost::add_vertex(graph);
  graph[v_goal].pos.x() = goal_x*resolution_+origin_x;
  graph[v_goal].pos.y() = goal_y*resolution_+origin_y;
  xy_list.push_back(std::pair<int, int>(goal_x, goal_y));

  boost::optional<std::pair<int,int>> goal_near_xy = spiralIsVoronoi(goal_x, goal_y);

  if (goal_near_xy)
  {
    boost::add_edge(vertices[(*goal_near_xy).first][(*goal_near_xy).second], v_goal, graph);
  }

  paths.clear();
  paths_world.clear();

  int depth = getSizeX() + getSizeY();

  teb_local_planner::HcGraph temp_graph;
  if (start_near_xy && goal_near_xy)
  {
    boost::copy_graph(graph, temp_graph);
  }

  if (start_near_xy){
    boost::remove_edge(v_start, vertices[(*start_near_xy).first][(*start_near_xy).second], graph);
    remove_vertex(v_start, graph);
    xy_list.pop_back();
  }
  if (goal_near_xy)
  {
    boost::remove_edge(vertices[(*goal_near_xy).first][(*goal_near_xy).second], v_goal, graph);
    remove_vertex(v_goal, graph);
    xy_list.pop_back();
  }

  return temp_graph;

}

void DynamicVoronoi::search(int start_x, int start_y, int goal_x, int goal_y)
{

  teb_local_planner::HcGraphVertexType v_start = boost::add_vertex(graph);
  graph[v_start].pos.x() = start_x*resolution_+origin_x;
  graph[v_start].pos.y() = start_y*resolution_+origin_y;
  xy_list.push_back(std::pair<int, int>(start_x, start_y));

  boost::optional<std::pair<int,int>> start_near_xy = spiralIsVoronoi(start_x, start_y);

  if (start_near_xy)
  {
    boost::add_edge(v_start, vertices[(*start_near_xy).first][(*start_near_xy).second], graph);
  }

  teb_local_planner::HcGraphVertexType v_goal = boost::add_vertex(graph);
  graph[v_goal].pos.x() = goal_x*resolution_+origin_x;
  graph[v_goal].pos.y() = goal_y*resolution_+origin_y;
  xy_list.push_back(std::pair<int, int>(goal_x, goal_y));

  boost::optional<std::pair<int,int>> goal_near_xy = spiralIsVoronoi(goal_x, goal_y);

  if (goal_near_xy)
  {
    boost::add_edge(vertices[(*goal_near_xy).first][(*goal_near_xy).second], v_goal, graph);
  }

  paths.clear();
  paths_world.clear();

  int depth = getSizeX() + getSizeY();

  if (start_near_xy && goal_near_xy)
  {
    std::vector<teb_local_planner::HcGraphVertexType> visited;
    visited.push_back(v_start);

    bool* visited_array = new bool[num_vertices(graph)]();
    visited_array[v_start] = true;

    DepthFirst(graph,visited, visited_array, v_goal, depth);

    delete[] visited_array;
  }

  if (start_near_xy){
    boost::remove_edge(v_start, vertices[(*start_near_xy).first][(*start_near_xy).second], graph);
    remove_vertex(v_start, graph);
    xy_list.pop_back();
  }
  if (goal_near_xy)
  {
    boost::remove_edge(vertices[(*goal_near_xy).first][(*goal_near_xy).second], v_goal, graph);
    remove_vertex(v_goal, graph);
    xy_list.pop_back();
  }

}

void DynamicVoronoi::DepthFirst(teb_local_planner::HcGraph& g, std::vector<teb_local_planner::HcGraphVertexType>& visited, bool*& visited_array, const teb_local_planner::HcGraphVertexType& goal, int depth)
{
  // see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths

  //if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
  //  return; // We do not need to search for further possible alternative homotopy classes.

  teb_local_planner::HcGraphVertexType back = visited.back();

  /// Examine adjacent nodes
  teb_local_planner::HcGraphAdjecencyIterator it, end;
  for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
  {
    //if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() )
    // constant time access
    if ( visited_array[*it] )
      continue; // already visited

    if ( *it == goal ) // goal reached
    {
      visited.push_back(*it);

      // Add new TEB, if this path belongs to a new homotopy class
      //hcp_->addAndInitNewTeb(visited.begin(), visited.end(), boost::bind(getVector2dFromHcGraph, _1, boost::cref(graph_)),
      //                       start_orientation, goal_orientation, start_velocity);

      std::vector<std::pair<int,int>> path;
      std::vector<std::pair<double, double>> path_world;

      for (auto it_v = visited.begin(); it_v != visited.end(); it_v++)
      {
        path.push_back(std::pair<int,int>(xy_list[*it_v].first, xy_list[*it_v].second));
        path_world.push_back( std::pair<double, double>(graph[*it_v].pos.x(), graph[*it_v].pos.y()) );
      }

      paths.push_back(path);
      paths_world.push_back(path_world);

      visited.pop_back();
      break;
    }
  }

  if (depth > 0)
  {
    /// Recursion for all adjacent vertices
    for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
    {
      if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() || *it == goal)
        continue; // already visited || goal reached

      visited.push_back(*it);
      visited_array[*it] = true;

      // recursion step
      DepthFirst(g, visited, visited_array, goal, depth-1);

      visited.pop_back();
      visited_array[*it] = false;
    }
  }
}

std::vector<std::vector<std::pair<int, int> >> DynamicVoronoi::getPaths()
{
  return paths;
}

std::vector<std::vector<std::pair<double, double> >> DynamicVoronoi::getPaths_world()
{
  return paths_world;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#define BOOST_GEOMETRY_BUFFER_TEST_SVG_USE_ALTERNATE_BOX_FOR_INPUT
#define BOOST_GEOMETRY_BUFFER_TEST_SVG_ALTERNATE_BOX "BOX(179 4, 180 5)"

#include <test_buffer.hpp>

#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/multi/geometries/multi_geometries.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

const int point_count = 90; // points for a full circle

// Function to let buffer-distance depend on alpha, e.g.:
inline double corrected_distance(double distance, double alpha)
{
    return distance * 1.0 + 0.2 * sin(alpha * 6.0);
}

class buffer_point_strategy_sample
{
public :

    template
    <
        typename Point,
        typename OutputRange,
        typename DistanceStrategy
    >
    void apply(Point const& point,
            DistanceStrategy const& distance_strategy,
            OutputRange& output_range) const
    {
        double const distance = distance_strategy.apply(point, point,
                        bg::strategy::buffer::buffer_side_left);

        double const angle_increment = 2.0 * M_PI / double(point_count);

        double alpha = 0;
        for (std::size_t i = 0; i <= point_count; i++, alpha -= angle_increment)
        {
            double const cd = corrected_distance(distance, alpha);

            typename boost::range_value<OutputRange>::type output_point;
            bg::set<0>(output_point, bg::get<0>(point) + cd * cos(alpha));
            bg::set<1>(output_point, bg::get<1>(point) + cd * sin(alpha));
            output_range.push_back(output_point);
        }
    }
};

class buffer_join_strategy_sample
{
private :
    template
    <
        typename Point,
        typename DistanceType,
        typename RangeOut
    >
    inline void generate_points(Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        double dx1 = bg::get<0>(perp1) - bg::get<0>(vertex);
        double dy1 = bg::get<1>(perp1) - bg::get<1>(vertex);
        double dx2 = bg::get<0>(perp2) - bg::get<0>(vertex);
        double dy2 = bg::get<1>(perp2) - bg::get<1>(vertex);

        // Assuming the corner is convex, angle2 < angle1
        double const angle1 = atan2(dy1, dx1);
        double angle2 = atan2(dy2, dx2);

        while (angle2 > angle1)
        {
            angle2 -= 2 * M_PI;
        }

        double const angle_increment = 2.0 * M_PI / double(point_count);
        double alpha = angle1 - angle_increment;

        for (int i = 0; alpha >= angle2 && i < point_count; i++, alpha -= angle_increment)
        {
            double cd = corrected_distance(buffer_distance, alpha);

            Point p;
            bg::set<0>(p, bg::get<0>(vertex) + cd * cos(alpha));
            bg::set<1>(p, bg::get<1>(vertex) + cd * sin(alpha));
            range_out.push_back(p);
        }
    }

public :

    template <typename Point, typename DistanceType, typename RangeOut>
    inline bool apply(Point const& ip, Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        generate_points(vertex, perp1, perp2, buffer_distance, range_out);
        return true;
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

};

class buffer_side_sample
{
public :
    template
    <
        typename Point,
        typename OutputRange,
        typename DistanceStrategy
    >
    static inline void apply(
                Point const& input_p1, Point const& input_p2,
                bg::strategy::buffer::buffer_side_selector side,
                DistanceStrategy const& distance,
                OutputRange& output_range)
    {
        // Generate a block along (left or right of) the segment

        double const dx = bg::get<0>(input_p2) - bg::get<0>(input_p1);
        double const dy = bg::get<1>(input_p2) - bg::get<1>(input_p1);

        // For normalization [0,1] (=dot product d.d, sqrt)
        double const length = bg::math::sqrt(dx * dx + dy * dy);

        if (bg::math::equals(length, 0))
        {
            return;
        }

        // Generate the normalized perpendicular p, to the left (ccw)
        double const px = -dy / length;
        double const py = dx / length;

        // Both vectors perpendicular to input p1 and input p2 have same angle
        double const alpha = atan2(py, px);

        double const d = distance.apply(input_p1, input_p2, side);

        double const cd = corrected_distance(d, alpha);

        output_range.resize(2);

        bg::set<0>(output_range.front(), bg::get<0>(input_p1) + px * cd);
        bg::set<1>(output_range.front(), bg::get<1>(input_p1) + py * cd);
        bg::set<0>(output_range.back(), bg::get<0>(input_p2) + px * cd);
        bg::set<1>(output_range.back(), bg::get<1>(input_p2) + py * cd);
    }
};

#ifdef TEST_WITH_SVG
template <typename Geometry1, typename Geometry2>
void create_svg(std::string const& filename, Geometry1 const& original, Geometry2 const& buffer, std::string const& color)
{
    typedef typename bg::point_type<Geometry1>::type point_type;
    std::ofstream svg(filename.c_str());

    bg::svg_mapper<point_type> mapper(svg, 800, 800);
    mapper.add(buffer);

    mapper.map(original, "fill-opacity:0.3;fill:rgb(255,0,0);stroke:rgb(0,0,0);stroke-width:1");

    std::string style = "fill-opacity:0.3;fill:";
    style += color;
    style += ";stroke:rgb(0,0,0);stroke-width:1";
    mapper.map(buffer, style);
}
#endif

void test_many_rings(int imn, int jmx, int count,
    double expected_area_exterior,
    double expected_area_interior)
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point;
    typedef bg::model::polygon<point> polygon_type;
    typedef bg::model::multi_polygon<polygon_type> multi_polygon_type;

    // Predefined strategies
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(1.3);
    bg::strategy::buffer::end_flat end_strategy; // not effectively used

    // Own strategies
    buffer_join_strategy_sample join_strategy;
    buffer_point_strategy_sample point_strategy;
    buffer_side_sample side_strategy;

    // Declare output

    bg::model::multi_point<point> mp;

    // Use a bit of random disturbance in the otherwise too regular grid
    typedef boost::minstd_rand base_generator_type;
    base_generator_type generator(12345);
    boost::uniform_real<> random_range(0.0, 0.5);
    boost::variate_generator
    <
        base_generator_type&,
        boost::uniform_real<>
    > random(generator, random_range);

    for (int i = 0; i < count; i++)
    {
        for (int j = 0; j < count; j++)
        {
            double x = i * 3.0 + random();
            double y = j * 3.0 + random();
            //if (i > 30 && j < 30)
            if (i > imn && j < jmx)
            {
                point p(x, y);
                mp.push_back(p);
            }
        }
    }

    multi_polygon_type many_rings;
    // Create the buffer of a multi-point
    bg::buffer(mp, many_rings,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, point_strategy);

    bg::model::box<point> envelope;
    bg::envelope(many_rings, envelope);
    bg::buffer(envelope, envelope, 1.0);

    multi_polygon_type many_interiors;
    bg::difference(envelope, many_rings, many_interiors);

#ifdef TEST_WITH_SVG
    create_svg("/tmp/many_interiors.svg", mp, many_interiors, "rgb(51,51,153)");
    create_svg("/tmp/buffer.svg", mp, many_rings, "rgb(51,51,153)");
#endif

    bg::strategy::buffer::join_round join_round(100);
    bg::strategy::buffer::end_flat end_flat;

    {
        std::ostringstream out;
        out << "many_rings_" << count;
        out << "_" << imn << "_" << jmx;
        std::ostringstream wkt;
        wkt << std::setprecision(12) << bg::wkt(many_rings);

        boost::timer t;
        test_one<multi_polygon_type, polygon_type>(out.str(), wkt.str(), join_round, end_flat, expected_area_exterior, 0.3);
        std::cout << "Exterior " << count << " " << t.elapsed() << std::endl;
    }

    return;
    {
        std::ostringstream out;
        out << "many_interiors_" << count;
        std::ostringstream wkt;
        wkt << std::setprecision(12) << bg::wkt(many_interiors);

        boost::timer t;
        test_one<multi_polygon_type, polygon_type>(out.str(), wkt.str(), join_round, end_flat, expected_area_interior, 0.3);
        std::cout << "Interior " << count << " " << t.elapsed() << std::endl;
    }
}

int test_main(int, char* [])
{
//    test_many_rings(10, 795.70334, 806.7609);
//    test_many_rings(30, 7136.7098, 6174.4496);
      test_many_rings(30, 30, 70, 38764.2721, 31910.3280);
//    for (int i = 30; i < 60; i++)
//    {
//        for (int j = 5; j <= 30; j++)
//        {
//            test_many_rings(i, j, 70, 38764.2721, 31910.3280);
//        }
//    }
    return 0;
}


/**
 * Contains the implementation of the protanope conversion.
 *
 * Copyright 2016 Jacob Voytko (jakevoytko@gmail.com)
 */

#include "proto.hpp"

#include <math.h>
#include <algorithm>

#include <boost/scoped_ptr.hpp>

#include "color.hpp"

using video_colorblind::color::Rgb;
using video_colorblind::color::Xyy;
using video_colorblind::color::Xyz;

/** The protanope confusion point in the xyY spectrum. */
Xyy xyyConfusionPoint = Xyy(0.747, 0.253, 1.0);

/** Stores the coefficients of a quadratic polynomial. */
struct QuadraticPolynomial {
  QuadraticPolynomial(double xSquare_, double x_, double c_):
      xSquare(xSquare_), x(x_), c(c_) {}

  const double xSquare;
  const double x;
  const double c;
};

/** Calculates a step of the LaGrange interpolation method. */
QuadraticPolynomial lagrangeStepXyy(
    const Xyy &xyy0, const Xyy &xyy1, const Xyy &xyy2) {
  const double xSquare = xyy0.y / ((xyy0.x - xyy1.x) * (xyy0.x - xyy2.x));
  const double x = xSquare * -xyy1.x + xSquare * -xyy2.x;
  const double c = xSquare * xyy1.x * xyy2.x;
  return QuadraticPolynomial(xSquare, x, c);
}

/** Interpolates through the three points given using Lagrange interpolation. */
QuadraticPolynomial lagrangeInterpolateXyy(
    const Xyy &xyy0, const Xyy &xyy1, const Xyy &xyy2) {
  QuadraticPolynomial result0 = lagrangeStepXyy(xyy0, xyy1, xyy2);
  QuadraticPolynomial result1 = lagrangeStepXyy(xyy1, xyy0, xyy2);
  QuadraticPolynomial result2 = lagrangeStepXyy(xyy2, xyy0, xyy1);

  return QuadraticPolynomial(
    result0.xSquare + result1.xSquare + result2.xSquare,
    result0.x + result1.x + result2.x,
    result0.c + result1.c + result2.c);
}

/**
 * Protanopes can see wavelengths 470 and 575 correctly, as well as white. Doing
 * polynomial interpolation between these points produces a polynomial that can
 * be used to estimate all colors that protanopes can see.
 */
QuadraticPolynomial xyyVisionCurve = lagrangeInterpolateXyy(
  XYY_470, XYY_575, XYY_WHITE_D50);

/** Represents a line. */
struct Line {
  Line(double m_, double b_): m(m_), b(b_) {}

  const double m;
  const double b;
};

/** Calculates a line between the two given xyY coordinates. */
Line xyyLine(const Xyy &xyy0, const Xyy &xyy1) {
  // The confusion point for protanopes is outside the sRGB colorspace, so this
  // can't divide-by-0.

  if (xyy0.x > xyy1.x) {
    return xyyLine(xyy1, xyy0);
  }
  double slope = (xyy1.y - xyy0.y) / (xyy1.x - xyy0.x);
  return Line(slope, xyy0.y - slope * xyy0.x);
}

/**
 * Protans have a dark response curve at the red end of the spectrum. Some reds
 * are perceived at ~1/10 of the intensity that a normal observer would
 * see. Adjust for this.
 */
double getProtanLuminance(const Xyz &xyz) {
  return -0.460 * xyz.x + 1.359 * xyz.y + 0.101 * xyz.z;
}

/**
 * Calculates the intersection between a quadratic and a line using the
 * ::drumroll:: quadratic formula!
 */
Xyy intersectCurveLineXyy(
    const QuadraticPolynomial &poly, const Line &line, double Y) {
  double A = poly.xSquare;
  double B = poly.x - line.m;
  double C = poly.c - line.b;
  double discriminant = B*B - 4.0 * A * C;

  // Only the smaller X root falls in the color space.
  assert(discriminant >= 0);

  double x = (-B + sqrt(discriminant)) / (2.0 * A);

  return Xyy(x, line.m * x + line.b, Y);
}

/**
 * If the xyy color falls outside the triangle defined by the sRGB primaries,
 * does a best-effort to move it back within the sRGB triangle by intersecting
 * the confusion line with the sRGB triangle.
 */
Xyy moveWithinRgb(const Line &line, const Xyy &xyy) {
  // Two cases for protanopes: line intersects Blue->Green, line intersects
  // Red->Green.
  Line primaryLine = xyy.x < XYY_GREEN_PRIMARY.x ?
        xyyLine(XYY_BLUE_PRIMARY, XYY_GREEN_PRIMARY) :
        xyyLine(XYY_GREEN_PRIMARY, XYY_RED_PRIMARY);

  double y = primaryLine.m * xyy.x + primaryLine.b;
  if (xyy.y > y) {
    double newX = (primaryLine.b - line.b) / (line.m - primaryLine.m);
    double newY = primaryLine.m * newX + primaryLine.b;
    return Xyy(newX, newY, xyy.Y);
  }

  return xyy;
}

/** Returns x if within min or max, otherwise the violated bound. */
double clamp(double min, double x, double max) {
  return std::max(min, std::min(x, max));
}

Rgb video_colorblind::color::getProtoColor(const Rgb &inputRgb) {
  Xyz xyz = rgbToXyz(inputRgb);
  Xyy xyy = xyzToXyy(xyz);

  // According to
  // http://nvlpubs.nist.gov/nistpubs/jres/33/jresv33n6p407_A1b.pdf, luminance
  // for protans must be adjusted because of the weak red frequency response.
  double protanLuminance = getProtanLuminance(xyz);

  // First, find the confusion line. All colors along this line are perceived as
  // identical to protanopes.
  Line confusionLine = xyyLine(xyyConfusionPoint, xyy);

  // The intersection between the vision curve and the confusion line is an
  // estimation of the color a protanope actually sees.
  Xyy xyyIntersection = intersectCurveLineXyy(
    xyyVisionCurve, confusionLine, protanLuminance);

  // The color may have fallen outside the sRGB colorspace. If so, move it back
  // along the confusion line. Note: This can still convert to colors outside of
  // the display sRGB colorspace.
  Xyy boundedXyy = moveWithinRgb(confusionLine, xyyIntersection);

  // Convert back to RGB.
  Rgb returnRgb = xyzToRgb(xyyToXyz(boundedXyy));

  // Unclear what to do here besides clamp. If this is outside [0-255], then
  // it's the case where there was no intersection between the confusion line
  // and the sRGB colorspace.
  return Rgb(
    clamp(0, round(returnRgb.r), 255),
    clamp(0, round(returnRgb.g), 255),
    clamp(0, round(returnRgb.b), 255));
}