/**
 * PathSetThreadPool.cpp
 *
 *  Created on: Feb 18, 2014
 *      Author: redheli
 */

#include "PathSetThreadPool.hpp"
#include "Path.hpp"
#include "geospatial/streetdir/A_StarShortestTravelTimePathImpl.hpp"
#include "geospatial/streetdir/A_StarShortestPathImpl.hpp"
#include "logging/Log.hpp"

#include <pthread.h>
#include <semaphore.h>
#include <cstdlib>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <iterator>
#include <algorithm>
#include <vector>

using namespace std;

namespace
{
    //sim_mob::BasicLogger & logger = sim_mob::Logger::log("pathset.log");
}

sim_mob::PathSetWorkerThread::PathSetWorkerThread() :
        path(NULL), linkLookup(NULL), fromNode(NULL), toNode(NULL), hasPath(false), timeBased(false), dbgStr(std::string()), graph(NULL)
{
}

sim_mob::PathSetWorkerThread::~PathSetWorkerThread() { }

//1.Create Blacklist
//2.clear the shortestWayPointpath
//3.Populate a vector<WayPoint> with a blacklist involved
//  or
//  Populate a vector<WayPoint> without blacklist involvement
//4.populate a singlepath instance
void sim_mob::PathSetWorkerThread::run()
{
    //Convert the blacklist into a list of blocked Vertices.
    std::set<StreetDirectory::Edge> blacklistEdges;
    std::map<const Link*, std::set<StreetDirectory::Edge> >::const_iterator lookIt;
    for (std::set<const Link*>::iterator it = excludedLinks.begin(); it != excludedLinks.end(); it++)
    {
        lookIt = linkLookup->find(*it);
        if (lookIt != linkLookup->end())
        {
            blacklistEdges.insert(lookIt->second.begin(), lookIt->second.end());
        }
    }

    //used for error checking and validation
    std::pair<StreetDirectory::Edge, bool> dbgPrevEdge;
    //output container
    vector<WayPoint> wps;

    if (blacklistEdges.empty())
    {
        if(timeBased)
        {
            std::list<StreetDirectory::Vertex> partialRes;
            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            vector<StreetDirectory::Vertex> p(boost::num_vertices(*graph)); //Output variable
            vector<double> d(boost::num_vertices(*graph)); //Output variable
            try
            {
                boost::astar_search(*graph, fromVertex, sim_mob::A_StarShortestTravelTimePathImpl::DistanceHeuristicGraph(graph, toVertex),
                                    boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestTravelTimePathImpl::GoalVisitor(toVertex)));
            }
            catch (sim_mob::A_StarShortestTravelTimePathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, *graph);
                        if (!edge.second)
                        {
                            Warn() << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, *graph, edge.first);
                        wps.push_back(wp);
                    }
                    //Save for later.
                    prev = it;
                }
            }
        }
        else
        {
            std::list<StreetDirectory::Vertex> partialRes;
            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            vector<StreetDirectory::Vertex> p(boost::num_vertices(*graph)); //Output variable
            vector<double> d(boost::num_vertices(*graph)); //Output variable
            try
            {
                boost::astar_search(*graph, fromVertex, sim_mob::A_StarShortestPathImpl::DistanceHeuristicGraph(graph, toVertex),
                                    boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestPathImpl::GoalVisitor(toVertex)));
            }
            catch (sim_mob::A_StarShortestPathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, *graph);
                        if (!edge.second)
                        {
                            Warn() << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, *graph, edge.first);
                        wps.push_back(wp);
                    }
                    //Save for later.
                    prev = it;
                }
            }
        }
    }
    else
    {
        if (timeBased)
        {
            //logger << "Blacklist NOT empty" << blacklistV.size() << std::endl;
            //Filter it.

            sim_mob::A_StarShortestTravelTimePathImpl::BlackListEdgeConstraint filter(blacklistEdges);
            boost::filtered_graph<StreetDirectory::Graph, sim_mob::A_StarShortestTravelTimePathImpl::BlackListEdgeConstraint> filtered(*graph, filter);
            ////////////////////////////////////////
            // TODO: This code is copied (since filtered_graph is not the same as adjacency_list) from searchShortestPath.
            ////////////////////////////////////////
            std::list<StreetDirectory::Vertex> partialRes;

            vector<StreetDirectory::Vertex> p(boost::num_vertices(filtered)); //Output variable
            vector<double> d(boost::num_vertices(filtered)); //Output variable

            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            //...which is available under the terms of the Boost Software License, 1.0
            try
            {
                if(!p.empty() && !d.empty())
                {
                    boost::astar_search(filtered, fromVertex, sim_mob::A_StarShortestTravelTimePathImpl::DistanceHeuristicFiltered(&filtered, toVertex),
                                        boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestTravelTimePathImpl::GoalVisitor(toVertex)));
                }
            }
            catch (sim_mob::A_StarShortestTravelTimePathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, filtered);
                        if (!edge.second)
                        {
                            std::cerr << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, filtered, edge.first);
                        wps.push_back(wp);
                    }

                    //Save for later.
                    prev = it;
                }
            } //catch
        }
        else
        {
            //logger << "Blacklist NOT empty" << blacklistV.size() << std::endl;
            //Filter it.
            sim_mob::A_StarShortestPathImpl::BlackListEdgeConstraint filter(blacklistEdges);
            boost::filtered_graph<StreetDirectory::Graph, sim_mob::A_StarShortestPathImpl::BlackListEdgeConstraint> filtered(*graph, filter);
            ////////////////////////////////////////
            // TODO: This code is copied (since filtered_graph is not the same as adjacency_list) from searchShortestPath.
            ////////////////////////////////////////
            std::list<StreetDirectory::Vertex> partialRes;

            vector<StreetDirectory::Vertex> p(boost::num_vertices(filtered)); //Output variable
            vector<double> d(boost::num_vertices(filtered)); //Output variable

            //Use A* to search for a path
            //Taken from: http://www.boost.org/doc/libs/1_38_0/libs/graph/example/astar-cities.cpp
            //...which is available under the terms of the Boost Software License, 1.0
            try
            {
                if(!p.empty() && !d.empty())
                {
                    boost::astar_search(filtered, fromVertex, sim_mob::A_StarShortestPathImpl::DistanceHeuristicFiltered(&filtered, toVertex),
                                        boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(sim_mob::A_StarShortestPathImpl::GoalVisitor(toVertex)));
                }
            }
            catch (sim_mob::A_StarShortestPathImpl::Goal& goal)
            {
                //Build backwards.
                for (StreetDirectory::Vertex v = toVertex;; v = p[v])
                {
                    partialRes.push_front(v);
                    if (p[v] == v)
                    {
                        break;
                    }
                }
                //Now build forwards.
                std::list<StreetDirectory::Vertex>::const_iterator prev = partialRes.end();
                for (std::list<StreetDirectory::Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++)
                {
                    //Add this edge.
                    if (prev != partialRes.end())
                    {
                        //This shouldn't fail.
                        std::pair < StreetDirectory::Edge, bool> edge = boost::edge(*prev, *it, filtered);
                        if (!edge.second)
                        {
                            std::cerr << "ERROR: Boost can't find an edge that it should know about." << std::endl;
                        }
                        //Retrieve, add this edge's WayPoint.
                        WayPoint wp = boost::get(boost::edge_name, filtered, edge.first);
                        wps.push_back(wp);
                    }

                    //Save for later.
                    prev = it;
                }
            }
        }
    }

    if (wps.empty())
    {
        hasPath = false;
    }
    else
    {
        // make sp id
        std::string id = sim_mob::makePathString(wps);
        if (id.empty())
        {
            hasPath = false;
        }
        else
        {
            path = new sim_mob::SinglePath();
            // fill data
            path->isNeedSave2DB = true;
            hasPath = true;
            path->pathSetId = pathSet->id;
            path->scenario = pathSet->scenario + dbgStr;
            path->init(wps);
            path->id = id;
            path->pathSize = 0;
            if (this->path->path.begin()->link->getFromNodeId() != this->pathSet->subTrip.origin.node->getNodeId())
            {
                safe_delete_item(path);
                hasPath = false;
            }
        }
    }
}


//#define CGAL_CHECK_EXPENSIVE
//#define CGAL_CHECK_EXACTNESS
#define NDEBUG
//#define CGAL_NO_STATIC_FILTERS
//#define MOVE_ALL

#include <CGAL/basic.h>
#include <CGAL/Polynomial/CORE_kernel.h>

#include <CGAL/Random.h>

#include <CGAL/Updatable_Delaunay_triangulation_2.h>
#include <CGAL/Timer.h>

#include <CGAL/Triangulation_hierarchy_2.h>
#include <CGAL/Triangulation_hierarchy_vertex_base_2.h>
#include <algorithm>

#ifdef CGAL_USE_BOOST_PROGRAM_OPTIONS
#include <boost/program_options.hpp>
#endif

//extern void moncontrol(int);

//#include <cstdio>

template <class TDS>
struct MTDSH
{
  typedef typename TDS::Edge Edge;
  typedef typename TDS::Vertex_handle Vertex_handle;
  typedef typename TDS::Face_handle Face_handle;

  static Edge mirror_edge(const Edge &e) {
    int i= e.first->neighbor(e.second)->index(e.first);
    return Edge(e.first->neighbor(e.second), i);
  }

  static Vertex_handle origin(const Edge &e) {
    int o= e.first->ccw(e.second);
    return e.first->vertex(o);
  }

  static Vertex_handle destination(const Edge &e) {
    int o= e.first->cw(e.second);
    return e.first->vertex(o);
  }

  static Vertex_handle third_vertex(const Edge &e) {
    return e.first->vertex(e.second);
  }
  static Vertex_handle mirror_vertex(const Edge &e) {
    return third_vertex(mirror_edge(e));
  }

  struct Unoriented_edge {
    Unoriented_edge(Face_handle f, int i) {
      if (&*f > &*f->neighbor(i)) {
	e_= mirror_edge(Edge(f,i));
      } else {
	e_ = Edge(f,i);
      }

    }
    const Edge& edge() const {
      return e_;
    }

    bool operator<(const Unoriented_edge &o) const {
      return &*e_.first < &*o.e_.first || &*e_.first == &*o.e_.first && e_.second < o.e_.second;
    }
    Edge e_;
  };
};

template <class Del>
static bool is_hull_edge(const typename Del::Edge &e) {
  typedef MTDSH <typename Del::Triangulation_data_structure> TDS_helper;
  return ! TDS_helper::mirror_vertex(e)->point().is_valid()
    || ! TDS_helper::third_vertex(e)->point().is_valid()
    || ! TDS_helper::origin(e)->point().is_valid()
    || ! TDS_helper::destination(e)->point().is_valid();
}

template <class Del, class Indirect_kernel>
static bool compute_ok(const typename Del::Edge &e,  Indirect_kernel sk) {
  typedef MTDSH<typename Del::Triangulation_data_structure>  TDS_helper;
  //typename Indirect_kernel::Current_coordinates
  //cc= sk.current_coordinates_object();
  typename Indirect_kernel::Point_2 ks[4];
  ks[0]= TDS_helper::origin(e)->point();
  ks[1]= TDS_helper::third_vertex(e)->point();
  ks[2]= TDS_helper::destination(e)->point();
  ks[3]= TDS_helper::mirror_vertex(e)->point();

  if (ks[0] == typename Indirect_kernel::Point_2() || ks[1]== typename Indirect_kernel::Point_2()
      || ks[2] == typename Indirect_kernel::Point_2() || ks[3] == typename Indirect_kernel::Point_2()){
    typename Indirect_kernel::Orientation_2 o2= sk.orientation_2_object();

    bool odd_parity=false;
    bool infinity=false;
    for (unsigned int i=0; i<4; ++i) {
      if (infinity) {
	ks[i-1]=ks[i];
      } else {
	if (ks[i] == typename Indirect_kernel::Point_2()) {
	  infinity=true;
	  odd_parity= ((i%2)==1);
	}
      }
    }
    if (odd_parity) {
      std::swap(ks[0], ks[1]);
    }
    CGAL::Orientation o=o2(ks[0], ks[1], ks[2]);
    return o==CGAL::POSITIVE;
  } else {
    typename Indirect_kernel::Side_of_oriented_circle_2 soc
      = sk.side_of_oriented_circle_2_object();

    CGAL::Oriented_side s=soc(ks[0], ks[1], ks[2], ks[3]);

    if (s== CGAL::ON_ORIENTED_BOUNDARY) {
      std::cout << "Degeneracy with edge "
		<< ks[0] << " " << ks[2] << std::endl;
    }
    return s!= CGAL::ON_NEGATIVE_SIDE;
  }
}

template <class CNT>
CNT  incircle(CNT ax, CNT ay, CNT bx, CNT by,
	      CNT cx, CNT cy, CNT dx, CNT dy) {
  CNT qpx = bx - ax;
  CNT qpy = by - ay;
  CNT rpx = cx - ax;
  CNT rpy = cy - ay;
  CNT tpx = dx - ax;
  CNT tpy = dy - ay;
  CNT det=CGAL::determinant(qpx*tpy - qpy*tpx, tpx*(dx - bx)
				  + tpy*(dy - by),
				  qpx*rpy - qpy*rpx, rpx*(cx - bx)
				  + rpy*(cy - by));
  return det;
}

template <class Indirect_kernel, class INT>
void ival(typename Indirect_kernel::Point_2 p, typename  Indirect_kernel::Current_coordinates ic,
	  typename Indirect_kernel::Current_coordinates fc, INT rp[2]){
  INT ix= ic(p).x();
  INT iy= ic(p).y();
  INT fx= fc(p).x();
  INT fy= fc(p).y();
  rp[0]= INT(std::min(ix.inf(), fx.inf()), std::max(ix.sup(), fx.sup()));
  rp[1]= INT(std::min(iy.inf(), fy.inf()), std::max(iy.sup(), fy.sup()));
}

template <class Indirect_kernel, class INT>
void ival(typename Indirect_kernel::Point_2 p,
	  typename Indirect_kernel::Point_2 pb, typename  Indirect_kernel::Current_coordinates ic,
	  typename Indirect_kernel::Current_coordinates fc, INT rp[2]){
  INT ix= ic(p).x();
  INT iy= ic(p).y();
  INT fx= fc(p).x();
  INT fy= fc(p).y();

  INT ixb= ic(pb).x();
  INT iyb= ic(pb).y();
  INT fxb= fc(pb).x();
  INT fyb= fc(pb).y();

  rp[0]= INT(std::min((ix-ixb).inf(), (fx-fxb).inf()), std::max((ix-ixb).sup(), (fx-fxb).sup()));
  rp[1]= INT(std::min((iy-iyb).inf(), (fy-fyb).inf()), std::max((iy-iyb).sup(), (fy-fyb).sup()));
}

template <class Del, class Indirect_kernel>
static bool compute_ok_int(const typename Del::Edge &e,  Indirect_kernel ik, Indirect_kernel fk) {
  typedef MTDSH<typename Del::Triangulation_data_structure>  TDS_helper;
  //typename Indirect_kernel::Current_coordinates
  //cc= sk.current_coordinates_object();
  typename Indirect_kernel::Point_2 ks[4];
  ks[0]= TDS_helper::origin(e)->point();
  ks[1]= TDS_helper::third_vertex(e)->point();
  ks[2]= TDS_helper::destination(e)->point();
  ks[3]= TDS_helper::mirror_vertex(e)->point();

  if (ks[0] == typename Indirect_kernel::Point_2() || ks[1]== typename Indirect_kernel::Point_2()
      || ks[2] == typename Indirect_kernel::Point_2() || ks[3] == typename Indirect_kernel::Point_2()){
    return true;
  } else {
    typename Indirect_kernel::Current_coordinates ic= ik.current_coordinates_object();
    typename Indirect_kernel::Current_coordinates fc= fk.current_coordinates_object();
    typedef CGAL::Interval_nt_advanced INT;
    INT a[2], b[2], c[2], d[2];
    ival<Indirect_kernel>(ks[0], ic, fc, a);
    ival<Indirect_kernel>(ks[1], ic, fc, b);
    ival<Indirect_kernel>(ks[2], ic, fc, c);
    ival<Indirect_kernel>(ks[3], ic, fc, d);
    INT v= incircle(a[0], a[1], b[0], b[1], c[0], c[1], d[0], d[1]);
    return v.inf() >0;
  }
}

template <class Del, class Indirect_kernel>
static bool compute_ok_intp(const typename Del::Edge &e,  Indirect_kernel ik, Indirect_kernel fk) {
  typedef MTDSH<typename Del::Triangulation_data_structure>  TDS_helper;
  //typename Indirect_kernel::Current_coordinates
  //cc= sk.current_coordinates_object();
  typename Indirect_kernel::Point_2 ks[4];
  ks[0]= TDS_helper::origin(e)->point();
  ks[1]= TDS_helper::third_vertex(e)->point();
  ks[2]= TDS_helper::destination(e)->point();
  ks[3]= TDS_helper::mirror_vertex(e)->point();

  if (ks[0] == typename Indirect_kernel::Point_2() || ks[1]== typename Indirect_kernel::Point_2()
      || ks[2] == typename Indirect_kernel::Point_2() || ks[3] == typename Indirect_kernel::Point_2()){
    return true;
  } else {
    typename Indirect_kernel::Current_coordinates ic= ik.current_coordinates_object();
    typename Indirect_kernel::Current_coordinates fc= fk.current_coordinates_object();
    typedef CGAL::Interval_nt_advanced INT;
    INT b[2], c[2], d[2];
    //ival<Indirect_kernel>(ks[0], ic, fc, a);
    ival<Indirect_kernel>(ks[1], ks[0], ic, fc, b);
    ival<Indirect_kernel>(ks[2], ks[0], ic, fc, c);
    ival<Indirect_kernel>(ks[3], ks[0], ic, fc, d);
    INT v= incircle(INT(0.0), INT(0.0), b[0], b[1], c[0], c[1], d[0], d[1]);
    return v.inf() >0;
  }
}

template <class Points>
void read(std::string name, Points &points) {
    std::ifstream in(name.c_str());
    if (!in) {
      std::cerr << "Error opening file " << name  << std::endl;
      exit(1);
    }

    while (true) {
      char ln[10000];
      in.getline(ln, 10000);
      if (!in) {
	break;
      }
      typename Points::value_type p;
      std::istringstream iss(ln);
      iss >> p;
      if (!iss) {
	CGAL::Simple_cartesian<double>::Point_2 dpt;
	std::istringstream iss2(ln);
	iss2 >> dpt;
	if (!iss2) {
	  std::cerr << "Error processing line " << ln << std::endl;
	} else {
	  points.push_back(typename Points::value_type(dpt.x(), dpt.y()));
	}
      } else {

	points.push_back(p);
      }
    };
  }

template <class Tri, class IK>
int hybrid(Tri &del, IK, IK fk) {
  typedef MTDSH<typename Tri::Triangulation_data_structure>  TDSH;
  typedef typename Tri::Edge Edge;

  int bc=0;
  typename IK::Orientation_2 o2= fk.orientation_2_object();
  for (typename Tri::Finite_faces_iterator fit = del.finite_faces_begin(); fit != del.finite_faces_end(); ++fit){
    if (o2(fit->vertex(0)->point(), fit->vertex(1)->point(), fit->vertex(2)->point()) != CGAL::POSITIVE) {
      ++bc;
    }
  }
  if (bc != 0) {
    std::cout << "bc is not zero " << std::endl;
  }
  typename std::set<typename TDSH::Unoriented_edge> bad;
  //typename IK::Side_of_oriented_circle_2 c2= fk.side_of_oriented_circle_2_object();
  for (typename Tri::Finite_edges_iterator fit = del.finite_edges_begin(); fit != del.finite_edges_end(); ++fit){
    if (!compute_ok<Tri, IK>(*fit, fk)) {
      bad.insert(typename TDSH::Unoriented_edge(fit->first, fit->second));
    }
  }

  int nflips= 0;
  while (!bad.empty()) {
    Edge e= bad.begin()->edge();
    bad.erase(bad.begin());
    Edge me= TDSH::mirror_edge(e);
    ++nflips;
    for (int i=0; i< 3; ++i) {
      if (i != e.second) {
	bad.erase(typename TDSH::Unoriented_edge(e.first, i));
      }
      if (i != me.second) {
	bad.erase(typename TDSH::Unoriented_edge(me.first, i));
      }
    }

    del.flip(e.first, e.second);

    for (int i=0; i< 3; ++i) {
      if (i != e.second) {
	if (!compute_ok<Tri, IK>(Edge(e.first, i), fk)) {
	  bad.insert(typename TDSH::Unoriented_edge(e.first, i));
	}
      }
      if (i != me.second) {
	if (!compute_ok<Tri, IK>(Edge(me.first, i), fk)) {
	  bad.insert(typename TDSH::Unoriented_edge(me.first, i));
	}
      }
    }
  }
  //del.geom_traits().swap(fk);
  return nflips;
}

int main(int argc, char *argv[]) {

  //moncontrol(0);

  bool print_help=false;
  bool speedup_only=false;
  bool profile=true;
  std::string ifile="../../demo/Kinetic_data_structures/data/before106",
    ffile="../../demo/Kinetic_data_structures/data/after106";
#ifdef CGAL_USE_BOOST_PROGRAM_OPTIONS
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
    ("help", boost::program_options::bool_switch(&print_help), "produce help message")
    ("speedup-only,s", boost::program_options::bool_switch(&speedup_only), "Only show the speedup and the fraction failed")
    ("disable-0,0", boost::program_options::bool_switch(&CGAL::disable_filter_0_), "Disable filter 0")
    ("disable-1,1", boost::program_options::bool_switch(&CGAL::disable_filter_1_), "Disable filter 1")
    ("disable-2,2", boost::program_options::bool_switch(&CGAL::disable_filter_2_), "Disable filter 2")
    ("disable-3,3", boost::program_options::bool_switch(&CGAL::disable_filter_3_), "Disable filter 3")

    ("profile,p", boost::program_options::bool_switch(&profile), "Just update for profiling")

    ("ifile,i", boost::program_options::value<std::string>(&ifile), "The inital coordinates.")
    ("ffile,f", boost::program_options::value<std::string>(&ffile), "The final coordinates.")
    ;

  boost::program_options::variables_map vm;
  boost::program_options::store(boost::program_options::command_line_parser(argc, argv).
				options(desc).run(), vm);
  boost::program_options::notify(vm);

  if (print_help) {
    std::cout << desc << "\n";
    return EXIT_FAILURE;
  }
#endif

  //if (exact) {
  typedef CGAL::Exact_predicates_inexact_constructions_kernel  CK;
  typedef CGAL::Indirect_point_2_kernel<CGAL::Exact_predicates_inexact_constructions_kernel>  K;
  typedef CGAL::Updatable_Delaunay_triangulation_2<K> UD;
  typedef CGAL::Delaunay_triangulation_2<K> Del;
  typedef CGAL::Delaunay_triangulation_2<CK> CDel;

  typedef CGAL::Triangulation_vertex_base_2<K>             HVbb;
  typedef CGAL::Triangulation_hierarchy_vertex_base_2<HVbb> HVb;
  typedef CGAL::Triangulation_face_base_2<K>               HFb;
  typedef CGAL::Triangulation_data_structure_2<HVb,HFb>      HTds;
  typedef CGAL::Delaunay_triangulation_2<K,HTds>            HDt;
  typedef CGAL::Triangulation_hierarchy_2<HDt>              H;

  typedef UD::Points Points;
  Points ipts, fpts;
  read(ifile, ipts);
  read(ffile, fpts);

  {
    K ik, fk;
    K::Key_range ir= ik.new_point_2s(ipts.begin(), ipts.end());
    K::Key_range fr= fk.new_point_2s(fpts.begin(), fpts.end());

    Del d(ik);
    d.insert(ir.first, ir.second);
    int bad_count=0;
    int filter_1_failure=0;
    int filter_1p_failure=0;
    for (Del::Edge_iterator it= d.edges_begin();
	 it != d.edges_end(); ++it){
      if (!compute_ok<Del>(*it, fk)) {
	++bad_count;
      }
      if (!compute_ok_int<Del>(*it, ik, fk)) {
	++filter_1_failure;
      }
      if (!compute_ok_intp<Del>(*it, ik, fk)) {
	++filter_1p_failure;
      }
    }
    if (!speedup_only) {
      std::cout << "For " << std::distance(d.edges_begin(), d.edges_end()) << " edges "
		<< " there were " << bad_count << " failed certs "
		<< "and " <<  filter_1_failure << " edges which failed on 1"
		<< " and " << filter_1p_failure << " which failed on the improved 1" << std::endl;
    } else {
      std::cout << static_cast<double>(bad_count)/std::distance(d.edges_begin(), d.edges_end()) << " ";
    }
  }
  int update_speed;
  int activated=0;
  {
    UD ud(ipts.begin(), ipts.end());

    // moncontrol(1);

    if (!speedup_only) std::cout << "Timing patching..." << std::flush;

    int count=0;
    double min_time=10;
    if (profile) min_time =60;
    CGAL::Timer tim;
    tim.start();
    while (tim.time() < min_time) {
      ud.update_coordinates(fpts);
      if (count ==0 ) activated= CGAL::stat_number_of_activated_vertices_;
      ud.update_coordinates(ipts);
      count+=2;
    }
    tim.stop();
    if (!speedup_only) {
      std::cout << "done." << std::endl;
      std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
      ud.write_statistics(std::cout);
    }
    ud.audit();
    update_speed= count;
  }

  if (!profile) {
    K ik, fk;
    K::Key_range ir= ik.new_point_2s(ipts.begin(), ipts.end());
    fk.new_point_2s(fpts.begin(), fpts.end());
    if (!speedup_only) std::cout << "Timing hybrid..." << std::flush;
    Del d(ik);
    d.insert(ir.first, ir.second);
    int count=0;
    CGAL::Timer tim;
    tim.start();
    int nflips;
    do  {
      nflips= hybrid(d, ik, fk);
      nflips+= hybrid(d, fk, ik);
      //std::cout << nflips << std::endl;
      count+=2;
   } while (tim.time() < 10);
    tim.stop();
    if (!speedup_only) {
      std::cout << "done." << std::endl;
      std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
      std::cout << "nflips is " << nflips << std::endl;
    }
  }
  if (!profile) {
    K ik, fk;
    K::Key_range ir= ik.new_point_2s(ipts.begin(), ipts.end());
    K::Key_range fr= fk.new_point_2s(fpts.begin(), fpts.end());
    if (!speedup_only) std::cout << "Timing verifying..." << std::flush;

    Del d(ik);
    d.insert(ir.first, ir.second);

    int count=0;
    CGAL::Timer tim;
    tim.start();
    int bad_count;
    do  {
      bad_count=0;
      for (Del::Edge_iterator it= d.edges_begin();
	   it != d.edges_end(); ++it){
	if (!compute_ok<Del>(*it, fk)) {
	  ++bad_count;
	}
      }
      ++count;
   } while (tim.time() < 10);
    tim.stop();
    if (!speedup_only) {
      std::cout << "done." << std::endl;
      std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
      std::cout << "Bad count is " << bad_count << std::endl;
    }
  }

  if (0) {
    K ik, fk;
    K::Key_range ir= ik.new_point_2s(ipts.begin(), ipts.end());
    K::Key_range fr= fk.new_point_2s(fpts.begin(), fpts.end());
    std::cout << "Timing rebuilding..." << std::flush;
    std::vector<K::Point_2> ipts(ir.first, ir.second);

    int count=0;
    CGAL::Timer tim;
    tim.start();
    while (tim.time() < 10) {
      std::random_shuffle(ipts.begin(), ipts.end());
      Del d(ik);
      d.insert(ipts.begin(), ipts.end());
      std::random_shuffle(ipts.begin(), ipts.end());
      Del d2(fk);
      d2.insert(ipts.begin(), ipts.end());
      count+=2;
    }
    tim.stop();
    std::cout << "done." << std::endl;
    std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
  }

  if (!profile) {
    K ik, fk;
    K::Key_range ir= ik.new_point_2s(ipts.begin(), ipts.end());
    K::Key_range fr= fk.new_point_2s(fpts.begin(), fpts.end());
    if (!speedup_only) std::cout << "Timing rebuilding with hierarchy..." << std::flush;
    std::vector<K::Point_2> ipts(ir.first, ir.second);

    int count=0;
    CGAL::Timer tim;
    tim.start();
    while (tim.time() < 10) {
      std::random_shuffle(ipts.begin(), ipts.end());
      H d(ik);
      d.insert(ipts.begin(), ipts.end());
      std::random_shuffle(ipts.begin(), ipts.end());
      H d2(fk);
      d2.insert(ipts.begin(), ipts.end());
      count+=2;
    }
    tim.stop();
    if (!speedup_only) {
      std::cout << "done." << std::endl;
      std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
    } else {
      std::cout << static_cast<double>(update_speed)/count << " " << static_cast<double>(activated)/
	CGAL::stat_number_of_vertices_ << std::endl;
    }
  }

  if (!profile) {
    CDel d;
    std::vector<CDel::Vertex_handle> vhs(ipts.size());
    for (unsigned int i=0; i < ipts.size(); ++i) {
      vhs[i]= d.insert(ipts[i]);
      if (vhs[i] == CDel::Vertex_handle()) {
	std::cerr  << "Point " << ipts[i] << " duplicated in input." << std::endl;
      }
    }
    if (0) {
      std::cout << "Timing reinsert ..." << std::flush;

      int count=0;
      CGAL::Timer tim;

      tim.start();
      while (tim.time() < 10) {
	for (unsigned int i=0; i < ipts.size(); ++i) {
	  if (fpts[i] != vhs[i]->point()) {
	    CDel::Vertex_handle nvhs= d.insert(fpts[i], vhs[i]->face());
	    d.remove(vhs[i]);
	    vhs[i]=nvhs;
	  }
	}
	for (unsigned int i=0; i < fpts.size(); ++i) {
	  if (ipts[i] != vhs[i]->point()) {
	    CDel::Vertex_handle nvhs=  d.insert(ipts[i], vhs[i]->face());
	    d.remove(vhs[i]);
	    vhs[i]=nvhs;
	  }
	}
	count+=2;
      }
      tim.stop();
      std::cout << "done." << std::endl;
      std::cout << count << " reps in " << tim.time()
		<< " seconds" << std::endl;
    }
  }
  if (0){
    UD ud(ipts.begin(), ipts.end());

    // moncontrol(1);

    std::cout << "Timing patching..." << std::flush;

    int count=0;
    double min_time=10;
    if (profile) min_time =60;
    CGAL::Timer tim;
    tim.start();
    while (tim.time() < min_time) {
      ud.update_coordinates(fpts);
      ud.update_coordinates(ipts);
      count+=2;
    }
    tim.stop();

    std::cout << "done." << std::endl;
    std::cout << count << " reps in " << tim.time() << " seconds" << std::endl;
    ud.write_statistics(std::cout);
    ud.audit();
  }

  return EXIT_SUCCESS;

};


/// DRIVE TO POSE

#include "ros/ros.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Quaternion.h"
#include "mutex"
#include "kinova_msgs/PoseVelocity.h"
#include "tf/transform_datatypes.h"
#include "angles/angles.h"

#define VEL_LIN_MAX 0.04
#define VEL_ANG_MAX 0.4

//float thresh_lin = 0.01;
//float thresh_ang = 0.05;

//double temp_rol, temp_pit, temp_yaw;
//double goal_rol, goal_pit, goal_yaw;
//double start_rol, start_pit, start_yaw;
//double del_rol, del_pit, del_yaw;

//std::string result;
//std::mutex lock_pose;
//std::mutex lock_status;
//std::mutex lock_proc;
//std::mutex lock_force;
//double force_f, force_b;
//bool processing=false;
//ros::Publisher cmd_pos;
//ros::Publisher cmd_vel;

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include "mutex"
#include "angles/angles.h"
#include "std_msgs/Int32.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"

#include <hri_package/Sens_Force.h>
#include <kinova_msgs/PoseVelocity.h>

#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#define ARC_DOWN 1
#define ARC_UP 2
#define TRANSLATE_BACK 3
#define RAISE_CUP 4
#define LOWER_CUP 5
#define TRANSLATE_UP 6
#define TRANSLATE_DOWN 7
#define TRANSLATE_FRONT 8
#define END_EFF_FRAME "j2s7s300_end_effector"
#define BASE_FRAME "j2s7s300_link_base"
#define SENSOR_FRAME "forcesensor"
#define FORCE_F_1_2_THRESH 0.2
#define FORCE_F_2_3_THRESH 1.0
#define ROTATION_STEP 10
#define MAX_STEPS 200
#define VEL_LIN_MAX 0.04
#define VEL_ANG_MAX 0.4
#define VEL_CMD_DURATION 0.8
#define UPPER_FEED_ANGLE_THRESH 140 // was 140

#define ACTION_DOWN 0
#define ACTION_STAY 1
#define ACTION_UP 2

double lower_angle_thresh = 85;
float thresh_lin = 0.01;
float thresh_ang = 0.05;

int step_count;

std::string result;
std::mutex lock_pose;
std::mutex lock_status;
std::mutex lock_proc;
std::mutex lock_force;
double force_f, force_b;
bool processing=false;
ros::Publisher cmd_pos;
ros::Publisher cmd_vel;
ros::Publisher pub_kinova_vel;

geometry_msgs::PoseStamped current_pose, start_pose, goal_pose;
std::mutex pose_lock;

void getRPYFromQuaternionMSG(geometry_msgs::Quaternion orientation, double& roll,double& pitch, double& yaw)
{
  tf::Quaternion quat;
  tf::quaternionMsgToTF(orientation,quat);
  quat.normalize();
  tf::Matrix3x3 mat(quat);
  mat.getRPY(roll, pitch,yaw);

  //ROS_INFO_STREAM("Quaternion is : " << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w());
  //ROS_INFO_STREAM("Converted R: " << roll << " P: " << pitch << " Y: " << yaw);
}

void poseGrabber(geometry_msgs::PoseStamped pose)
{
  pose_lock.lock();
  current_pose = pose;
  pose_lock.unlock();
  //ROS_INFO_STREAM("Got current pose as x=" << pose << " saved as " << current_pose);
}

void publishTwistForDuration(geometry_msgs::TwistStamped twist_msg, double duration)
{
  ros::Time time_start = ros::Time::now();
  while (ros::Time::now() - time_start < ros::Duration(duration))
  {
    ros::spinOnce();
    cmd_vel.publish(twist_msg);
  }
}

void driveToRollGoalWithVelocity(int direction)
{
  if(direction == RAISE_CUP)
    ROS_INFO_STREAM("Raise cup");
  else if (direction == LOWER_CUP)
    ROS_INFO_STREAM("Lower cup");
  else
  {
    ROS_ERROR_STREAM("Wrong direction given!!");
    return;
  }

  geometry_msgs::PoseStamped start_pose, goal_pose, temp_pose;

  lock_pose.lock();
  start_pose=current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pit, temp_yaw;
  double goal_rol, goal_pit, goal_yaw;
  double start_rol, start_pit, start_yaw;
  double del_rol;

  goal_pose = start_pose;

  tf::Quaternion q_rot = tf::createQuaternionFromRPY(angles::from_degrees( direction==RAISE_CUP?ROTATION_STEP:-ROTATION_STEP),angles::from_degrees(0),angles::from_degrees(0)); // Rotate about x by 20 degrees
  tf::Quaternion q_start; tf::quaternionMsgToTF(start_pose.pose.orientation, q_start);
  tf::Quaternion q_goal = q_start*q_rot;

  tf::quaternionTFToMsg(q_goal,goal_pose.pose.orientation);

  ros::Rate loop_rate(100);
  //ros::Time start = ros::Time::now();
  while(ros::ok())
  {
    ros::spinOnce();
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    getRPYFromQuaternionMSG(goal_pose.pose.orientation, goal_rol, goal_pit, goal_yaw);
    getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pit, temp_yaw);
    getRPYFromQuaternionMSG(start_pose.pose.orientation, start_rol, start_pit, start_yaw);

    if (goal_rol < 0 ) goal_rol = goal_rol + angles::from_degrees(360);
    if (temp_rol < 0 ) temp_rol = temp_rol + angles::from_degrees(360);

    del_rol = goal_rol - temp_rol;

    if (std::fabs(del_rol)>=thresh_ang)
    {
      geometry_msgs::TwistStamped twist_msg;
      twist_msg.twist.linear.x = 0;
      twist_msg.twist.linear.y = 0;
      twist_msg.twist.linear.z = 0;
      twist_msg.twist.angular.x= del_rol>0?VEL_ANG_MAX:-VEL_ANG_MAX;
      twist_msg.twist.angular.y = 0;
      twist_msg.twist.angular.z= 0;

      ROS_INFO_STREAM("goal_rol: " << angles::to_degrees(goal_rol) << " temp_rol : " << angles::to_degrees(temp_rol) << " del_rol: " << del_rol);
      cmd_vel.publish(twist_msg);
    }
    else
    {
      //ROS_INFO_STREAM("Roll: " << angles::to_degrees(temp_rol)  << " Delta: " << del_rol << " Goal reached, breaking loop");
      break;
    }

    ros::spinOnce();
    loop_rate.sleep();
  }

}

void printRoll(std::string text)
{
  ros::spinOnce();
  lock_pose.lock();
  geometry_msgs::PoseStamped initial_pose=current_pose;
  lock_pose.unlock();

  double r,p,y;
  getRPYFromQuaternionMSG(initial_pose.pose.orientation,r,p,y);
  lower_angle_thresh = r;
  ROS_INFO_STREAM(text << angles::to_degrees(lower_angle_thresh));
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "velocity_drive");
  ros::NodeHandle nh;

  //ros::Publisher cmd_vel = nh.advertise<geometry_msgs::TwistStamped>("/RobotControl/VelocityControl", 1000);
  cmd_vel = nh.advertise<geometry_msgs::TwistStamped>("/RobotControl/VelocityControl", 1000);
  pub_kinova_vel = nh.advertise<kinova_msgs::PoseVelocity>("/j2s7s300_driver/in/cartesian_velocity", 1000);
  ros::Subscriber tool_pose = nh.subscribe("/j2s7s300_driver/out/tool_pose",1000, poseGrabber );

  ros::Rate loop_rate(100);
  geometry_msgs::PoseStamped temp_pose;

  geometry_msgs::TwistStamped vel_msg;

  // This loop to wait until subscriber starts returning valid poses.
  while(ros::ok())
  {
    pose_lock.lock();
    temp_pose = current_pose;
    pose_lock.unlock();
    ros::spinOnce();
    loop_rate.sleep();
    if (temp_pose.pose.position.x > 0) break;
  }

  ROS_INFO("Valid pose available");

//  for (int i =0; i<5; i++)

//  {
//    printRoll("Roll before change ");

//    driveToRollGoalWithVelocity(RAISE_CUP);

//    printRoll("Roll after change, pre pause ");
//    sleep(0.7);
//    printRoll("Roll after change, post pause ");
//    ROS_INFO("----------------------");

//    printRoll("Roll before change");

//    driveToRollGoalWithVelocity(LOWER_CUP);

//    //printRoll("Roll after change, pre pause ");
//    sleep(0.7);
//    printRoll("Roll after change, post pause ");
//    ROS_INFO("----------------------");
//  }

  printRoll("Roll before change ");
  driveToRollGoalWithVelocity(RAISE_CUP);
  printRoll("Roll after change, pre pause ");
  sleep(1.0);
  printRoll("Roll after change, post pause ");

  ros::spinOnce();
  return 0;
}


#include <cstdlib>
#include <cmath>
#include <boost/program_options.hpp>
#include <QtWidgets/QApplication>
#include <QtWidgets/QMainWindow>
#include <QtCharts/QChartView>
#include <QtCharts/QLineSeries>
#include "app.h"
#include "Coordinator.h"
#include "RandomStrategy.h"
#include "BuyAndHoldRandomStrategy.h"

#include <QtCharts>

using namespace QtCharts;

namespace po = boost::program_options;
std::unordered_map<std::string, StockData> Coordinator::stocks;

po::options_description getDescription() {

  po::options_description desc("Options");
  desc.add_options()
    ("help,h", "Display this message")
    ("version,v", "Display the version number");

  return desc;
}

/*
int main(int argc, char *argv[]) {

    po::options_description desc = getDescription();
    po::variables_map varmap;

    try {
        store(po::command_line_parser(argc, argv).options(desc).run(), varmap);
        notify(varmap);
    } catch (po::error &error) {
        std::cerr << error.what() << std::endl;
        return EXIT_FAILURE;
    }

    if (varmap.count("help")) {
        std::cout << desc << std::endl;
    }

    if (varmap.count("version")) {
        App app;
        std::cout << app.getProjectVersion() << std::endl;
    }

    return EXIT_SUCCESS;
}
*/

int test_trading_strategy(int argc, char **argv, Strategy &cur_strat, int investment_money) {
  std::string RESOURCES_PATH = "/home/jan/StockAnalyzer/resources/";
  Coordinator::read_stock_data(RESOURCES_PATH);
  for (const std::string& stocks : Coordinator::get_all_stocks()) {
    std::cout << stocks << std::endl;
  }

  std::string first_date_string("19500103");
  boost::gregorian::date first_date(boost::gregorian::from_undelimited_string(first_date_string));
  boost::gregorian::date current_date(boost::gregorian::from_undelimited_string(first_date_string));
  std::string last_date_string("20180827");
  boost::gregorian::date last_date(boost::gregorian::from_undelimited_string(last_date_string));

  std::cout << "Started on " << boost::gregorian::to_simple_string(first_date);
  auto *series = new QLineSeries();
  series->setName(QString::fromStdString(cur_strat.getName()));
  auto *dax_series = new QLineSeries();
  dax_series->setName("DAX Series");
  auto *sp_series = new QLineSeries();
  sp_series->setName("S&P 500 Series");
  auto *inflation_series2 = new QLineSeries();
  inflation_series2->setName("Inflation @ 2%");
  auto *inflation_series3 = new QLineSeries();
  inflation_series3->setName("Inflation @ 3%");
  int i = 0;
  while (current_date <= last_date) {
    cur_strat.rebalance(current_date);
    series->append(i, cur_strat.get_current_networth(current_date));
    dax_series->append(i, 1000 / 16 * Coordinator::get_price_at("DAX_30_historical_data_fake.csv", current_date).value().get_open());
    sp_series->append(i, 1000 / 16 * Coordinator::get_price_at("SnP500_historical_data.csv", current_date).value().get_open());
    boost::gregorian::date_duration passed_duration = current_date - first_date;
    float exponent = passed_duration.days() / 365.0f;
    inflation_series2->append(i, investment_money * std::pow(1.02, exponent));
    inflation_series3->append(i, investment_money * std::pow(1.03, exponent));

    do {
      boost::gregorian::days next_day(1);
      current_date = current_date + next_day;
      i++;
    } while (!Coordinator::date_valid(current_date) && current_date <= last_date);
  }

  std::cout << "\nEnded up with " << cur_strat.get_current_networth(last_date) << " on "
            << boost::gregorian::to_simple_string(last_date) << std::endl;
  QApplication app(argc, argv);

  QChart *chart = new QChart();
  chart->legend()->setVisible(true);
  chart->addSeries(series);
  chart->addSeries(dax_series);
  chart->addSeries(sp_series);
  chart->addSeries(inflation_series2);
  chart->addSeries(inflation_series3);
  chart->createDefaultAxes();
  chart->setTitle("Portfolio performance over time");

  QChartView *chartView = new QChartView(chart);
  chartView->setRenderHint(QPainter::Antialiasing);
  QMainWindow window;
  window.setCentralWidget(chartView);
  window.resize(400, 300);
  window.show();

  return app.exec();
}

int main(int argc, char **argv) {
  long double investment_money = 1000;
  RNGType rng;
  boost::uniform_real<> zero_to_one(0, 1);
  BuyAndHoldRandomStrategy r(investment_money, rng, zero_to_one);

  std::cout << "Starting with " << investment_money << " money." << std::endl;
  return test_trading_strategy(argc, argv, r, investment_money);
}


/******************************************************************************
 * Copyright (c) 2017 Johann Prankl
 *
 * 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.
 *
 ******************************************************************************/

#define AO_DBG_IMAGES

#include <float.h>
#include <pcl/common/time.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <v4r/common/convertImage.h>
#include <v4r/common/convertPose.h>
#include <v4r/common/pcl_opencv.h>
#include <v4r/keypoints/ArticulatedObject.h>
#include <v4r/keypoints/io.h>
#include <v4r/tracking/ObjectTrackerMono.h>
#include <boost/smart_ptr.hpp>
#include <fstream>
#include <iostream>
#include <map>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdexcept>
#include <v4r/keypoints/impl/invPose.hpp>
#include <v4r/keypoints/impl/toString.hpp>
#include <v4r/reconstruction/impl/projectPointToImage.hpp>

#include <pcl/io/grabber.h>
//#include <pcl/io/openni2_grabber.h>
#include <pcl/io/openni_grabber.h>

#include <glog/logging.h>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
using namespace v4r;

inline double sqr(double v) {
  return v * v;
}

class ObjTrackerMono {
 private:
  std::string log_dir_;
  bool live_;
  int dbg_;

  int ao_sleep_;
  size_t start_;
  size_t last_frame_;
  int camera_id_;
  cv::Mat_<cv::Vec3b> image_;
  cv::Mat_<cv::Vec3b> im_draw_;

  cv::Mat_<double> dist_coeffs_;
  cv::Mat_<double> intrinsic_;
  cv::Mat_<double> src_dist_coeffs_;
  cv::Mat_<double> src_intrinsic_;
  Eigen::Matrix4f pose_;
  v4r::ArticulatedObject::Ptr model_;

  std::string model_file_, filenames_, cam_file_, cam_file_model_;
  std::vector<Eigen::Vector3f> cam_trajectory;
  bool log_cams_;
  bool log_files_;
  bool loop_;
  v4r::ObjectTrackerMono::Parameter param_;

  std::shared_ptr<pcl::Grabber> interface_;
  std::shared_ptr<cv::VideoCapture> cap_;
  v4r::ObjectTrackerMono::Ptr tracker_;

  /**
   *@brief drawCoordinateSystem
   *@param im
   *@param pose
   *@param intrinsic
   *@param dist_coeffs
   */
  void drawCoordinateSystem(cv::Mat &im, const Eigen::Matrix4f &pose, const cv::Mat_<double> &intrinsic,
                            const cv::Mat_<double> &dist_coeffs, double size, int thickness) const {
    Eigen::Matrix3f R = pose.topLeftCorner<3, 3>();
    Eigen::Vector3f t = pose.block<3, 1>(0, 3);

    Eigen::Vector3f pt0 = R * Eigen::Vector3f(0, 0, 0) + t;
    Eigen::Vector3f pt_x = R * Eigen::Vector3f(size, 0, 0) + t;
    Eigen::Vector3f pt_y = R * Eigen::Vector3f(0, size, 0) + t;
    Eigen::Vector3f pt_z = R * Eigen::Vector3f(0, 0, size) + t;

    cv::Point2f im_pt0, im_pt_x, im_pt_y, im_pt_z;

    if (!dist_coeffs.empty()) {
      v4r::projectPointToImage(&pt0[0], &intrinsic(0), &dist_coeffs(0), &im_pt0.x);
      v4r::projectPointToImage(&pt_x[0], &intrinsic(0), &dist_coeffs(0), &im_pt_x.x);
      v4r::projectPointToImage(&pt_y[0], &intrinsic(0), &dist_coeffs(0), &im_pt_y.x);
      v4r::projectPointToImage(&pt_z[0], &intrinsic(0), &dist_coeffs(0), &im_pt_z.x);
    } else {
      v4r::projectPointToImage(&pt0[0], &intrinsic(0), &im_pt0.x);
      v4r::projectPointToImage(&pt_x[0], &intrinsic(0), &im_pt_x.x);
      v4r::projectPointToImage(&pt_y[0], &intrinsic(0), &im_pt_y.x);
      v4r::projectPointToImage(&pt_z[0], &intrinsic(0), &im_pt_z.x);
    }

    cv::line(im, im_pt0, im_pt_x, CV_RGB(255, 0, 0), thickness);
    cv::line(im, im_pt0, im_pt_y, CV_RGB(0, 255, 0), thickness);
    cv::line(im, im_pt0, im_pt_z, CV_RGB(0, 0, 255), thickness);
  }

  /**
   *drawConfidenceBar
   */
  void drawConfidenceBar(cv::Mat &im, const double &conf) const {
    int bar_start = 50, bar_end = 200;
    int diff = bar_end - bar_start;
    int draw_end = diff * conf;
    double col_scale = 255. / (double)diff;
    cv::Point2f pt1(0, 30);
    cv::Point2f pt2(0, 30);
    cv::Vec3b col(0, 0, 0);

    if (draw_end <= 0)
      draw_end = 1;

    for (int i = 0; i < draw_end; i++) {
      col = cv::Vec3b(255 - (i * col_scale), i * col_scale, 0);
      pt1.x = bar_start + i;
      pt2.x = bar_start + i + 1;
      cv::line(im, pt1, pt2, CV_RGB(col[0], col[1], col[2]), 8);
    }
  }

 public:
  ObjTrackerMono() {
    log_dir_ = "/tmp/log/";
    live_ = -1;
    dbg_ = 0;
    ao_sleep_ = 1;
    start_ = 0;
    last_frame_ = 100;
    loop_ = false;
    camera_id_ = -1;

    intrinsic_ = cv::Mat_<double>::eye(3, 3);
    dist_coeffs_ = cv::Mat::zeros(4, 1, CV_64F);
    log_cams_ = false;
    log_files_ = false;

    param_.kt_param.plk_param.use_ncc = true;
    param_.kt_param.plk_param.ncc_residual = .5;

    tracker_.reset(new v4r::ObjectTrackerMono(param_));
    model_.reset(new ArticulatedObject());
  }

  void cloud_cb(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) {
    PCLOpenCVConverter<pcl::PointXYZRGBA> pcl_opencv_converter;
    pcl_opencv_converter.setInputCloud(cloud);
    image_ = pcl_opencv_converter.getRGBImage();
  }

  void setup(int argc, char **argv) {
    po::options_description desc("Monocular Object Tracker\n======================================\n**Allowed options");
    desc.add_options()("help,h", "produce help message")("model_file,m",
                                                         po::value<std::string>(&model_file_)->required(),
                                                         "model file (.ao)  (pointcloud or stored model with .bin)")(
        "live,l", po::bool_switch(&live_), "use live stream of camera id")(
        "camera_id,c", po::value<int>(&camera_id_)->default_value(camera_id_), "camera id (-1... use PCL Openni)")(
        "start frame,s", po::value<size_t>(&start_)->default_value(start_), "start frame")(
        "last frame,e", po::value<size_t>(&last_frame_)->default_value(last_frame_), "last frame")(
        "cam_file,a", po::value<std::string>(&cam_file_),
        "camera calibration files for the tracking sequence (.yml) (opencv format)")(
        "filenames,f", po::value<std::string>(&filenames_), "image filename (printf-style) or filename")(
        "log_dir", po::value<std::string>(&log_dir_)->default_value(log_dir_), "directory for logging output")(
        "loop", po::bool_switch(&loop_), "loop through files");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << std::endl;
    }

    try {
      po::notify(vm);
    } catch (std::exception &e) {
      std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
    }

    if (live_) {
      loop_ = true;

      if (camera_id_ == -1) {
        try {
          //                    interface_.reset( new pcl::io::OpenNI2Grabber() );
          interface_.reset(new pcl::OpenNIGrabber());
          boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &)> f =
              boost::bind(&ObjTrackerMono::cloud_cb, this, _1);
          interface_->registerCallback(f);
        } catch (pcl::IOException e) {
          std::cerr << "PCL threw error " << e.what() << ". Could not start camera..." << std::endl;
        }
      } else {
        cap_.reset(new cv::VideoCapture);
        cap_->open(camera_id_);
        cap_->set(CV_CAP_PROP_FRAME_WIDTH, 640);
        cap_->set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        if (!cap_->isOpened())
          throw std::runtime_error("Could not initialize capturing...");
      }
    }

    intrinsic_(0, 0) = intrinsic_(1, 1) = 525;
    intrinsic_(0, 2) = 320, intrinsic_(1, 2) = 240;

    if (!cam_file_.empty()) {
      cv::FileStorage fs(cam_file_, cv::FileStorage::READ);
      fs["camera_matrix"] >> intrinsic_;
      fs["distortion_coefficients"] >> dist_coeffs_;
    }
    if (!cam_file_model_.empty()) {
      cv::FileStorage fs(cam_file_model_, cv::FileStorage::READ);
      fs["camera_matrix"] >> src_intrinsic_;
      fs["distortion_coefficients"] >> src_dist_coeffs_;
    }

    if (!src_intrinsic_.empty())
      tracker_->setObjectCameraParameter(src_intrinsic_, src_dist_coeffs_);

    tracker_->setCameraParameter(intrinsic_, dist_coeffs_);

    // -------------------- load model ---------------------------
    std::cout << "Load: " << model_file_ << std::endl;

    if (v4r::io::read(model_file_, model_))
      tracker_->setObjectModel(model_);
    else
      throw std::runtime_error("Tracking model file not found!");
  }

  void run() {
    double time, conf;
    double mean_time = 0;
    int cnt4time = 0;

    cv::namedWindow("image", CV_WINDOW_AUTOSIZE);

    if (camera_id_ == -1)
      interface_->start();

    for (size_t i = start_; i < last_frame_ || loop_; i++) {
      std::cout << "---------------- FRAME #" << i << " -----------------------" << std::endl;

      if (live_) {
        if (camera_id_ != -1)
          *cap_ >> image_;
      } else {
        char filename[PATH_MAX];
        snprintf(filename, PATH_MAX, filenames_.c_str(), i);
        std::cout << filename << std::endl;
        image_ = cv::imread(filename, 1);
      }

      if (image_.empty()) {
        cv::waitKey(200);
        continue;
      }

      image_.copyTo(im_draw_);

      bool is_ok = false;

      if (dbg_)
        tracker_->dbg = im_draw_;

      {
        pcl::ScopeTime t("overall time");
        is_ok = tracker_->track(image_, pose_, conf);
        time = t.getTime();
      }

      cnt4time++;
      mean_time += time;
      std::cout << "tracker conf = " << conf << std::endl
                << "mean_frame_rate = " << mean_time / double(cnt4time) << " (" << cnt4time << ")" << std::endl;

      if (is_ok)
        std::cout << "Status: stable tracking" << std::endl;
      else if (conf > 0.05)
        std::cout << "Status: unstable tracking" << std::endl;
      else
        std::cout << "Status: object lost!" << std::endl;

      if (conf > 0.05 && i > start_) {
        if (log_cams_)
          cam_trajectory.push_back(pose_.block<3, 1>(0, 3));

        drawCoordinateSystem(im_draw_, pose_, intrinsic_, dist_coeffs_, 0.1, 4);
      }

      drawConfidenceBar(im_draw_, conf);

      cv::imshow("image", im_draw_);

      int key = cv::waitKey(ao_sleep_);
      if (((char)key) == 27)
        break;
      if (((char)key) == 'r')
        ao_sleep_ = 1;
      if (((char)key) == 's')
        ao_sleep_ = 0;
      if (((char)key) == 'c')
        cam_trajectory.clear();
      if (((char)key) == 'l') {
        log_cams_ = !log_cams_;
        cv::waitKey(100);
      }
      if (((char)key) == 'r') {
        tracker_->reset();
      }
    }

    // compute standard dev. of the object location
    double x_mean = 0, y_mean = 0, z_mean = 0;
    double x_var = 0, y_var = 0, z_var = 0;
    for (unsigned i = 0; i < cam_trajectory.size(); i++) {
      x_mean += cam_trajectory[i][0];
      y_mean += cam_trajectory[i][1];
      z_mean += cam_trajectory[i][2];
    }
    if (cam_trajectory.size() > 0) {
      x_mean /= (double)cam_trajectory.size();
      y_mean /= (double)cam_trajectory.size();
      z_mean /= (double)cam_trajectory.size();
    }
    for (unsigned i = 0; i < cam_trajectory.size(); i++) {
      x_var += sqr(cam_trajectory[i][0] - x_mean);
      y_var += sqr(cam_trajectory[i][1] - y_mean);
      z_var += sqr(cam_trajectory[i][2] - z_mean);
    }
    if (cam_trajectory.size() > 0) {
      x_var /= (double)cam_trajectory.size();
      y_var /= (double)cam_trajectory.size();
      z_var /= (double)cam_trajectory.size();
    }

    std::cout << "mean object location (x,y,z): " << x_mean << "," << y_mean << "," << z_mean << std::endl;
    std::cout << "standard deviation (x,y,z): " << std::sqrt(x_var) << "," << std::sqrt(y_var) << ","
              << std::sqrt(z_var) << std::endl;

    if (camera_id_ == -1)
      interface_->stop();
  }
};

int main(int argc, char *argv[]) {
  ObjTrackerMono tracker;
  tracker.setup(argc, argv);
  tracker.run();
  return 0;
}


#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <boost/program_options.hpp>

#include "msa.hpp"
#include "model.hpp"
#include "MCseq.hpp"
#include "OptParam.hpp"
using namespace std;

namespace po = boost::program_options;

int main(int argc, char *argv[])
{
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h","produce help message")
    ("param,p", po::value<string>(), "LGM parameter file")
    ("logbase,l", po::value<string>(), "basename for log files")
    ("nloop", po::value<int>(), "number of optimization loops")
    ("nsweep", po::value<int>(), "number of sweeps")
    ("temp_min", po::value<double>(), "lowest temperature")
    ("temp_max", po::value<double>(), "highest temperature")
    ("min_energy", po::value<double>(), "minimun energy")
    ("max_energy", po::value<double>(), "maximum energy")
    ("delta_energy", po::value<double>(), "width of energy bin")
    ("mu_len", po::value<double>(), "Mu_len")
    ("mu_I", po::value<double>(), "Mu_I")
    ("eps,e", po::value<double>(), "precision of DOS")
    ("dos", po::value<string>(), "WL DOS file for continuation!")
    ("wlfactor,w", po::value<double>(), "WLfactor for (re)starting")
    ("seed", po::value<int>(), "random seed")
    ;

  po::positional_options_description p;
  po::variables_map vm;
  po::store(po::command_line_parser(argc,argv).
	    options(desc).positional(p).run(), vm);
  po::notify(vm);

  if(vm.count("help")) {
    cout << desc << endl;
    return 1;
  }

  string logbase = "wldos";
  if(vm.count("logbase")) {
    logbase = vm["logbase"].as<string>();
  }
  cerr << "# LOGBASE= " << logbase << endl;

  if(!vm.count("param")) {
    cerr << "Specify the parameter file!" << endl;
    exit(1);
  }

  Model model;
  model.read_parameters(vm["param"].as<string>());
  if(vm.count("mu_len")) {
    const double mu_len  = vm["mu_len"].as<double>();
    cerr << "Mu_len = " << mu_len << endl;
    model.set_mu_len(mu_len);
  }
  if(vm.count("mu_I")) {
    const double mu_I  = vm["mu_I"].as<double>();
    cerr << "Mu_I = " << mu_I << endl;
    model.set_mu_lenI(mu_I);
  }

  MCseq mcs(model);
  if(vm.count("seed")) {
    mcs.set_seed(vm["seed"].as<int>());
    cerr << "random seed= " << (vm["seed"].as<int>()) << endl;;
  }
  double tmax=1.2,tmin=0.5;
  if(vm.count("temp_max")) {
    tmax = (vm["temp_max"].as<double>());
  }
  if(vm.count("temp_min")) {
    tmin = (vm["temp_min"].as<double>());
  }
  cerr << "Temperature range: " << tmin << " ... " << tmax << endl;

  mcs.select_type = CANONICAL;

  mcs.NMONITOR = 10000;
  mcs.gibbs_sampler_p = true;
  const int nsweeps_eq = 10000;
  double min_energy=0;
  if(vm.count("min_energy")) {
    min_energy = vm["min_energy"].as<double>();
  } else if(!vm.count("dos")) {
    mcs.init_stats(model);
    for(int k = 0; k < 10; ++k) {
      mcs.set_random_sequence(model);
      mcs.set_temperature(1.0);
      mcs.run(model, 10000, false);
      mcs.anneal(model,1000, 100, 1.0, tmin);
      mcs.set_temperature(tmin);
      mcs.run(model, 10000, true);
    }
    mcs.finish_stats(model);
    cerr << "Tmin " << tmin
	 << "\t" << mcs.Eave << "\t" << sqrt(mcs.Evar) << endl;
    min_energy = floor(mcs.Eave);
  }

  double max_energy=100;
  if(vm.count("max_energy")) {
    max_energy = vm["max_energy"].as<double>();
  } else if(!vm.count("dos")) {
    mcs.init_stats(model);
    mcs.set_temperature(tmax);
    for(int k = 0; k < 100; ++k) {
      mcs.set_random_sequence(model);
      mcs.run(model, 1000, false);
      mcs.run(model, 1000, true);
    }
    mcs.finish_stats(model);
    cerr << "Tmax " << tmax
	 << "\t" << mcs.Eave << "\t" << sqrt(mcs.Evar) << endl;
    max_energy = ceil(mcs.Eave);
  }

  double delta_energy = 0.2;
  if(vm.count("delta_energy")) {
    delta_energy = vm["delta_energy"].as<double>();
  }
  mcs.setup_WL(min_energy, max_energy, delta_energy);

  if(vm.count("dos")) {
    mcs.read_DOS(vm["dos"].as<string>(), true);
  }
  if(vm.count("wlfactor")) {
    mcs.WLfactor = vm["wlfactor"].as<double>();
  }
  min_energy = mcs.MIN_ENERGY;
  max_energy = mcs.MAX_ENERGY;

  double eps = 1e-5;
  if(vm.count("eps")) {
    eps = vm["eps"].as<double>();
  }
  mcs.WLprec = eps;

  int nsweeps = 100000;
  if(vm.count("nsweep")) {
    nsweeps = vm["nsweep"].as<int>();
  }

  mcs.NMONITOR = 10000; //nsweeps / 10 ;

  int nloops = 10;
  if(vm.count("nloop")) {
    nloops = vm["nloop"].as<int>();
  }

  const clock_t begin_time = clock();
  cerr << "# WL sampling run for "
       << nloops << " loops of " << nsweeps << " sweeps." << endl;
  int iloop,isweeps;

  for(iloop = 1; iloop <= nloops; ++iloop) {
    cerr << "#Loop: " << iloop << endl;
    mcs.select_type = CANONICAL;
    mcs.set_random_sequence(model);
    mcs.set_temperature(tmax);
    mcs.run(model, 1000, false);
    mcs.set_temperature(1.0);
    while(1) {
      mcs.run(model, 10, false);
      if(mcs.Energy > mcs.MIN_ENERGY && mcs.Energy < mcs.MAX_ENERGY) break;
    }
    mcs.select_type = MULTI_CANONICAL;
    isweeps = mcs.run(model, nsweeps, false);
    mcs.save_DOS(logbase + ".dos");
    if(isweeps < nsweeps) break;
  }
  const clock_t end_time = clock();
  cerr << "Time " << float(end_time - begin_time)/CLOCKS_PER_SEC << endl;
  if(iloop == nloops + 1) {
    cerr << "### Wang-Landau sampling did NOT converge!" << endl;
  } else {
    cerr << "### Wang-Landau sampling CONVERGED in "
	 << iloop << " loops and "
	 << isweeps << " sweeps!" << endl;
  }
return 0;
}


/**
 * Test suite for the iterator_concepts_ordering.hpp header.
 */

#include <duck/detail/iterator_concepts_ordering.hpp>

#include <boost/mpl/assert.hpp>

BOOST_MPL_ASSERT((duck::is_more_specific_than<
                    duck::BidirectionalIterator,
                    duck::RandomAccessIterator>));

BOOST_MPL_ASSERT((duck::is_more_specific_than<
                    duck::IncrementableIterator,
                    duck::RandomAccessIterator>));

BOOST_MPL_ASSERT((duck::is_more_specific_than<
                    duck::ForwardIterator,
                    duck::RandomAccessIterator>));

BOOST_MPL_ASSERT_NOT((duck::is_more_specific_than<
                        duck::RandomAccessIterator,
                        duck::BidirectionalIterator>));

BOOST_MPL_ASSERT_NOT((duck::is_more_specific_than<
                        duck::RandomAccessIterator,
                        duck::IncrementableIterator>));

BOOST_MPL_ASSERT_NOT((duck::is_more_specific_than<
                        duck::ForwardIterator,
                        duck::ForwardIterator>));

BOOST_MPL_ASSERT_NOT((duck::is_more_specific_than<
                        duck::RandomAccessIterator,
                        duck::ForwardIterator>));


/**
 * \ file GainMaxExpanderFilter.cpp
 */

#include <iostream>

#include <ATK/Dynamic/GainMaxExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainMaxExpanderFilter, softness_test)
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainMaxExpanderFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainMaxExpanderFilter, maxreduc_test)
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_max_reduction(0.5);
  ASSERT_EQ(filter.get_max_reduction(), 0.5);
}

TEST(GainMaxExpanderFilter, maxreduc_db_test)
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_max_reduction_db(20);
  ASSERT_EQ(filter.get_max_reduction(), 100);
}

TEST(GainMaxExpanderFilter, maxreduc_range_test)
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  ASSERT_THROW(filter.set_max_reduction(-0.000001), ATK::RuntimeError);
}

TEST(GainMaxExpanderFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainMaxExpanderFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 0.f, 1.f);
  }
}

TEST(GainMaxExpanderFilter, const_1_threshold_2_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.set_max_reduction(0.001);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.700553358, outdata[i], 0.1);
  }
}

TEST(GainMaxExpanderFilter, const_1_threshold_2_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.389224231, outdata[i], 0.1);
  }
}

TEST(GainMaxExpanderFilter, const_1_threshold_2_ratio_4_max_reduction_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(0.0001);
  filter.set_max_reduction(.1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_GE(outdata[i], .1 - std::numeric_limits<float>::epsilon());
  }
}


#include <gltbx/quadrics.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/def.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace gltbx { namespace quadrics { namespace boost_python {

#define GLTBX_LOC_COMMON_ARGS \
  arg("draw_style")=GLU_FILL,\
  arg("orientation")=GLU_OUTSIDE,\
  arg("normals")=GLU_SMOOTH

  struct proto_cylinder_wrapper
  {
    typedef proto_cylinder wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt>("proto_cylinder", no_init)
        .def(init<GLdouble, GLint, GLint, GLenum, GLenum, GLenum>(
             (arg("top_to_base_radius_ratio"), arg("slices"), arg("stacks")=1,
              GLTBX_LOC_COMMON_ARGS)))
        .def(init<GLint, GLenum, GLenum, GLenum>(
                  (arg("slices"), GLTBX_LOC_COMMON_ARGS)))
        .def("draw", &wt::draw, (arg("start"), arg("end"), arg("base_radius")))
      ;
    }
  };

  struct ellipsoid_to_sphere_transform_wrapper
  {
    typedef ellipsoid_to_sphere_transform wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt>("ellipsoid_to_sphere_transform", no_init)
        .def(init<scitbx::vec3<GLdouble> const &,
                  scitbx::sym_mat3<GLdouble> const &>
                ((arg("centre"), arg("metrics"))))
        .def("non_positive_definite", &wt::non_positive_definite)
        .def("linear_part", &wt::linear_part)
        .def("translation_part", &wt::translation_part)
      ;
    }

  };

  struct ellipsoid_to_sphere_transform_shared_wrapper
  {
    typedef ellipsoid_to_sphere_transform e_t;
    typedef af::shared<e_t> w_t;

    static w_t *make(af::const_ref<scitbx::vec3<GLdouble> > const &centre,
                     af::const_ref<scitbx::sym_mat3<GLdouble> > const &metrics)
    {
      GLTBX_ASSERT(centre.size() == metrics.size());
      long n = centre.size();
      w_t result;
      result.reserve(n);
      for (int i=0; i<n; ++i) {
        result.push_back(ellipsoid_to_sphere_transform(centre[i], metrics[i]));
      }
      return new w_t(result);
    }

    static void draw(w_t const &self, proto_ellipsoid &proto)
    {
      GLfloat black[4] = { 0.f, 0.f, 0.f, 1.f };
      for (int i=0; i<self.size(); ++i) {
        if (self[i].non_positive_definite()) {
          glPushAttrib(GL_LIGHTING_BIT);
          GLfloat c[4];
          glGetMaterialfv(GL_FRONT, GL_AMBIENT, c);
          glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black);
          glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, c);
          glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, black);
          proto.draw(self[i]);
          glPopAttrib();
        }
        else proto.draw(self[i]);
      }
    }

    static void wrap() {
      using namespace boost::python;
      using namespace scitbx::af::boost_python;
      typedef return_internal_reference<> rir;
      class_<w_t> klass =
        shared_wrapper<e_t, rir>::wrap("shared_ellipsoid_to_sphere_transforms");
      klass.def("__init__",
                make_constructor(make,
                                 default_call_policies(),
                                 (arg("centres"), arg("metrics"))))
           .def("draw", draw)
        ;
    }
  };

  struct proto_ellipsoid_wrapper
  {
    typedef proto_ellipsoid wt;

    static void wrap() {
      using namespace boost::python;
      void (wt::*draw_1)(wt::vec3_t const &, wt::metrics_t const &) = &wt::draw;
      void (wt::*draw_2)(ellipsoid_to_sphere_transform const &) = &wt::draw     ;
      class_<wt>("proto_ellipsoid", no_init)
        .def(init<GLint, GLint, GLenum, GLenum, GLenum>(
             (arg("slices"), arg("stacks"), GLTBX_LOC_COMMON_ARGS)))
        .def("draw", draw_1, (arg("centre"), arg("metrics")))
        .def("draw", draw_2, arg("ellipsoid_to_sphere_transform"))
      ;
    }
  };

  struct ellipsoid_principal_sections_texture_wrapper
  {
    typedef ellipsoid_principal_sections_texture wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt>("ellipsoid_principal_sections_texture", no_init)
        .def(init<GLdouble, int, int>(
            (arg("darkening"), arg("n_s"), arg("n_t"))))
        .def("bind", &wt::bind)
        .def("unbind", &wt::unbind)
        ;
    }
  };

  double
  time_ellipsoid_to_sphere_transform(
    scitbx::af::const_ref<proto_ellipsoid::metrics_t> const &u)
  {
    GLdouble hash = 0;
    scitbx::vec3<GLdouble> centre(0.1, 0.2, 0.3);
    for (int i=0; i< u.size(); ++i) {
      ellipsoid_to_sphere_transform e2s(centre, u[i]);
      GLdouble const *m = e2s.matrix();
      hash += m[0] + m[3] + m[6];
    }
    return hash;
  }

  namespace {
    void init_module() {
      using namespace boost::python;
      proto_cylinder_wrapper::wrap();
      proto_ellipsoid_wrapper::wrap();
      ellipsoid_to_sphere_transform_wrapper::wrap();
      def("time_ellipsoid_to_sphere_transform",
          time_ellipsoid_to_sphere_transform);
      ellipsoid_to_sphere_transform_shared_wrapper::wrap();
      ellipsoid_principal_sections_texture_wrapper::wrap();
    }
  }

#undef GLTBX_LOC_COMMON_ARGS

}}} // gltbx::quadrics::boost_python

BOOST_PYTHON_MODULE(gltbx_quadrics_ext)
{
  gltbx::quadrics::boost_python::init_module();
}


#ifndef Interpolators__1D_InterpolatorBase_hpp
#define Interpolators__1D_InterpolatorBase_hpp

/** @file InterpolatorBase.hpp
  * @brief
  * @author C.D. Clark III
  * @date 12/24/16
  */

#include <memory>
#include <algorithm>
#include <type_traits>
#include <vector>
#include <Eigen/Dense>

namespace _1D {

/** @class
  * @brief A base class that provides some useful functions for interpolating data.
  * @author C.D. Clark III
  *
  * This class provides eigen matrices to store the data that is interpolated, a few setData methods
  * to populate the data.
  */

template<typename T>
struct RealTypeOf { using type = double; };
template<template<typename> class T,typename R>
struct RealTypeOf<T<R>> { using type = R; };

template<class Derived, typename Real = typename RealTypeOf<Derived>::type>
class InterpolatorBase
{

  public:
    using VectorType =  Eigen::Matrix<Real,Eigen::Dynamic,1>;
    using MapType =  Eigen::Map<const VectorType>;

  protected:
    // data members
    std::vector<Real> xData, yData;        ///< storage for interpolated data
    std::unique_ptr<MapType> xView, yView; ///< eigen matrix view of the data

  private:
    // making constructors private and the derived class a friend
    // helps to make sure that the derived class actually
    // passes itself as the template argument.
    friend Derived;

    InterpolatorBase(){ }

    // copy constructor
    // we only want to initialize x and y views if the object
    // we are copying from is initialized.
    InterpolatorBase(const InterpolatorBase& rhs)
    :xData(rhs.xData)
    ,yData(rhs.yData)
    ,xView( rhs.xView ? new MapType( xData.data(), xData.size() ) : nullptr )
    ,yView( rhs.xView ? new MapType( yData.data(), yData.size() ) : nullptr )
    {
    }

    // we use the copy-swap idiom for copy assignment
    friend void swap( InterpolatorBase& lhs, InterpolatorBase& rhs)
    {
      std::swap( lhs.xData, rhs.xData );
      std::swap( lhs.yData, rhs.yData );
      std::swap( lhs.xView, rhs.xView );
      std::swap( lhs.yView, rhs.yView );
    }

    InterpolatorBase& operator=(InterpolatorBase rhs)
    {
      swap(*this,rhs);
      return *this;
    }

  public:

    // methods to get the data
    std::vector<Real> getXData() const { return xData; }
    std::vector<Real> getYData() const { return yData; }

    // methods to set data
    // primary method. this is used by the others
    template<typename I>
    void setData( I n, const Real *x, const Real *y );

    // this template is ambiguous with the pointer template above,
    // so we want to disable it for pointers.
    template<typename X, typename Y>
    typename std::enable_if<!std::is_pointer<Y>::value>::type
    setData( const X &x, const Y &y);

  protected:

    void checkData() const; ///< Check that data has been initialized and throw exception if not.

  private:
    // callSetupInterpolator will call a function named setupInterpolator in the derived class, if
    // it exists. this is just some template magic to detect if the derived class has implemented a
    // setupInterpolator function, and to call it if it does.

    template<typename T>
    struct has_setupInterpolator
    {
      private:
        typedef std::true_type yes;
        typedef std::false_type no;

        template<typename U> static auto test(int) -> decltype(std::declval<U>().setupInterpolator(),yes());
        template<typename  > static no   test(...);

      public:
        static constexpr bool value = std::is_same<decltype(test<T>(0)),yes>::value;
    };

    template<typename T>
    typename std::enable_if<has_setupInterpolator<T>::value>::type
    callSetupInterpolator(){ static_cast<Derived*>(this)->setupInterpolator(); }

    template<typename T>
    typename std::enable_if<!has_setupInterpolator<T>::value>::type
    callSetupInterpolator(){ }

};

// implementations

template<class Derived, typename Real>
void
InterpolatorBase<Derived,Real>::checkData() const
{
  if(!this->xView || !this->yView)
    throw std::logic_error("Interpolator data is not initialized. Did you call setData()?");
  if(this->xView->size() == 0 || this->yView->size() == 0)
    throw std::logic_error("Interpolator data is zero size. Did you call setData() with non-zero sized vectors?");
}

template<class Derived, typename Real>
template<typename I>
void
InterpolatorBase<Derived,Real>::setData( I n, const Real *x, const Real *y)
{
  xData.clear();
  yData.clear();
  std::copy( x, x+n, std::back_inserter(xData) );
  std::copy( y, y+n, std::back_inserter(yData) );

  this->xView.reset( new MapType( xData.data(), n ) );
  this->yView.reset( new MapType( yData.data(), n ) );

  this->callSetupInterpolator<Derived>();
}

template<class Derived, typename Real>
template<typename X, typename Y>
typename std::enable_if<!std::is_pointer<Y>::value>::type
InterpolatorBase<Derived,Real>::setData( const X &x, const Y &y)
{
  this->setData( x.size(), x.data(), y.data() );
}

}

#endif // include protector


/*
 * Worker.cpp
 *
 *  Created on: 2 Feb 2015
 *      Author: rian
 */

#include <cmath>
#include <boost/log/trivial.hpp>
#include "Worker.h"
#include "../core/Application.h"
#include "../core/CommandLine.h"

namespace raytracer {
namespace threading {

	using namespace math;
	using namespace tracer;
	using namespace boost::log;

	Worker::Worker() {

	}

	boost::thread* Worker::start(Ray r) {

		return new boost::thread(&Worker::process, this, r);
	}

	void Worker::process(Ray r) {

		BOOST_LOG_TRIVIAL(info) << "Worker started for ray " << r.rayNumber;

		r.trace();

		BOOST_LOG_TRIVIAL(info) << "Worker ended for ray " << r.rayNumber;

		if (Application::getInstance().getVerbosity() > boost::log::trivial::info) {
			char buffer[80];
			sprintf(buffer, "Progress: %d/%d (%4.2f\%)", r.rayNumber, Application::getInstance().numWorkers,
					100.0*r.rayNumber/((double)Application::getInstance().numWorkers));
			CommandLine::getInstance().updateBody(buffer);
		}
	}

	void Worker::schedule(boost::threadpool::pool *tp, Ray r) {

		tp->schedule(boost::bind(&Worker::process, this, r));
	}

} /* namespace threading */
} /* namespace raytracer */


/* 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 "xmg_expr.hpp"

#include <boost/format.hpp>

namespace cirkit
{

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

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

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

expression_t::ptr xmg_to_expression( const xmg_graph& xmg, const xmg_function& f )
{
  auto expr = std::make_shared<expression_t>();

  if ( f.node == 0 )
  {
    expr->type = expression_t::_const;
    expr->value = f.complemented ? 1u : 0u;
  }
  else if ( f.complemented )
  {
    expr->type = expression_t::_inv;
    expr->children.push_back( xmg_to_expression( xmg, !f ) );
  }
  else if ( xmg.is_maj( f.node ) )
  {
    expr->type = expression_t::_maj;
    for ( const auto& c : xmg.children( f.node ) )
    {
      expr->children.push_back( xmg_to_expression( xmg, c ) );
    }
  }
  else if ( xmg.is_xor( f.node ) )
  {
    expr->type = expression_t::_xor;
    for ( const auto& c : xmg.children( f.node ) )
    {
      expr->children.push_back( xmg_to_expression( xmg, c ) );
    }
  }
  else
  {
    expr->type = expression_t::_var;
    expr->value = xmg.input_index( f.node );
  }

  return expr;
}

xmg_function xmg_from_expression( xmg_graph& xmg, std::vector<xmg_function>& pis, const expression_t::ptr& expr )
{
  switch ( expr->type )
  {
  case expression_t::_const:
    return xmg.get_constant( expr->value == 1u );

  case expression_t::_var:
    {
      const auto idx = expr->value;
      if ( idx >= pis.size() )
      {
        for ( auto i = pis.size(); i <= idx; ++i )
        {
          pis.push_back( xmg.create_pi( boost::str( boost::format( "x%d" ) % i )  ) );
        }
      }
      return pis[idx];
    }

  case expression_t::_inv:
    return !xmg_from_expression( xmg, pis, expr->children.front() );

  case expression_t::_and:
    {
      auto it = expr->children.begin();
      const auto a = xmg_from_expression( xmg, pis, *it++ );
      const auto b = xmg_from_expression( xmg, pis, *it );

      return xmg.create_and( a, b );
    }

  case expression_t::_or:
    {
      auto it = expr->children.begin();
      const auto a = xmg_from_expression( xmg, pis, *it++ );
      const auto b = xmg_from_expression( xmg, pis, *it );

      return xmg.create_or( a, b );
    }

  case expression_t::_maj:
    {
      auto it = expr->children.begin();
      const auto a = xmg_from_expression( xmg, pis, *it++ );
      const auto b = xmg_from_expression( xmg, pis, *it++ );
      const auto c = xmg_from_expression( xmg, pis, *it );

      return xmg.create_maj( a, b, c );
    }

  case expression_t::_xor:
    {
      auto it = expr->children.begin();
      const auto a = xmg_from_expression( xmg, pis, *it++ );
      const auto b = xmg_from_expression( xmg, pis, *it );

      return xmg.create_xor( a, b );
    }

  default:
    assert( false );
  }
}

}

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


#include "serialize.h"
#include "bounded_integer.h"
#include "assembler.h"

#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <cassert>

#include <boost/endian/conversion.hpp>

void serialize_as_binary(std::ostream& out_buffer, bint const& v)
{
	if (v.maximum_ < std::numeric_limits<std::uint8_t>::max()) // value can be serialized as 8 bits  value
	{
		uint8_t endian_corrected = boost::endian::native_to_little(static_cast<uint8_t>(v.value_));
		out_buffer.write(reinterpret_cast<const char*>(&endian_corrected), sizeof(uint8_t));
	}
	else if (v.maximum_ < std::numeric_limits<std::uint16_t>::max()) // value can be serialized as 16 bits  value
	{
		uint16_t endian_corrected = boost::endian::native_to_little(static_cast<uint16_t>(v.value_));
		out_buffer.write(reinterpret_cast<const char*>(&endian_corrected), sizeof(uint16_t));
	}
	else if (v.maximum_ < std::numeric_limits<std::uint32_t>::max()) // value can be serialized as 32 bits  value
	{
		uint32_t endian_corrected = boost::endian::native_to_little(static_cast<uint32_t>(v.value_));
		out_buffer.write(reinterpret_cast<const char*>(&endian_corrected), sizeof(uint32_t));
	}
	else if (v.maximum_ < std::numeric_limits<std::uint64_t>::max()) // value can be serialized as 64 bits  value
	{
		uint64_t endian_corrected = boost::endian::native_to_little(static_cast<uint64_t>(v.value_));
		out_buffer.write(reinterpret_cast<const char*>(&endian_corrected), sizeof(uint64_t));
	}
	else // WTF ? > 64 bits ???
	{
		throw std::out_of_range{"can't serialize value as binary : too big"};
	}
}

void serialize_as_txt(std::ostream& out_buffer, bint const& v)
{
	out_buffer << std::setfill('0') << std::setw(v.nb_digits_) << v.value_;
}

void serialize_as_txt(assembled_data const& in, bint const& start, std::ostream& out)
{
	serialize_as_txt(out, start);
	out << "\n";
	for (auto &e : in)
	{
		serialize_as_txt(out, e.first);
		out << ": ";
		serialize_as_txt(out, e.second);
		out << "\n";
	}
}

/*
	Optimization can be done here !
*/
void serialize_as_binary(assembled_data const & in, bint const& start, std::ostream & out)
{
	if (in[0].first.digits() >= 256 && in[0].second.digits() >= 256) //overflow ?
		throw std::overflow_error{"can't serialize more than 255 digits"};

	// header
	const char header[] = "CB"; //header of the file
	uint8_t address_digits = boost::endian::native_to_little(static_cast<uint8_t>(in[0].first.digits()));

	out.write(header, sizeof(header));
	out.write(reinterpret_cast<char*>(&address_digits), sizeof(address_digits));
	serialize_as_binary(out, start);

	// body
	for (auto &e : in)
	{
		serialize_as_binary(out, e.first); // address serialization
		serialize_as_binary(out, e.second); // instruction serialization
	}
}