/*******************************************************************************
* PacElemFactory
*******************************************************************************/

#include "BaBar/BaBar.hh"

#include "PacDetector/PacElemFactory.hh"
#include "PacDetector/PacMeasurementFactory.hh"
#include "PacDetector/PacTypeFactory.hh"

#include "PacGeom/PacCylDetElem.hh"
#include "PacGeom/PacCylDetType.hh"
#include "PacGeom/PacConeDetElem.hh"
#include "PacGeom/PacConeDetType.hh"
#include "PacGeom/PacPlaneDetElem.hh"
#include "PacGeom/PacRingDetType.hh"
#include "PacGeom/PacRectDetType.hh"
#include "PacGeom/PacMeasurement.hh"

#include "PacEnv/EdmlCylDetElement.hh"
#include "PacEnv/EdmlRingDetElement.hh"
#include "PacEnv/EdmlRectDetElement.hh"
#include "PacEnv/EdmlConeDetElement.hh"

#include <iostream>
#include <boost/tokenizer.hpp>

using namespace std;

#include <assert.h>

DetElem*
PacElemFactory::get( const EdmlDetElement* theEdmlElement )
{
    return PacElemFactory::instance().doGet( theEdmlElement );
}

PacElemFactory&
PacElemFactory::instance()
{
    static PacElemFactory factory;
    return factory;
}

PacElemFactory::PacElemFactory()
{}

PacElemFactory::~PacElemFactory()
{
}

DetElem*
PacElemFactory::doGet( const EdmlDetElement* theEdmlElement )
{
  assert( theEdmlElement );

    // find the measuring devices (if any)
    //
  const std::string& device = theEdmlElement->device();
  std::vector<const PacMeasurement*> mdevs;

  if(!device.empty()){
    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(",;:| ");
    tokenizer tokens(device, sep);
    for (tokenizer::iterator titer = tokens.begin();titer != tokens.end(); ++titer){
      PacMeasurement* meas = PacMeasurementFactory::get(*titer);
      if(meas != 0){
        mdevs.push_back(meas);
      } else {
        ErrMsg(fatal)  << "Can't find measurement device " << *titer << endmsg;
      }
    }
  }

    // Build a subdetector of the given type
    //
  if( theEdmlElement->type().find("Cylinder") != string::npos) {

    const EdmlCylDetElement* cyl = dynamic_cast<const EdmlCylDetElement*>(theEdmlElement);
    if(ErrLogging(routine)){
      cout << "Building " << cyl->type() << " " << cyl->name() << " " << cyl->id() << " at radius "
        << cyl->radius();
      printElem(cout,theEdmlElement);
    }
    PacCylDetType* type = PacTypeFactory::get(cyl);
    assert(type);
    return new PacCylDetElem( type, cyl->radius(), cyl->name().c_str(), cyl->id(), mdevs,
      cyl->gap(), cyl->overlap() );

  }
  if(theEdmlElement->type().find("Ring") != string::npos) {

    const EdmlRingDetElement* ring = dynamic_cast<const EdmlRingDetElement*>(theEdmlElement);

    if(ErrLogging(routine)){
      cout << "Building " << ring->type() << " " << ring->name() << " " << ring->id() << " at z "
        << ring->z();
      printElem(cout,theEdmlElement);
    }
    PacRingDetType* type = PacTypeFactory::get(ring);
    assert(type);
    return new PacPlaneDetElem( type, PacPlaneDetElem::zparallel,Hep3Vector(0.0,0.0,ring->z()), 0.0,
      ring->name().c_str(), ring->id(), mdevs,
      ring->gap(), ring->overlap() );
  }
  if(theEdmlElement->type().find("Rect") != string::npos) {

    const EdmlRectDetElement* rect = dynamic_cast<const EdmlRectDetElement*>(theEdmlElement);

    if(ErrLogging(routine)){
      cout << "Building " << rect->type() << " " << rect->name() << " " << rect->id()
        << " orientation " << rect->orientation()
        << " center rho " << rect->centerRho()
        << " center z " << rect->centerZ()
        << " center phi "  << rect->centerPhi()
        << " axis phi "  << rect->axisPhi() ;
      printElem(cout,theEdmlElement);
    }
    PacRectDetType* type = PacTypeFactory::get(rect);
    assert(type);
    return new PacPlaneDetElem( type, (PacPlaneDetElem::orientation)rect->orientation(),
      Hep3Vector(rect->centerRho()*cos(rect->centerPhi()),rect->centerRho()*sin(rect->centerPhi()),rect->centerZ()),
      rect->axisPhi(),
      rect->name().c_str(), rect->id(), mdevs,
      rect->gap(), rect->overlap() );
  }
  if( theEdmlElement->type().find("Cone") != string::npos ) {

    const EdmlConeDetElement* cone = dynamic_cast<const EdmlConeDetElement*>(theEdmlElement);

    if(ErrLogging(routine)){
      cout << "Building " << cone->type() << " " << cone->name() << " " << cone->id() << " tan(angle) "
        << cone->tanth() << " z vertex " << cone->zvert();
      printElem(cout,theEdmlElement);
    }
    PacConeDetType* type = PacTypeFactory::get(cone);
    assert(type);
    return new PacConeDetElem( type, cone->tanth(), cone->zvert(), cone->name().c_str(), theEdmlElement->id(),
      mdevs, cone->gap(), cone->overlap() );

  }
  ErrMsg(fatal) << "PacElemFactory::get(): unsupported EDML detector type: '" << theEdmlElement->type() << "'" << endmsg;
  return 0;
}

void
PacElemFactory::printElem(ostream& str, const EdmlDetElement* theEdmlElement ) const {
  str << " measurement " << theEdmlElement->device();
  if(theEdmlElement->gap() != 0.0)
    str << " gap " << theEdmlElement->gap();
  if(theEdmlElement->overlap() != 0.0)
    str << " overlap " << theEdmlElement->overlap();
  return;
}


/*
==============================================================================
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain

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 condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNER.

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.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: rrossi $
//   Date:                $Date: 2007-03-06 10:30:34 $
//   Revision:            $Revision: 1.2 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "python/add_equation_systems_to_python.h"
#include "equation_systems/equation_system.h"
#include "includes/dof.h"
#include "spaces/ublas_space.h"

namespace Kratos
{

namespace Python
{
  void  AddEquationSystemsToPython()
  {
typedef UblasSpace<double, CompressedMatrix, Vector> SpaceType;
typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;
typedef EquationSystem<SpaceType,  LocalSpaceType, Dof<double> > EquationSystemType;

	void (EquationSystemType::*pointer_to_initalize)() = &EquationSystemType::Initialize;
	const EquationSystemType::SystemMatrixType& (EquationSystemType::*pointer_to_get_system_matrix)() const = &EquationSystemType::GetSystemMatrix;
	const EquationSystemType::SystemVectorType& (EquationSystemType::*pointer_to_get_results)() const = &EquationSystemType::GetResults;
	const EquationSystemType::SystemVectorType& (EquationSystemType::*pointer_to_get_rhs)() const = &EquationSystemType::GetRightHandSide;
	const EquationSystemType::SystemMatrixType& (EquationSystemType::*pointer_to_get_dirichlet_matrix)() const = &EquationSystemType::GetDirichletMatrix;

  using namespace boost::python;

  class_<EquationSystemType, EquationSystemType::Pointer>("EquationSystem")
		  //.def(init<std::string const&>())
		  .def(init<EquationSystemType::SizeType>())
		  .def("Initialize",pointer_to_initalize)
		  .def("ApplyDirichletConditions",&EquationSystemType::ApplyDirichletConditions)
		  .def("Size",&EquationSystemType::Size)
		  .def("DirichletSize",&EquationSystemType::DirichletSize)
		  .add_property("SystemMatrix", make_function(pointer_to_get_system_matrix, return_internal_reference<>()),&EquationSystemType::SetSystemMatrix)
		  .add_property("Results", make_function(pointer_to_get_results, return_internal_reference<>()),&EquationSystemType::SetResults)
		  .add_property("RightHandSide", make_function(pointer_to_get_rhs, return_internal_reference<>()),&EquationSystemType::SetRightHandSide)
		  .add_property("DirichletMatrix", make_function(pointer_to_get_dirichlet_matrix, return_internal_reference<>()))
		  //.def("",&EquationSystemType::)
		  .def(self_ns::str(self))
		  ;
  }

}  // namespace Python.

} // Namespace Kratos


#ifndef OVERLAP_ANALYSIS_DAGALIGNER_H
#define OVERLAP_ANALYSIS_DAGALIGNER_H

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

using boost::geometry::wkt;
using boost::geometry::model::point;
using boost::geometry::model::box;
using boost::geometry::cs::cartesian;
using boost::geometry::index::rtree;
using boost::geometry::index::quadratic;
using boost::geometry::index::intersects;

#include <experimental/filesystem>
#include <stdexcept>
#include <cmath>
#include <chrono>
#include <vector>
#include <iostream>
#include <memory>
#include <utility>
#include <queue>
#include <deque>
#include <iostream>

using std::experimental::filesystem::create_directories;
using std::experimental::filesystem::path;
using std::runtime_error;
using std::shared_ptr;
using std::to_string;
using std::vector;
using std::cerr;
using std::make_pair;
using std::pair;
using std::queue;
using std::deque;
using std::ostream;

using coord_t = pair<size_t,size_t>;

// Required data type for using the Boost spatial indexing data structure
using xy_point = point<size_t, 2, cartesian>;

// Represents an exact match found in a 2d alignment matrix, with x=ref, y=query, and its length along the diagonal
using xy_match = pair<xy_point, size_t>;

// Graph form of an exact match found in a 2d alignment matrix, with x=ref, y=query, and its length along the diagonal.
// x,y coords are used to compute gap (edge) scores later, based on some scoring scheme
class Node {
public:
    // Vector indexes which point to neighbor nodes
    vector <size_t> prev;
    vector <size_t> next;

    size_t x;
    size_t y;
    size_t length;
    int64_t score;

    Node(size_t x, size_t y, size_t length, size_t score);
    size_t get_x_stop();
    size_t get_y_stop();
};

ostream& operator<<(ostream& o, Node& n);

class Dag {
public:
    vector <Node> nodes;
    deque <size_t> alignment_path;
    size_t x_size;
    size_t y_size;
    size_t n_edges;
    size_t max_gap;

    int64_t match_score = 1;
    int64_t gap_open_score = -1;
    int64_t gap_score = -1;

    const size_t SOURCE_INDEX = 0;
    const size_t SINK_INDEX = 1;
    const size_t START_INDEX = 2;

    Dag(const vector<xy_match>& matches, size_t x_size, size_t y_size, size_t max_gap);
    Dag(const vector <pair <coord_t,size_t> >& matches, size_t x_size, size_t y_size, size_t max_gap);

    template <class T> void for_each_in_kahns_iteration(T function);
    void compute_alignment();

    void write_to_svg(path output_path, bool autoscale=true);

private:
    void step_alignment_forward(size_t i, vector<size_t>& max_prev);
    size_t get_x_diagonal(Node& n);
    size_t get_y_diagonal(Node& n);
};

//  Kahns algorithm (adapted to be non-destructive from https://en.wikipedia.org/wiki/Topological_sorting)
//  ------------------------------------------------------------------------------------------------------
//      L ← Empty list that will contain the sorted elements
//      S ← Set of all nodes with no incoming edge
//
//      while S is not empty do
//        remove a node n from S
//        add n to L
//        for each node m with an edge e from n to m do
//            remove edge e from the graph
//            if m has no other incoming edges then
//                  insert m into S
//
//      if graph has edges then
//        return error   (graph has at least one cycle)
//      else
//        return L   (a topologically sorted order)
//
template <class T> void Dag::for_each_in_kahns_iteration(T f){
    vector<size_t> edge_counts(nodes.size(),0);
    size_t edges_counted = 0;
    vector<size_t> l;
    queue<size_t> s;

    for (size_t i=0; i<nodes.size(); i++) {
        if (nodes[i].prev.empty()){
            s.push(i);
        }
    }

    while (not s.empty()){
        const auto i = s.front();
        s.pop();

        // Where the work is done, at this step it is guaranteed
        // that all previous paths have been visited
        f(i);
        l.emplace_back(i);

        for (auto j: nodes[i].next){
            edge_counts[j]++;
            edges_counted++;

            if (nodes[j].prev.size() == edge_counts[j]){
                s.push(j);
            }
        }
    }

    if (edges_counted != n_edges){
        throw runtime_error("ERROR: graph not a DAG:\n" \
                            "\tn_edges: " + to_string(n_edges) + ", edges iterated: " + to_string(edges_counted));
    }
}

ostream& operator<<(ostream& o, Dag& dag);

#endif //OVERLAP_ANALYSIS_DAGALIGNER_H


#include <iostream>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>

#include <besiq/method/stagewise_method.hpp>
#include <besiq/method/method.hpp>

#include "common_options.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-stagewise [OPTIONS] pairs genotype_plink_prefix";
const std::string DESCRIPTION = "A stage-wise test for genetic interactions.";

int
main(int argc, char *argv[])
{
    OptionParser parser = create_common_options( USAGE, DESCRIPTION, true );

    char const* const model_choices[] = { "binomial", "normal" };
    parser.add_option( "-m", "--model" ).choices( &model_choices[ 0 ], &model_choices[ 2 ] ).metavar( "model" ).help( "The model to use for the phenotype, 'binomial' or 'normal', default = 'binomial'." ).set_default( "binomial" );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }
    shared_ptr<common_options> parsed_data = parse_common_options( options, parser.args( ) );

    method_type *m = new stagewise_method( parsed_data->data, options[ "model" ] );

    run_method( *m, parsed_data->genotypes, *parsed_data->pairs, *parsed_data->result_file );

    delete m;

    return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     pivoting_object.cpp
* \author   Collin Johnson
*
* Definition of PivotingObject.
*/

#include <tracker/objects/pivoting_object.h>
#include <tracker/dynamic_object_visitor.h>
#include <tracker/object_state.h>
#include <utils/stub.h>
#include <boost/range/iterator_range.hpp>

namespace vulcan
{
namespace tracker
{

PivotingObject::PivotingObject(ObjectId id, int64_t timestamp, const FixedObjectModel& model)
: FixedObject(timestamp, model)
, id_(id)
{
}

Arc PivotingObject::arc(void) const
{
    return model().estimatedArc();
}

std::unique_ptr<DynamicObject> PivotingObject::clone(void) const
{
    return std::unique_ptr<DynamicObject>(new PivotingObject(*this));
}

void PivotingObject::accept(DynamicObjectVisitor& visitor) const
{
    visitor.visitPivotingObject(*this);
}

velocity_t PivotingObject::estimateVelocity(int64_t timestamp) const
{
    PRINT_STUB("PivotingObject::estimateVelocity");

    return velocity_t(0.0f, 0.0f);

//     const int64_t kVelocityTimeWindowUs = 200000;
//
//     // Find where the first saved state is within the window for which to estimate the velocity
//     auto windowBeginIt = std::find_if(beginRecentTrajectory(), endRecentTrajectory(),
//                                       [timestamp](const object_state_t& state)
//                                       {
//                                           return timestamp - kVelocityTimeWindowUs < state.timestamp;
//                                       });
//
//     // If no states exist yet, then no velocity
//     if(windowBeginIt == endRecentTrajectory())
//     {
//         std::cout << "Didn't detect a state before " << (timestamp - kVelocityTimeWindowUs) << '\n';
//         return velocity_t(0.0f, 0.0f);
//     }
//
//     // Calculate the total angle subtended during the time window
//     int64_t totalTime  = 0;
//     double  totalAngle = 0.0;
//
//     auto fixedPosition = model().fixedPosition();
//     auto prevAngle     = angle_to_point(fixedPosition, Position(windowBeginIt->x, windowBeginIt->y));
//     auto prevTime      = state_.timestamp;
//
//     for(auto& state : boost::make_iterator_range(windowBeginIt+1, endRecentTrajectory()))
//     {
//         auto angle = angle_to_point(fixedPosition, Position(state.x, state.y));
//
//         totalTime  += state.timestamp - prevTime;
//         totalAngle += angle_diff(angle, prevAngle);
//
//         prevAngle = angle;
//         prevTime  = state.timestamp;
//     }
//
//     std::cout << "Totals: Time:" << totalTime << " Angle:" << totalAngle << " Final angle:" << prevAngle << '\n';
//
//     // If no time has progressed, then not moving
//     if(totalTime == 0)
//     {
//         return velocity_t(0.0f, 0.0f);
//     }
//
//     // Otherwise, the total angle moved over time is the magnitude of the velocity.
//     // The direction of the velocity points tangent to the current angle of the object, which is stored in prevAngle
//     // The sign of totalAngle determines the direction of the velocity
//     // v = angVel * radius
//     double magnitude = (totalAngle / (totalTime / 1000000.0)) * model().estimatedArc().radius();
//     double direction = prevAngle + std::copysign(M_PI_2, totalAngle);
//
//     return velocity_t(std::cos(direction) * magnitude, std::sin(direction) * magnitude);
}

} // namespace tracker
} // namespace vulcan


#ifndef OSRM_GUIDANCE_TOOLKIT_HPP_
#define OSRM_GUIDANCE_TOOLKIT_HPP_

#include "util/bearing.hpp"
#include "util/coordinate.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/guidance/toolkit.hpp"
#include "util/guidance/turn_lanes.hpp"
#include "util/typedefs.hpp"

#include "extractor/compressed_edge_container.hpp"
#include "extractor/query_node.hpp"
#include "extractor/suffix_table.hpp"

#include "extractor/guidance/classification_data.hpp"
#include "extractor/guidance/discrete_angle.hpp"
#include "extractor/guidance/intersection.hpp"
#include "extractor/guidance/turn_instruction.hpp"

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <map>
#include <string>
#include <unordered_map>
#include <utility>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/functional/hash.hpp>

namespace osrm
{
namespace extractor
{
namespace guidance
{

using util::guidance::LaneTupelIdPair;
using LaneDataIdMap = std::unordered_map<LaneTupelIdPair, LaneDataID, boost::hash<LaneTupelIdPair>>;

using util::guidance::angularDeviation;
using util::guidance::entersRoundabout;
using util::guidance::leavesRoundabout;

namespace detail
{
const constexpr double DESIRED_SEGMENT_LENGTH = 10.0;
const constexpr bool shiftable_ccw[] = {false, true, true, false, false, true, true, false};
const constexpr bool shiftable_cw[] = {false, false, true, true, false, false, true, true};
const constexpr std::uint8_t modifier_bounds[detail::num_direction_modifiers] = {
    0, 36, 93, 121, 136, 163, 220, 255};

const constexpr double discrete_angle_step_size = 360. / 24;

template <typename IteratorType>
util::Coordinate
getCoordinateFromCompressedRange(util::Coordinate current_coordinate,
                                 const IteratorType compressed_geometry_begin,
                                 const IteratorType compressed_geometry_end,
                                 const util::Coordinate final_coordinate,
                                 const std::vector<extractor::QueryNode> &query_nodes)
{
    const auto extractCoordinateFromNode =
        [](const extractor::QueryNode &node) -> util::Coordinate {
        return {node.lon, node.lat};
    };
    double distance_to_current_coordinate = 0;
    double distance_to_next_coordinate = 0;

    // get the length that is missing from the current segment to reach DESIRED_SEGMENT_LENGTH
    const auto getFactor = [](const double first_distance, const double second_distance) {
        BOOST_ASSERT(first_distance < detail::DESIRED_SEGMENT_LENGTH);
        double segment_length = second_distance - first_distance;
        BOOST_ASSERT(segment_length > 0);
        BOOST_ASSERT(second_distance >= detail::DESIRED_SEGMENT_LENGTH);
        double missing_distance = detail::DESIRED_SEGMENT_LENGTH - first_distance;
        return std::max(0., std::min(missing_distance / segment_length, 1.0));
    };

    for (auto compressed_geometry_itr = compressed_geometry_begin;
         compressed_geometry_itr != compressed_geometry_end;
         ++compressed_geometry_itr)
    {
        const auto next_coordinate =
            extractCoordinateFromNode(query_nodes[compressed_geometry_itr->node_id]);
        distance_to_next_coordinate =
            distance_to_current_coordinate +
            util::coordinate_calculation::haversineDistance(current_coordinate, next_coordinate);

        // reached point where coordinates switch between
        if (distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH)
            return util::coordinate_calculation::interpolateLinear(
                getFactor(distance_to_current_coordinate, distance_to_next_coordinate),
                current_coordinate,
                next_coordinate);

        // prepare for next iteration
        current_coordinate = next_coordinate;
        distance_to_current_coordinate = distance_to_next_coordinate;
    }

    distance_to_next_coordinate =
        distance_to_current_coordinate +
        util::coordinate_calculation::haversineDistance(current_coordinate, final_coordinate);

    // reached point where coordinates switch between
    if (distance_to_current_coordinate < detail::DESIRED_SEGMENT_LENGTH &&
        distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH)
        return util::coordinate_calculation::interpolateLinear(
            getFactor(distance_to_current_coordinate, distance_to_next_coordinate),
            current_coordinate,
            final_coordinate);
    else
        return final_coordinate;
}
} // namespace detail

// Finds a (potentially interpolated) coordinate that is DESIRED_SEGMENT_LENGTH away
// from the start of an edge
inline util::Coordinate
getRepresentativeCoordinate(const NodeID from_node,
                            const NodeID to_node,
                            const EdgeID via_edge_id,
                            const bool traverse_in_reverse,
                            const extractor::CompressedEdgeContainer &compressed_geometries,
                            const std::vector<extractor::QueryNode> &query_nodes)
{
    const auto extractCoordinateFromNode =
        [](const extractor::QueryNode &node) -> util::Coordinate {
        return {node.lon, node.lat};
    };

    // Uncompressed roads are simple, return the coordinate at the end
    if (!compressed_geometries.HasEntryForID(via_edge_id))
    {
        return extractCoordinateFromNode(traverse_in_reverse ? query_nodes[from_node]
                                                             : query_nodes[to_node]);
    }
    else
    {
        const auto &geometry = compressed_geometries.GetBucketReference(via_edge_id);

        const auto base_node_id = (traverse_in_reverse) ? to_node : from_node;
        const auto base_coordinate = extractCoordinateFromNode(query_nodes[base_node_id]);

        const auto final_node = (traverse_in_reverse) ? from_node : to_node;
        const auto final_coordinate = extractCoordinateFromNode(query_nodes[final_node]);

        if (traverse_in_reverse)
            return detail::getCoordinateFromCompressedRange(
                base_coordinate, geometry.rbegin(), geometry.rend(), final_coordinate, query_nodes);
        else
            return detail::getCoordinateFromCompressedRange(
                base_coordinate, geometry.begin(), geometry.end(), final_coordinate, query_nodes);
    }
}

// shift an instruction around the degree circle in CCW order
inline DirectionModifier::Enum forcedShiftCCW(const DirectionModifier::Enum modifier)
{
    return static_cast<DirectionModifier::Enum>((static_cast<std::uint32_t>(modifier) + 1) %
                                                detail::num_direction_modifiers);
}

inline DirectionModifier::Enum shiftCCW(const DirectionModifier::Enum modifier)
{
    if (detail::shiftable_ccw[static_cast<int>(modifier)])
        return forcedShiftCCW(modifier);
    else
        return modifier;
}

// shift an instruction around the degree circle in CW order
inline DirectionModifier::Enum forcedShiftCW(const DirectionModifier::Enum modifier)
{
    return static_cast<DirectionModifier::Enum>(
        (static_cast<std::uint32_t>(modifier) + detail::num_direction_modifiers - 1) %
        detail::num_direction_modifiers);
}

inline DirectionModifier::Enum shiftCW(const DirectionModifier::Enum modifier)
{
    if (detail::shiftable_cw[static_cast<int>(modifier)])
        return forcedShiftCW(modifier);
    else
        return modifier;
}

inline bool isBasic(const TurnType::Enum type)
{
    return type == TurnType::Turn || type == TurnType::EndOfRoad;
}

inline bool isUturn(const TurnInstruction instruction)
{
    return isBasic(instruction.type) && instruction.direction_modifier == DirectionModifier::UTurn;
}

inline bool resolve(TurnInstruction &to_resolve, const TurnInstruction neighbor, bool resolve_cw)
{
    const auto shifted_turn = resolve_cw ? shiftCW(to_resolve.direction_modifier)
                                         : shiftCCW(to_resolve.direction_modifier);
    if (shifted_turn == neighbor.direction_modifier ||
        shifted_turn == to_resolve.direction_modifier)
        return false;

    to_resolve.direction_modifier = shifted_turn;
    return true;
}

inline bool resolveTransitive(TurnInstruction &first,
                              TurnInstruction &second,
                              const TurnInstruction third,
                              bool resolve_cw)
{
    if (resolve(second, third, resolve_cw))
    {
        first.direction_modifier =
            resolve_cw ? shiftCW(first.direction_modifier) : shiftCCW(first.direction_modifier);
        return true;
    }
    return false;
}

inline bool isSlightTurn(const TurnInstruction turn)
{
    return (isBasic(turn.type) || turn.type == TurnType::NoTurn) &&
           (turn.direction_modifier == DirectionModifier::Straight ||
            turn.direction_modifier == DirectionModifier::SlightRight ||
            turn.direction_modifier == DirectionModifier::SlightLeft);
}

inline bool isSlightModifier(const DirectionModifier::Enum direction_modifier)
{
    return (direction_modifier == DirectionModifier::Straight ||
            direction_modifier == DirectionModifier::SlightRight ||
            direction_modifier == DirectionModifier::SlightLeft);
}

inline bool isSharpTurn(const TurnInstruction turn)
{
    return isBasic(turn.type) && (turn.direction_modifier == DirectionModifier::SharpLeft ||
                                  turn.direction_modifier == DirectionModifier::SharpRight);
}

inline bool isStraight(const TurnInstruction turn)
{
    return (isBasic(turn.type) || turn.type == TurnType::NoTurn) &&
           turn.direction_modifier == DirectionModifier::Straight;
}

inline bool isConflict(const TurnInstruction first, const TurnInstruction second)
{
    return (first.type == second.type && first.direction_modifier == second.direction_modifier) ||
           (isStraight(first) && isStraight(second));
}

inline DiscreteAngle discretizeAngle(const double angle)
{
    BOOST_ASSERT(angle >= 0. && angle <= 360.);
    return DiscreteAngle(static_cast<std::uint8_t>(
        (angle + 0.5 * detail::discrete_angle_step_size) / detail::discrete_angle_step_size));
}

inline double angleFromDiscreteAngle(const DiscreteAngle angle)
{
    return static_cast<double>(angle) * detail::discrete_angle_step_size +
           0.5 * detail::discrete_angle_step_size;
}

inline double getAngularPenalty(const double angle, DirectionModifier::Enum modifier)
{
    // these are not aligned with getTurnDirection but represent an ideal center
    const double center[] = {0, 45, 90, 135, 180, 225, 270, 315};
    return angularDeviation(center[static_cast<int>(modifier)], angle);
}

inline double getTurnConfidence(const double angle, TurnInstruction instruction)
{

    // special handling of U-Turns and Roundabout
    if (!isBasic(instruction.type) || instruction.direction_modifier == DirectionModifier::UTurn)
        return 1.0;

    const double deviations[] = {0, 45, 50, 30, 20, 30, 50, 45};
    const double difference = getAngularPenalty(angle, instruction.direction_modifier);
    const double max_deviation = deviations[static_cast<int>(instruction.direction_modifier)];
    return 1.0 - (difference / max_deviation) * (difference / max_deviation);
}

inline bool canBeSuppressed(const TurnType::Enum type)
{
    if (type == TurnType::Turn)
        return true;
    return false;
}

inline bool isLowPriorityRoadClass(const FunctionalRoadClass road_class)
{
    return road_class == FunctionalRoadClass::LOW_PRIORITY_ROAD ||
           road_class == FunctionalRoadClass::SERVICE;
}

inline bool isDistinct(const DirectionModifier::Enum first, const DirectionModifier::Enum second)
{
    if ((first + 1) % detail::num_direction_modifiers == second)
        return false;

    if ((second + 1) % detail::num_direction_modifiers == first)
        return false;

    return true;
}

inline std::pair<std::string, std::string> getPrefixAndSuffix(const std::string &data)
{
    const auto suffix_pos = data.find_last_of(' ');
    if (suffix_pos == std::string::npos)
        return {};

    const auto prefix_pos = data.find_first_of(' ');
    auto result = std::make_pair(data.substr(0, prefix_pos), data.substr(suffix_pos + 1));
    boost::to_lower(result.first);
    boost::to_lower(result.second);
    return result;
}

inline bool requiresNameAnnounced(const std::string &from,
                                  const std::string &to,
                                  const SuffixTable &suffix_table)
{
    // first is empty and the second is not
    if (from.empty() && !to.empty())
        return true;

    // FIXME, handle in profile to begin with?
    // this uses the encoding of references in the profile, which is very BAD
    // Input for this function should be a struct separating streetname, suffix (e.g. road,
    // boulevard, North, West ...), and a list of references
    std::string from_name;
    std::string from_ref;
    std::string to_name;
    std::string to_ref;

    // Split from the format "{name} ({ref})" -> name, ref
    auto split = [](const std::string &name, std::string &out_name, std::string &out_ref) {
        const auto ref_begin = name.find_first_of('(');
        if (ref_begin != std::string::npos)
        {
            if (ref_begin != 0)
                out_name = name.substr(0, ref_begin - 1);
            const auto ref_end = name.find_first_of(')');
            out_ref = name.substr(ref_begin + 1, ref_end - ref_begin - 1);
        }
        else
        {
            out_name = name;
        }
    };

    split(from, from_name, from_ref);
    split(to, to_name, to_ref);

    // check similarity of names
    const auto names_are_empty = from_name.empty() && to_name.empty();
    const auto name_is_contained =
        boost::starts_with(from_name, to_name) || boost::starts_with(to_name, from_name);

    const auto checkForPrefixOrSuffixChange =
        [](const std::string &first, const std::string &second, const SuffixTable &suffix_table) {

            const auto first_prefix_and_suffixes = getPrefixAndSuffix(first);
            const auto second_prefix_and_suffixes = getPrefixAndSuffix(second);
            // reverse strings, get suffices and reverse them to get prefixes
            const auto checkTable = [&](const std::string str) {
                return str.empty() || suffix_table.isSuffix(str);
            };

            const bool is_prefix_change = [&]() -> bool {
                if (!checkTable(first_prefix_and_suffixes.first))
                    return false;
                if (!checkTable(first_prefix_and_suffixes.first))
                    return false;
                return !first.compare(first_prefix_and_suffixes.first.length(),
                                      std::string::npos,
                                      second,
                                      second_prefix_and_suffixes.first.length(),
                                      std::string::npos);
            }();

            const bool is_suffix_change = [&]() -> bool {
                if (!checkTable(first_prefix_and_suffixes.second))
                    return false;
                if (!checkTable(first_prefix_and_suffixes.second))
                    return false;
                return !first.compare(0,
                                      first.length() - first_prefix_and_suffixes.second.length(),
                                      second,
                                      0,
                                      second.length() - second_prefix_and_suffixes.second.length());
            }();

            return is_prefix_change || is_suffix_change;
        };

    const auto is_suffix_change = checkForPrefixOrSuffixChange(from_name, to_name, suffix_table);
    const auto names_are_equal = from_name == to_name || name_is_contained || is_suffix_change;
    const auto name_is_removed = !from_name.empty() && to_name.empty();
    // references are contained in one another
    const auto refs_are_empty = from_ref.empty() && to_ref.empty();
    const auto ref_is_contained =
        from_ref.empty() || to_ref.empty() ||
        (from_ref.find(to_ref) != std::string::npos || to_ref.find(from_ref) != std::string::npos);
    const auto ref_is_removed = !from_ref.empty() && to_ref.empty();

    const auto obvious_change =
        (names_are_empty && refs_are_empty) || (names_are_equal && ref_is_contained) ||
        (names_are_equal && refs_are_empty) || (ref_is_contained && name_is_removed) ||
        (names_are_equal && ref_is_removed) || is_suffix_change;

    return !obvious_change;
}

inline int getPriority(const FunctionalRoadClass road_class)
{
    // The road priorities indicate which roads can bee seen as more or less equal.
    // They are used in Fork-Discovery. Possibly should be moved to profiles post v5?
    // A fork can happen between road types that are at most 1 priority apart from each other
    const constexpr int road_priority[] = {
        10, 0, 10, 2, 10, 4, 10, 6, 10, 8, 10, 11, 10, 12, 10, 14};
    return road_priority[static_cast<int>(road_class)];
}

inline bool canBeSeenAsFork(const FunctionalRoadClass first, const FunctionalRoadClass second)
{
    // forks require similar road categories
    // Based on the priorities assigned above, we can set forks only if the road priorities match
    // closely.
    // Potentially we could include features like number of lanes here and others?
    // Should also be moved to profiles
    return std::abs(getPriority(first) - getPriority(second)) <= 1;
}

// To simplify handling of Left/Right hand turns, we can mirror turns and write an intersection
// handler only for one side. The mirror function turns a left-hand turn in a equivalent right-hand
// turn and vice versa.
inline ConnectedRoad mirror(ConnectedRoad road)
{
    const constexpr DirectionModifier::Enum mirrored_modifiers[] = {DirectionModifier::UTurn,
                                                                    DirectionModifier::SharpLeft,
                                                                    DirectionModifier::Left,
                                                                    DirectionModifier::SlightLeft,
                                                                    DirectionModifier::Straight,
                                                                    DirectionModifier::SlightRight,
                                                                    DirectionModifier::Right,
                                                                    DirectionModifier::SharpRight};

    if (angularDeviation(road.turn.angle, 0) > std::numeric_limits<double>::epsilon())
    {
        road.turn.angle = 360 - road.turn.angle;
        road.turn.instruction.direction_modifier =
            mirrored_modifiers[road.turn.instruction.direction_modifier];
    }
    return road;
}

inline bool hasRoundaboutType(const TurnInstruction instruction)
{
    using namespace extractor::guidance::TurnType;
    const constexpr TurnType::Enum valid_types[] = {TurnType::EnterRoundabout,
                                                    TurnType::EnterAndExitRoundabout,
                                                    TurnType::EnterRotary,
                                                    TurnType::EnterAndExitRotary,
                                                    TurnType::EnterRoundaboutIntersection,
                                                    TurnType::EnterAndExitRoundaboutIntersection,
                                                    TurnType::EnterRoundaboutAtExit,
                                                    TurnType::ExitRoundabout,
                                                    TurnType::EnterRotaryAtExit,
                                                    TurnType::ExitRotary,
                                                    TurnType::EnterRoundaboutIntersectionAtExit,
                                                    TurnType::ExitRoundaboutIntersection,
                                                    TurnType::StayOnRoundabout};
    const auto valid_end = valid_types + 13;
    return std::find(valid_types, valid_end, instruction.type) != valid_end;
}

// Public service vehicle lanes and similar can introduce additional lanes into the lane string that
// are not specifically marked for left/right turns. This function can be used from the profile to
// trim the lane string appropriately
//
// left|throught|
// in combination with lanes:psv:forward=1
// will be corrected to left|throught, since the final lane is not drivable.
// This is in contrast to a situation with lanes:psv:forward=0 (or not set) where left|through|
// represents left|through|through
inline std::string
trimLaneString(std::string lane_string, std::int32_t count_left, std::int32_t count_right)
{
    if (count_left)
    {
        bool sane = count_left < static_cast<std::int32_t>(lane_string.size());
        for (std::int32_t i = 0; i < count_left; ++i)
            // this is adjusted for our fake pipe. The moment cucumber can handle multiple escaped
            // pipes, the '&' part can be removed
            if (lane_string[i] != '|' && lane_string[i] != '&')
            {
                sane = false;
                break;
            }

        if (sane)
        {
            lane_string.erase(lane_string.begin(), lane_string.begin() + count_left);
        }
    }
    if (count_right)
    {
        bool sane = count_right < static_cast<std::int32_t>(lane_string.size());
        for (auto itr = lane_string.rbegin();
             itr != lane_string.rend() && itr != lane_string.rbegin() + count_right;
             ++itr)
        {
            if (*itr != '|' && *itr != '&')
            {
                sane = false;
                break;
            }
        }
        if (sane)
            lane_string.resize(lane_string.size() - count_right);
    }
    return lane_string;
}

} // namespace guidance
} // namespace extractor
} // namespace osrm

#endif // OSRM_GUIDANCE_TOOLKIT_HPP_


/*
 * Planning.cpp
 *
 *  Created on: 30/07/2012
 *      Author: thomas
 */

#include "Planning.hpp"

#include <boost/tokenizer.hpp>

#include <rw/common.hpp>
#include <rw/kinematics/Kinematics.hpp>
#include <rw/pathplanning/PlannerConstraint.hpp>
#include <rwlibs/pathplanners/rrt/RRTQToQPlanner.hpp>
#include <rwlibs/pathoptimization/pathlength/PathLengthOptimizer.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyFactory.hpp>

using namespace rw::common;
using namespace rw::math;
using namespace rw::models;
using namespace rw::kinematics;
using namespace rw::pathplanning;
using namespace rw::trajectory;
using namespace rw::proximity;
using namespace rwlibs::task;
using namespace rwlibs::pathplanners;
using namespace rwlibs::pathoptimization;
using namespace rwlibs::proximitystrategies;

Planning::Planning(WorkCell::Ptr wc, Device::Ptr device, Device::Ptr gripper, Frame* objectFrame, Transform3D<> tcpTgoal, GraspDB::Ptr db):
	_db(db),
	_device(device),
	_gripper(gripper),
	_gripperBase(wc->findFrame<MovableFrame>(gripper->getBase()->getName())),
	_objectFrame(objectFrame),
	_targetTtcp(inverse(tcpTgoal)),
	_strategy(RANDOM)
{
	_detector = ownedPtr(new CollisionDetector(wc,ProximityStrategyFactory::makeDefaultCollisionStrategy()));
	_metric = MetricFactory::makeEuclidean<Q>();
	_iksolver = ownedPtr(new ClosedFormURSolver(_device,wc->getDefaultState()));
}

Planning::~Planning() {
}

void Planning::setStrategy(Strategy strategy) {
	_strategy = strategy;
}

Planning::Status Planning::inverseKin(std::vector<Q>& sol, const Transform3D<> &target, const State &state) const {
	PlannerConstraint constraint = PlannerConstraint::make(_detector,_device,state);

	Q curQ = _device->getQ(state);
	std::vector<Q> solutions = _iksolver->solve(target,state);
	if (solutions.size() <= 0)
		return INVKIN_FAIL;
	while(solutions.size() > 0) {
		std::vector<Q>::iterator bestSolution;
		double bestDist = -1;
		for (std::vector<Q>::iterator it = solutions.begin(); it != solutions.end(); it++) {
			State diffState = state;
			_device->setQ(*it,diffState);
			double dist = (*it-curQ).norm2();
			if (bestDist == -1) {
				bestSolution = it;
				bestDist = dist;
			} else if (dist < bestDist) {
				bestSolution = it;
				bestDist = dist;
			}
		}
		if (!constraint.inCollision(*bestSolution)) {
			sol.push_back(*bestSolution);
		}
		solutions.erase(bestSolution);
	}
	if (sol.size()>0)
		return SUCCESS;
	else
		return COLLISION;
}

bool Planning::compFct(std::pair<unsigned int,double> i,std::pair<unsigned int,double> j) {
	return i.second > j.second;
}

Planning::Status Planning::getPath(std::pair<unsigned int, QPath> &res, const State &state, double approach, std::vector<unsigned int> blacklist) const {
	if (_detector->inCollision(state))
		return COLLISION_INITIALLY;

	const std::vector<GraspSubTask> tasks = _db->getTasks();
	if (tasks.size() <= 0)
		return NO_TASK;

	std::vector<unsigned int> ind;
	for (unsigned int i = 0; i < tasks.size(); i++)
		ind.push_back(i);
	if (_strategy == RANDOM) {
		std::random_shuffle(ind.begin(),ind.end());
	} else if (_strategy == BEST_QUALITY) {
		// Order after quality descending
		std::vector<std::pair<unsigned int,double> > sortList;
		for (unsigned int i = 0; i < ind.size(); i++) {
			std::pair<unsigned int,double> pair;
			pair.first = ind[i];
			pair.second = tasks[i].targets[0].result->qualityAfterLifting[0];
			sortList.push_back(pair);
		}
		std::sort(sortList.begin(),sortList.end(),Planning::compFct);
		// Create vector of ids for each different quality
		std::vector<std::pair<double,std::vector<unsigned int> > > list;
		double lastQual = -100;
		for (unsigned int i = 0; i < sortList.size(); i++) {
			unsigned int id = sortList[i].first;
			double qual = sortList[i].second;
			if (qual!=lastQual) {
				std::pair<double,std::vector<unsigned int> > pair;
				pair.first = qual;
				std::vector<unsigned int> vec;
				pair.second = vec;
				list.push_back(pair);
				lastQual = qual;
			}
			list[list.size()-1].second.push_back(id);
		}
		// Randomize ids
		for (unsigned int i = 0; i < list.size(); i++) {
			std::random_shuffle(list[i].second.begin(),list[i].second.end());
		}
		// Linearize list
		ind.clear();
		for (unsigned int i = 0; i < list.size(); i++) {
			std::vector<unsigned int> ids = list[i].second;
			for (unsigned int j = 0; j < ids.size(); j++) {
				ind.push_back(ids[j]);
			}
		}
	}

	Transform3D<> baseTobject = Kinematics::frameTframe(_device->getBase(),_objectFrame,state);
	for (std::vector<unsigned int>::iterator it = ind.begin(); it != ind.end(); it++) {
		bool blacklisted = false;
		for (std::vector<unsigned int>::iterator bit = blacklist.begin(); bit != blacklist.end(); bit++) {
			if ((*bit) == (*it))
				blacklisted = true;
		}
		if (!blacklisted) {
			GraspSubTask task = tasks[*it];
			Transform3D<> target = baseTobject*task.getTargets()[0].pose*_targetTtcp;

			Planning::Status status;
			if (approach > 0.) {
				Vector3D<> approachPos = target.P()-target.R()*Vector3D<>::z()*approach;
				Transform3D<> approachTarget(approachPos,target.R());
				QPath pathA;
				status = getPath(pathA, approachTarget, task.getOpenQ(), state);
				if (status == SUCCESS) {
					State approachState = state;
					_device->setQ(pathA[pathA.size()-1],approachState);
					QPath pathB;
					status = getPath(pathB, target, task.getOpenQ(), approachState);
					if (status == SUCCESS) {
						res.first = *it;
						QPath comb;
						for (QPath::iterator itA = pathA.begin(); itA != pathA.end(); itA++)
							comb.push_back(*itA);
						bool first = true;
						for (QPath::iterator itB = pathB.begin(); itB != pathB.end(); itB++) {
							if (!first)
								comb.push_back(*itB);
							first = false;
						}
						res.second = comb;
						return SUCCESS;
					}
				}
			} else {
				QPath path;
				status = getPath(path, target, task.getOpenQ(), state);
				if (status == SUCCESS) {
					res.first = *it;
					res.second = path;
					return SUCCESS;
				}
			}
		}
	}

	return NO_PATH_FOUND;
}

Planning::Status Planning::getPath(std::pair<unsigned int, QPath> &res, unsigned int id, const State &state, double approach) const {
	if (_detector->inCollision(state))
		return COLLISION_INITIALLY;

	std::vector<GraspSubTask> tasks = _db->getTasks();
	if (id >= tasks.size())
		return NO_TASK;
	GraspSubTask task = tasks[id];

	Transform3D<> baseTobject = Kinematics::frameTframe(_device->getBase(),_objectFrame,state);
	Transform3D<> target = baseTobject*task.getTargets()[0].pose*_targetTtcp;

	Planning::Status status;
	if (approach > 0.) {
		Vector3D<> approachPos = target.P()-target.R()*Vector3D<>::z()*approach;
		Transform3D<> approachTarget(approachPos,target.R());
		QPath pathA;
		status = getPath(pathA, approachTarget, task.getOpenQ(), state);
		if (status == SUCCESS) {
			State approachState = state;
			_device->setQ(pathA[pathA.size()-1],approachState);
			QPath pathB;
			status = getPath(pathB, target, task.getOpenQ(), approachState);
			if (status == SUCCESS) {
				res.first = id;
				QPath comb;
				for (QPath::iterator itA = pathA.begin(); itA != pathA.end(); itA++)
					comb.push_back(*itA);
				bool first = true;
				for (QPath::iterator itB = pathB.begin(); itB != pathB.end(); itB++) {
					if (!first)
						comb.push_back(*itB);
					first = false;
				}
				res.second = comb;
				return SUCCESS;
			}
		}
	} else {
		QPath path;
		status = getPath(path, target,task.getOpenQ(),state);
		if (status == SUCCESS) {
			res.first = id;
			res.second = path;
			return SUCCESS;
		}
	}

	return NO_PATH_FOUND;
}

Planning::Status Planning::getPath(QPath &res, const rw::math::Q qTo, const rw::math::Q qFrom, const rw::kinematics::State &state) const {
	State fromState = state;
	_device->setQ(qTo,fromState);
	State toState = state;
	_device->setQ(qTo,toState);
	if (_detector->inCollision(fromState))
		return COLLISION_INITIALLY;
	if (_detector->inCollision(toState))
		return COLLISION_END;

	QPath path;
	PlannerConstraint constraint = PlannerConstraint::make(_detector,_device,state);
	QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(_device),constraint.getQConstraintPtr());
	QToQPlanner::Ptr planner = RRTQToQPlanner::makeConnect(constraint,sampler,_metric,0.1);

	if (planner->query(qFrom,qTo,path,10.)) {
		PathLengthOptimizer optimizer(constraint, _metric);
		path = optimizer.pathPruning(path);
		if (path.size() > 2)
			path = optimizer.partialShortCut(path,10,0.1,0.25);
		path = optimizer.pathPruning(path);
		res.clear();
		for (unsigned int i = 0; i < path.size(); i++) {
			res.push_back(path[i]);
		}
		return SUCCESS;
	} else {
		return NO_PATH_FOUND;
	}
}

Planning::Status Planning::getPath(QPath &res, const Transform3D<> pose, const Q gripper, const State &state) const {
	std::string deviceName = _device->getName();
	std::string gripperName = _gripper->getName();

	if (_detector->inCollision(state))
		return COLLISION_INITIALLY;

	State gripState(state);
	_gripperBase->moveTo(Kinematics::worldTframe(_device->getBase(),state)*pose,gripState);
	CollisionDetector::QueryResult* query = new CollisionDetector::QueryResult();
	bool handColl = _detector->inCollision(gripState,query);
	FramePairSet fps = query->collidingFrames;
	handColl = false;
    boost::char_separator<char> sep(".");
	for (FramePairSet::iterator it = fps.begin(); it != fps.end(); it++) {
	    boost::tokenizer< boost::char_separator<char> > tokens1((*it).first->getName(), sep);
	    boost::tokenizer< boost::char_separator<char> > tokens2((*it).second->getName(), sep);
	    std::string dev1 = *tokens1.begin();
	    std::string dev2 = *tokens2.begin();
		if (!(!dev1.compare(deviceName) && !dev2.compare(gripperName)) && !(!dev1.compare(gripperName) && !dev2.compare(deviceName)))
			handColl = true;
	}
	delete query;
	if (handColl) {
		return COLLISION_END;
	} else {
		std::vector<Q> qs;
		State invState = state;
		_gripper->setQ(gripper,invState);
		Planning::Status invKinSuc = inverseKin(qs,pose,invState);
		if (invKinSuc != SUCCESS)
			return INVKIN_FAIL;
		else {
			for (unsigned int i = 0; i < qs.size(); i++) {
				State tempState(state);
				_gripper->setQ(gripper,tempState);
				_device->setQ(qs[i],tempState);
				if (!_detector->inCollision(tempState)) {
					QPath path;
					PlannerConstraint constraint = PlannerConstraint::make(_detector,_device,tempState);
					QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(_device),constraint.getQConstraintPtr());
					QToQPlanner::Ptr planner = RRTQToQPlanner::makeConnect(constraint,sampler,_metric,0.1);

					Q start = _device->getQ(state);
					Q end = qs[i];
					if (planner->query(start,end,path,10.)) {
						PathLengthOptimizer optimizer(constraint, _metric);
						path = optimizer.pathPruning(path);
						if (path.size() > 2)
							path = optimizer.partialShortCut(path,10,0.1,0.25);
						path = optimizer.pathPruning(path);
						res.clear();
						for (unsigned int i = 0; i < path.size(); i++) {
							res.push_back(path[i]);
						}
						return SUCCESS;
					}
				}
			}
		}
	}

	return NO_PATH_FOUND;
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/aggregation/staticcreditxvacalculator.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace std;
using namespace QuantLib;

using namespace boost::accumulators;

namespace ore {
namespace analytics {

StaticCreditXvaCalculator::StaticCreditXvaCalculator(
    const boost::shared_ptr<Portfolio> portfolio, const boost::shared_ptr<Market> market,
    const string& configuration, const string& baseCurrency, const string& dvaName,
    const string& fvaBorrowingCurve, const string& fvaLendingCurve,
    const bool applyDynamicInitialMargin,
    const boost::shared_ptr<DynamicInitialMarginCalculator> dimCalculator,
    const boost::shared_ptr<NPVCube> tradeExposureCube,
    const boost::shared_ptr<NPVCube> nettingSetExposureCube,
    const Size tradeEpeIndex, const Size tradeEneIndex,
    const Size nettingSetEpeIndex, const Size nettingSetEneIndex,
    const bool flipViewXVA, const string& flipViewBorrowingCurvePostfix, const string& flipViewLendingCurvePostfix)
    : ValueAdjustmentCalculator(portfolio, market, configuration, baseCurrency, dvaName,
                                fvaBorrowingCurve, fvaLendingCurve, applyDynamicInitialMargin,
                                dimCalculator, tradeExposureCube, nettingSetExposureCube, tradeEpeIndex, tradeEneIndex,
                                nettingSetEpeIndex, nettingSetEneIndex,
                                flipViewXVA, flipViewBorrowingCurvePostfix, flipViewLendingCurvePostfix) {
    for (Size i = 0; i < dates().size(); i++) {
        dateIndexMap_.emplace(dates()[i], i);
    }
}

const Real StaticCreditXvaCalculator::calculateCvaIncrement(
    const string& tid, const string& cid, const Date& d0, const Date& d1, const Real& rr) {
    Handle<DefaultProbabilityTermStructure> dts = market_->defaultCurve(cid, configuration_);
    QL_REQUIRE(!dts.empty(), "Default curve missing for counterparty " << cid);
    Real increment = 0.0;
    Real s0 = dts->survivalProbability(d0);
    Real s1 = dts->survivalProbability(d1);
    Real epe = tradeExposureCube_->get(tid, d1, 0, tradeEpeIndex_);
    increment = (1.0 - rr) * (s0 - s1) * epe;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateDvaIncrement(
    const string& tid, const Date& d0, const Date& d1, const Real& rr) {
    Handle<DefaultProbabilityTermStructure> dts = market_->defaultCurve(dvaName_, configuration_);
    QL_REQUIRE(!dts.empty(), "Default curve missing for counterparty " << dvaName_);
    Real increment = 0.0;
    Real s0 = dts->survivalProbability(d0);
    Real s1 = dts->survivalProbability(d1);
    Real ene = tradeExposureCube_->get(tid, d1, 0, tradeEneIndex_);
    increment = (1.0 - rr) * (s0 - s1) * ene;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateNettingSetCvaIncrement(
    const string& nid, const string& cid, const Date& d0, const Date& d1, const Real& rr) {
    Handle<DefaultProbabilityTermStructure> dts = market_->defaultCurve(cid, configuration_);
    QL_REQUIRE(!dts.empty(), "Default curve missing for counterparty " << cid);
    Real increment = 0.0;
    Real s0 = dts->survivalProbability(d0);
    Real s1 = dts->survivalProbability(d1);
    Real epe = nettingSetExposureCube_->get(nid, d1, 0, nettingSetEpeIndex_);
    increment = (1.0 - rr) * (s0 - s1) * epe;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateNettingSetDvaIncrement(
    const string& nid, const Date& d0, const Date& d1, const Real& rr) {
    Handle<DefaultProbabilityTermStructure> dts = market_->defaultCurve(dvaName_, configuration_);
    QL_REQUIRE(!dts.empty(), "Default curve missing for counterparty " << dvaName_);
    Real increment = 0.0;
    Real s0 = dts->survivalProbability(d0);
    Real s1 = dts->survivalProbability(d1);
    Real ene = nettingSetExposureCube_->get(nid, d1, 0, nettingSetEneIndex_);
    increment = (1.0 - rr) * (s0 - s1) * ene;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateFbaIncrement(
    const string& tid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Handle<DefaultProbabilityTermStructure> dts_cid;
    Handle<DefaultProbabilityTermStructure> dts_dvaName;
    if (cid != "") {
        dts_cid = market_->defaultCurve(cid, configuration_);
        QL_REQUIRE(!dts_cid.empty(), "Default curve missing for counterparty " << cid);
    }
    if (dvaName != "") {
        dts_dvaName = market_->defaultCurve(dvaName, configuration_);
        QL_REQUIRE(!dts_dvaName.empty(), "Default curve missing for counterparty " << cid);
    }

    Real increment = 0.0;
    Real s0 = cid == "" ? 1.0 : dts_cid->survivalProbability(d0);
    Real s1 = dvaName == "" ? 1.0 : dts_dvaName->survivalProbability(d0);
    Real ene = tradeExposureCube_->get(tid, d1, 0, tradeEneIndex_);
    increment = s0 * s1 * ene * dcf;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateFcaIncrement(
    const string& tid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Handle<DefaultProbabilityTermStructure> dts_cid;
    Handle<DefaultProbabilityTermStructure> dts_dvaName;
    if (cid != "") {
        dts_cid = market_->defaultCurve(cid, configuration_);
        QL_REQUIRE(!dts_cid.empty(), "Default curve missing for counterparty " << cid);
    }
    if (dvaName != "") {
        dts_dvaName = market_->defaultCurve(dvaName, configuration_);
        QL_REQUIRE(!dts_dvaName.empty(), "Default curve missing for counterparty " << cid);
    }

    Real increment = 0.0;
    Real s0 = cid == "" ? 1.0 : dts_cid->survivalProbability(d0);
    Real s1 = dvaName == "" ? 1.0 : dts_dvaName->survivalProbability(d0);
    Real epe = tradeExposureCube_->get(tid, d1, 0, tradeEpeIndex_);
    increment = s0 * s1 * epe * dcf;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateNettingSetFbaIncrement(
    const string& nid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Handle<DefaultProbabilityTermStructure> dts_cid;
    Handle<DefaultProbabilityTermStructure> dts_dvaName;
    if (cid != "") {
        dts_cid = market_->defaultCurve(cid, configuration_);
        QL_REQUIRE(!dts_cid.empty(), "Default curve missing for counterparty " << cid);
    }
    if (dvaName != "") {
        dts_dvaName = market_->defaultCurve(dvaName, configuration_);
        QL_REQUIRE(!dts_dvaName.empty(), "Default curve missing for counterparty " << cid);
    }

    Real increment = 0.0;
    Real s0 = cid == "" ? 1.0 : dts_cid->survivalProbability(d0);
    Real s1 = dvaName == "" ? 1.0 : dts_dvaName->survivalProbability(d0);
    Real ene = nettingSetExposureCube_->get(nid, d1, 0, nettingSetEneIndex_);
    increment = s0 * s1 * ene * dcf;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateNettingSetFcaIncrement(
    const string& nid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Handle<DefaultProbabilityTermStructure> dts_cid;
    Handle<DefaultProbabilityTermStructure> dts_dvaName;
    if (cid != "") {
        dts_cid = market_->defaultCurve(cid, configuration_);
        QL_REQUIRE(!dts_cid.empty(), "Default curve missing for counterparty " << cid);
    }
    if (dvaName != "") {
        dts_dvaName = market_->defaultCurve(dvaName, configuration_);
        QL_REQUIRE(!dts_dvaName.empty(), "Default curve missing for counterparty " << cid);
    }

    Real increment = 0.0;
    Real s0 = cid == "" ? 1.0 : dts_cid->survivalProbability(d0);
    Real s1 = dvaName == "" ? 1.0 : dts_dvaName->survivalProbability(d0);
    Real epe = nettingSetExposureCube_->get(nid, d1, 0, nettingSetEpeIndex_);
    increment = s0 * s1 * epe * dcf;
    return increment;
}

const Real StaticCreditXvaCalculator::calculateNettingSetMvaIncrement(
    const string& nid, const string& cid, const Date& d0, const Date& d1, const Real& dcf) {
    Handle<DefaultProbabilityTermStructure> dts_cid = market_->defaultCurve(cid, configuration_);
    QL_REQUIRE(cid == "" || !dts_cid.empty(), "Default curve missing for counterparty " << cid);
    Handle<DefaultProbabilityTermStructure> dts_dvaName = market_->defaultCurve(dvaName_, configuration_);
    QL_REQUIRE(dvaName_ == "" || !dts_dvaName.empty(), "Default curve missing for counterparty " << dvaName_);

    Real increment = 0.0;
    Real s0 = cid == "" ? 1.0 : dts_cid->survivalProbability(d0);
    Real s1 = dvaName_ == "" ? 1.0 : dts_dvaName->survivalProbability(d0);
    increment = s0 * s1 * dimCalculator_->expectedIM(nid)[dateIndexMap_[d1]] * dcf;
    return increment;
}

} // namespace analytics
} // namespace ore


#include "itkdetails/ImageUtility.hpp"

#include <glm/glm.hpp>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/epsilon.hpp>

#include <vnl/vnl_matrix_fixed.h>

#include <itkGDCMImageIO.h>
#include <itkFileTools.h>

#include <boost/filesystem.hpp>

#include <iostream>
#include <streambuf>
#include <tuple>
#include <stdio.h>

namespace itkdetails
{

namespace utility
{

std::pair< std::string, bool >
getSpiralCodeFromDirectionMatrix( const vnl_matrix_fixed< double, 3, 3 >& matrix )
{
    /// @note Fourth character is null-terminating character
    char spiralCode[4] = "???";

    /// LPS positive
    static const char codes[3][2] = {
        { 'R', 'L'},
        { 'A', 'P'},
        { 'I', 'S'} };

    bool isOblique = false;

    for ( uint8_t col = 0; col < 3; ++col )
    {
        /// @internal Get the direction cosine for voxel direction \c col
        const vnl_vector_fixed< double, 3 > dirCos = matrix.get_column( col );

        const double dirAbsMax = dirCos.inf_norm();

        for ( uint8_t row = 0; row < 3; ++row )
        {
            /// @note In Convert3D, the assignment of \c dsgn to 1 or 0 is flipped
            const int dsgn = ( dirCos[row] > 0 ) ? 1 : 0;
            const double dabs = std::abs( dirCos[row] );

            if ( glm::epsilonEqual( 1.0, dabs, glm::epsilon<double>() ) )
            {
                spiralCode[col] = codes[row][dsgn];
            }
            else if ( glm::epsilonEqual( dirAbsMax, dabs, glm::epsilon<double>() ) )
            {
                isOblique = true;
                spiralCode[col] = codes[row][dsgn];
            }
        }
    }

    return std::make_pair( std::string{ spiralCode }, isOblique );
}

namespace dicom
{

/**
 * @brief dicomSeriesSearch
 * @param directory
 * @return
 */
/// @internal Identify from a given directory the set of file names that belong together
/// to the same volumetric image. \c GDCMSeriesFileNames will explore the directory and
/// will generate a sequence of filenames for DICOM files for one study/series.
/// The \c GDCMSeriesFileNames object first identifies the list of DICOM series
/// present in the given directory.
///
/// @internal We use additional DICOM information (tag 0008 0021 : DA 1 Series Date)
/// to sub-refine each seriesto distinguish unique volumes within the directory.
/// This is useful, for example, if a DICOM device assigns the same SeriesID to a scout
/// scan and its 3D volume; by using additional DICOM information the scout scan will not be
/// included as part of the 3D volume.
///
/// By default @code{SetUseSeriesDetails(true)} will use the following DICOM tags to sub-refine
/// a set of files into multiple series:
///
/// @begin{description}
/// @item[0020 0011] Series Number
/// @item[0018 0024] Sequence Name
/// @item[0018 0050] Slice Thickness
/// @item[0028 0010] Rows
/// @item[0028 0011] Columns
/// @end{description}
std::pair< std::vector< std::string >, NameGeneratorType::Pointer >
seriesSearch( const char* directory )
{
    /// @internal Directory in which to search for the series
//        const std::string seriesDirectory =
//                ( ::itksys::SystemTools::FileIsDirectory( fileName ) )
//                ? fileName
//                : ::itksys::SystemTools::GetParentDirectory( fileName );

    const std::string seriesDirectory =
            ( boost::filesystem::is_directory( directory ) )
            ? directory
            : boost::filesystem::path( directory ).parent_path().c_str();

    typename NameGeneratorType::Pointer nameGenerator = NameGeneratorType::New();
    nameGenerator->SetUseSeriesDetails( true );
    nameGenerator->AddSeriesRestriction( "0008|0021" ); //!< Series date
    nameGenerator->SetDirectory( seriesDirectory );

    std::vector< std::string > seriesUIDs;

    try
    {
        seriesUIDs = nameGenerator->GetSeriesUIDs();

#if 0
        if ( seriesUIDs.empty() )
        {
            std::cerr << "No DICOM series in directory '"
                      << seriesDirectory << "'" << std::endl;
        }
        else
        {
            std::cout << "The directory '" << seriesDirectory
                      << "' contains the following DICOM series: " << std::endl;

            for ( const auto& i : seriesUIDs )
            {
                std::cout << "\t" << i << std::endl;
            }
            std::cout << std::endl;
        }
#endif
    }
    catch ( const itk::ExceptionObject& e )
    {
        std::cerr << "Exception when generating DICOM series UIDs: "
                  << e.what() << std::endl;

        nameGenerator = ITK_NULLPTR;
    }
    catch ( ... )
    {
        std::cerr << "Exception when generating DICOM series UIDs." << std::endl;

        nameGenerator = ITK_NULLPTR;
    }

    return std::make_pair( seriesUIDs, nameGenerator );
}

/**
  * @return Filenames associated with series
  */
::itk::ImageIOBase::Pointer
createDicomImageIO( const char* firstFileName )
{
    try
    {
        const ::itk::GDCMImageIO::Pointer dicomIO = ::itk::GDCMImageIO::New();

        if ( dicomIO.IsNull() )
        {
            std::cerr << "GDCM cannot create the DICOM I/O object." << std::endl;
            return ITK_NULLPTR;
        }

        dicomIO->SetFileName( firstFileName );
        dicomIO->ReadImageInformation();

        /// @internal Return pointer to base class (\c ImageIOBase)
        const ::itk::ImageIOBase::Pointer imageIO = dicomIO.GetPointer();

        return imageIO;
    }
    catch ( const ::itk::ExceptionObject& e )
    {
        std::cerr << "Exception while creating GDCMImageIO: " << e.what() << std::endl;

        return ITK_NULLPTR;
    }
    catch ( ... )
    {
        std::cerr << "Exception while creating GDCMImageIO." << std::endl;

        return ITK_NULLPTR;
    }
}

} // namespace dicom

/**
 * Delegate creation of I/O object to factory function
 */
::itk::ImageIOBase::Pointer
createStandardImageIO( const char* fileName )
{
    class redirect_stderror
    {
    public:
        redirect_stderror()
            : m_descriptor( dup( fileno(stderr) ) )
        {
            freopen( "/tmp/stderr_log.txt","a", stderr );
        }

        ~redirect_stderror()
        {
            dup2( m_descriptor, fileno(stderr) );
            close( m_descriptor );
        }

        int m_descriptor;
    } redirector;

    try
    {
        const ::itk::ImageIOBase::Pointer imageIO =
                ::itk::ImageIOFactory::CreateImageIO(
                    fileName, ::itk::ImageIOFactory::ReadMode );

        if ( imageIO.IsNull() )
        {
            /// @internal None of the registered ImageIO classes can read the file
            std::cerr << "ITK image I/O factory could not create the I/O object for image '"
                      << fileName << "'" << std::endl;

            return ITK_NULLPTR;
        }

        imageIO->SetFileName( fileName );
        imageIO->ReadImageInformation();

        return imageIO;
    }
    catch ( const ::itk::ExceptionObject& e )
    {
        std::cerr << "Exception while creating ImageIOBase: " << e.what() << std::endl;

        return ITK_NULLPTR;
    }
    catch ( ... )
    {
        std::cerr << "Exception while creating ImageIOBase." << std::endl;

        return ITK_NULLPTR;
    }

    return ITK_NULLPTR;
}

/**
 * @brief Logic needed to determine the image type.
 * Could be extended to support other types, like TIFF and JPEG.
 *
 * @param path
 *
 * @return
 */
ImageFileType getImageFileType( const char* path )
{
    const ::itk::ImageIOBase::Pointer imageIO = createStandardImageIO( path );

    if ( imageIO.IsNotNull() )
    {
        /// @internal Path is a file that ITK's I/O factory can read
        return ImageFileType::SingleImage;
    }
    else
    {
        if ( ! dicom::seriesSearch( path ).first.empty() )
        {
            /// @internal The path is either a directory containing one or more
            /// DICOM series, or the path is a file whose parent directory
            /// contains one or more DICOM series
            return ImageFileType::DICOMSeries;
        }
    }

    return ImageFileType::Undefined;
}

} // namespace utility

} // namespace itkdetails

std::ostream& operator<< ( std::ostream& os, const ::itkdetails::utility::PixelStatistics<double>& stats )
{
    os << "Minimum: "   << stats.m_minimum << std::endl
       << "Maximum: "   << stats.m_maximum << std::endl
       << "Mean: "      << stats.m_mean << std::endl
       << "Std. dev: "  << stats.m_stdDeviation << std::endl
       << "Variance: "  << stats.m_variance << std::endl
       << "Sum: "       << stats.m_sum << std::endl
       << "Quartiles: " << stats.m_quantiles[25] << ", "
                        << stats.m_quantiles[50] << ", "
                        << stats.m_quantiles[75] << std::endl
       << std::endl;

    return os;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_CENTROID_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_CENTROID_HPP

#include <boost/geometry/algorithms/centroid.hpp>

#include <boost/geometry/extensions/nsphere/views/center_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace centroid
{

struct centroid_nsphere
{
    template<typename NSphere, typename PointCentroid, typename Strategy>
    static inline void apply(NSphere const& nsphere, PointCentroid& centroid,
            Strategy const&)
    {
        geometry::convert(center_view<const NSphere>(nsphere), centroid);
    }
};

}} // namespace detail::centroid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Geometry>
struct centroid<Geometry, nsphere_tag>
    : detail::centroid::centroid_nsphere
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_CENTROID_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_MaxwellFissionDistribution.hpp
//! \author Aaron Tumulak
//! \brief  Maxwell Fission distribution class declaration. Modified by Alex
//!         Robison to accommodate units.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_MAXWELLFISSION_DISTRIBUTION_HPP
#define UTILITY_MAXWELLFISSION_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/units/physical_dimensions/energy.hpp>
#include <boost/units/dimensionless_type.hpp>

// FRENSIE Includes
#include "Utility_OneDDistribution.hpp"
#include "Utility_ParameterListCompatibleObject.hpp"

namespace Utility{

/*! The unit-aware Maxwell fission distribution class
 * \ingroup one_d_distributions
 */
template<typename IndependentUnit, typename DependentUnit = void>
class UnitAwareMaxwellFissionDistribution : public UnitAwareOneDDistribution<IndependentUnit,DependentUnit>,
					    public ParameterListCompatibleObject<UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit> >
{

  // Only allow construction when the independent unit corresponds to energy
  RESTRICT_UNIT_TO_BOOST_DIMENSION( IndependentUnit, energy_dimension );

private:

  // The distribution multiplier unit traits typedef
  typedef UnitTraits<typename UnitTraits<DependentUnit>::template GetMultipliedUnitType<typename UnitTraits<IndependentUnit>::template GetUnitToPowerType<1,-2>::type>::type> DistMultiplierUnitTraits;

  // The distribution multiplier quantity type
  typedef typename DistMultiplierUnitTraits::template GetQuantityType<double>::type DistMultiplierQuantity;

  // The distribution normalization quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DistNormQuantity DistNormQuantity;

  // Typedef for QuantityTraits<double>
  typedef QuantityTraits<double> QT;

  // Typedef for QuantityTraits<IndepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::IndepQuantity> IQT;

  // Typedef for QuantityTraits<InverseIndepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity> IIQT;

  // Typedef for QuantityTraits<DepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DepQuantity> DQT;

  // Typedef for QuantityTraits<DistMultiplierQuantity>
  typedef QuantityTraits<DistMultiplierQuantity> DMQT;

public:

  //! This distribution type
  typedef UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit> ThisType;

  //! The independent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::IndepQuantity IndepQuantity;

  //! The inverse independent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity InverseIndepQuantity;

  //! The dependent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DepQuantity DepQuantity;

  //! Default Constructor
  UnitAwareMaxwellFissionDistribution(
			const IndepQuantity incident_energy = IQT::one(),
			const IndepQuantity nuclear_temperature = IQT::one(),
			const IndepQuantity restriction_energy = IQT::zero(),
			const double constant_multiplier = 1.0 );

  //! Constructor
  template<typename InputIndepQuantityA,
	   typename InputIndepQuantityB,
	   typename InputIndepQuantityC>
  UnitAwareMaxwellFissionDistribution(
				const InputIndepQuantityA incident_energy,
				const InputIndepQuantityB nuclear_temperature,
				const InputIndepQuantityC restriction_energy,
				const double constant_multiplier = 1.0 );

  //! Copy constructor
  template<typename InputIndepUnit, typename InputDepUnit>
  UnitAwareMaxwellFissionDistribution( const UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>& dist_instance );

  //! Construct distribution from a unitless dist. (potentially dangerous)
  static UnitAwareMaxwellFissionDistribution fromUnitlessDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_distribution );

  //! Assignment operator
  UnitAwareMaxwellFissionDistribution& operator=( const UnitAwareMaxwellFissionDistribution& dist_instance );

  //! Destructor
  ~UnitAwareMaxwellFissionDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  DepQuantity evaluate( const IndepQuantity indep_var_value ) const;

  //! Evaluate the PDF
  InverseIndepQuantity evaluatePDF( const IndepQuantity indep_var_value ) const;

  //! Return a random sample from the distribution
  IndepQuantity sample() const;

  //! Return a random sample from the distribution
  static IndepQuantity sample( const IndepQuantity incident_energy,
			       const IndepQuantity nuclear_temperature,
			       const IndepQuantity restriction_energy );

  //! Return a random sample from the distribution and record the number of trials
  IndepQuantity sampleAndRecordTrials( unsigned& trials ) const;

  //! Return a random sample from the corresponding CDF and record the number of trials
  static IndepQuantity sampleAndRecordTrials(
    const IndepQuantity incident_energy,
    const IndepQuantity nuclear_temperature,
    const IndepQuantity restriction_energy,
    unsigned& trials );

  //! Test if the distribution is continuous
  bool isContinuous() const;

  //! Return the upper bound of the distribution independent variable
  IndepQuantity getUpperBoundOfIndepVar() const;

  //! Return the lower bound of the distribution independent variable
  IndepQuantity getLowerBoundOfIndepVar() const;

  //! Return the distribution type
  OneDDistributionType getDistributionType() const;

  //! Method for placing the object in an output stream
  void toStream( std::ostream& os ) const;

  //! Method for initializing the object from an input stream
  void fromStream( std::istream& is );

  //! Method for testing if two objects are equivalent
  bool isEqual( const UnitAwareMaxwellFissionDistribution& other ) const;

protected:

  //! Copy constructor (copying from unitless distribution only)
  UnitAwareMaxwellFissionDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_dist_instance, int );

private:

  // Calculate the normalization constant of the distribution
  void calculateNormalizationConstant();

  // All possible instantiations are friends
  template<typename FriendIndepUnit, typename FriendDepUnit>
  friend class UnitAwareMaxwellFissionDistribution;

  // The distribution type
  static const OneDDistributionType distribution_type = MAXWELLFISSION_DISTRIBUTION;

  // The incident neutron energy of the distribution
  IndepQuantity d_incident_energy;

  // The nuclear temperature of the distribution
  IndepQuantity d_nuclear_temperature;

  // The restriction energy of the distribution
  IndepQuantity d_restriction_energy;

  // The distribution multiplier
  DistMultiplierQuantity d_multiplier;

  // The distribution normalization constant
  DistNormQuantity d_norm_constant;
};

/*! The Maxwell fission distribution (unit-agnostic)
 * \ingroup one_d_distributions
 */
typedef UnitAwareMaxwellFissionDistribution<void,void> MaxwellFissionDistribution;

} // end Utility namespace

namespace Teuchos{

/*! Type name traits specialization for the Utility::MaxwellFissionDistribution
 *
 * \details The name function will set the type name that must be used in
 * xml files.
 */
template<>
class TypeNameTraits<Utility::MaxwellFissionDistribution>
{
public:
  static std::string name()
  {
    return "Maxwell Fission Distribution";
  }
  static std::string concreteName(
				const Utility::MaxwellFissionDistribution& instance )
  {
    return name();
  }
};

/*! \brief Type name traits partial specialization for the
 * Utility::UnitAwareMaxwellFissionDistribution
 *
 * \details The name function will set the type name that must be used in
 * xml files.
 */
template<typename U,typename V>
class TypeNameTraits<Utility::UnitAwareMaxwellFissionDistribution<U,V> >
{
public:
  static std::string name()
  {
    return "Unit-Aware Maxwell Fission Distribution (" +
      Utility::UnitTraits<U>::symbol() + "," +
      Utility::UnitTraits<V>::symbol() + ")";
  }
  static std::string concreteName(
	    const Utility::UnitAwareMaxwellFissionDistribution<U,V>& instance )
  {
    return name();
  }
};

} // end Teuchos namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_MaxwellFissionDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_MAXWELLFISSION_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_MaxwellFissionDistribution.hpp
//---------------------------------------------------------------------------//


#ifndef __OBJECT_DETECTOR_IMPL_HPP__
#define __OBJECT_DETECTOR_IMPL_HPP__

#include <map>
#include <string>

#include <cvx/viz/scene/scene.hpp>
#include <cvx/viz/renderer/renderer.hpp>
#include <cvx/util/camera/camera.hpp>
#include <cvx/util/geometry/kdtree.hpp>
#include <cvx/util/misc/path.hpp>

#include <Eigen/Core>

#include "rf.hpp"
#include "detector.hpp"

typedef std::map<std::string, uint16_t> LabelMapType ;
typedef std::map<std::string, float> LabelProbMapType ;

using RendererPtr = std::shared_ptr<cvx::viz::Renderer> ;
using ScenePtr = cvx::viz::ScenePtr ;

struct ModelData {
    ScenePtr scene_ ;
    cvx::util::PointList3f cloud_ ;
    cvx::util::KDTree3 search_ ;
    Eigen::Vector3f center_, bmin_, bmax_ ;
    float diameter_ ;
    RendererPtr renderer_ ;
    Eigen::Matrix4f camera_ ;
};

class ObjectDetectorImpl {

public:

    ObjectDetectorImpl(const ObjectDetector::Parameters &params): params_(params) {}

    bool init(const std::string &rf_path, const Dataset &models) ;

    void detectAll(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cvx::util::PinholeCamera &cam,
                   std::vector<ObjectDetector::Result> &results) ;
    bool detect(const std::string &label, const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const cvx::util::PinholeCamera &cam,
                   Eigen::Matrix4f &pose, float &error) ;

    void draw_results(cv::Mat &img, const cvx::util::PinholeCamera &cam, const std::vector<ObjectDetector::Result> &results ) ;
    void draw_result(cv::Mat &img, const cvx::util::PinholeCamera &cam, const std::string &clabel, const Eigen::Matrix4f &pose, const cv::Vec3b &clr) ;

    void refine(const std::string &label, const cv::Mat &depth, const cvx::util::PinholeCamera &cam, Eigen::Matrix4f &pose, float &error) ;

protected:

    typedef std::vector< std::vector<RandomForest::Node *> > LeafList ;
    typedef std::vector< std::map<std::string, float> > LabelProbList ;

    void compute_label_probabilities(const LeafList &leafs, LabelProbList &probs) ;

    void show_classes(const Dataset &ds, const LabelProbList &probs) ;
    void show_coords(const Dataset &ds, const LeafList &leafs, uint t) ;

    void sample_pose_hypotheses(const std::string &clabel, const cvx::util::PinholeCamera &cam,
                                const Dataset &ds,
                                const LeafList &leafs,
                                const LabelProbList &probs,
                                std::vector<Eigen::Matrix4f> &poses) ;

    bool load_cloud(const std::string &fp, ModelData &data) ;
    bool load_camera(const std::string &dir, Eigen::Matrix4f &cam) ;

    float compute_energy(const std::string &clabel, const Eigen::Matrix4f &poses, const LeafList &leafs,
                        const LabelProbList &probs,
                        const cvx::util::PinholeCamera &cam, const Dataset &ds) ;

    void draw_bbox(cv::Mat &img, const cvx::util::PinholeCamera &cam, const std::string &clabel, const Eigen::Matrix4f &pose, const cv::Vec3b &clr) ;

    void find_pose_candidates(const std::string &clabel, const cvx::util::PinholeCamera &cam,
                              const Dataset &ds,
                              const LeafList &leafs,
                              const LabelProbList &probs,
                              std::vector<Eigen::Matrix4f> &poses,
                              std::vector<float> &errors) ;

    void refine_pose_candidates(const std::string &clabel, const cvx::util::PinholeCamera &cam,
                                const Dataset &ds,
                                const LeafList &leafs,
                                const LabelProbList &probs,
                                std::vector<Eigen::Matrix4f> &poses,
                                std::vector<float> &errors) ;

    cv::Mat render_mask(const std::string &clabel, const cvx::util::PinholeCamera &cam, const Eigen::Matrix4f &pose) ;
    float refine_pose_icp(const std::string &clabel, const cv::Mat &dim, const cvx::util::PinholeCamera &cam, const Eigen::Matrix4f &pose) ;

    RandomForest forest_ ;
    std::vector<std::string> labels_ ;
    std::map<std::string, ModelData> model_map_ ;
    ObjectDetector::Parameters params_ ;
    RNG g_ ;
};

#endif


#include <yawiel/colext/colext.hpp>
#include <yawiel/colext/eam/am/pmi.hpp>
#include <yawiel/colext/eam/am/dice.hpp>
#include <yawiel/colext/eam/am/chi_squared.hpp>
#include <yawiel/colext/eam/am/log_likelihood.hpp>
#include <yawiel/colext/eam/ep/g0.hpp>
#include <yawiel/colext/eam/ep/g1.hpp>
#include <yawiel/colext/eam/ep/g2.hpp>
#include <yawiel/colext/eam/ep/g3.hpp>
#include <yawiel/colext/eam/ep/g4.hpp>
#include <yawiel/colext/eam/ep/g5.hpp>
#include <boost/test/unit_test.hpp>
#include <unordered_map>
#include <map>

using namespace std;
using namespace yawiel;
using namespace yawiel::text;
using namespace yawiel::util;
using namespace yawiel::colext;

BOOST_AUTO_TEST_SUITE(ColextTest);

const std::string text1 = "Lorem Ipsum is simply dummy text of the printing "
    "and typesetting industry. Lorem Ipsum has been the industry's "
    "standard dummy text ever since the 1500s, when an unknown printer "
    "took a galley of type and scrambled it to make a type specimen book.";

BOOST_AUTO_TEST_CASE(BasicTestWithNormalization)
{
  Corpus<> corpus;
  corpus.LoadString(text1);
  Colext<G1<PMI<NGramCounter<>>>> colext;
  colext.LoadCorpus(corpus);
  unordered_map<vector<size_t>, double> scores;
  BOOST_REQUIRE_NO_THROW(colext.GetScores(scores, 3, true));
}

BOOST_AUTO_TEST_CASE(BasicTestWithoutNormalization)
{
  Corpus<> corpus;
  corpus.LoadString(text1);
  Colext<G1<PMI<NGramCounter<>>>> colext;
  colext.LoadCorpus(corpus);
  unordered_map<vector<size_t>, double> scores;
  BOOST_REQUIRE_NO_THROW(colext.GetScores(scores, 4));
}

BOOST_AUTO_TEST_CASE(BasicTestSortedScores)
{
  Corpus<> corpus;
  corpus.LoadString(text1);
  Colext<G1<PMI<NGramCounter<>>>> colext;
  colext.LoadCorpus(corpus);
  vector<pair<vector<size_t>, double>> scores;
  BOOST_REQUIRE_NO_THROW(colext.GetSortedScores(scores, 3, true));
}

BOOST_AUTO_TEST_CASE(CountsSerialization)
{
  std::string text = "Hi hello hi hello this is a a a test test a";
  unordered_map<vector<size_t>, double> scores1, scores2;
  Corpus<> corpus;
  corpus.LoadString(text);
  NGramCounter<> counter(std::move(corpus));

  // Compute model 1.
  Colext<G1<PMI<NGramCounter<>>>> colext1(&counter);
  colext1.GetScores(scores1, 2);
  colext1.SaveModel(std::string("../colext_model_1.txt"));

  // Compute model 2.
  Colext<G1<PMI<NGramCounter<>>>> colext2;
  colext2.LoadModel(std::string("../colext_model_1.txt"));
  colext2.GetScores(scores2, 2);

  // Check results are equal.
  for (const auto kv : scores1)
  {
    const auto it = scores2.find(kv.first);
    const bool ngramsEqual = kv.first == it->first;
    BOOST_REQUIRE_EQUAL(ngramsEqual, true);
    BOOST_REQUIRE_CLOSE(kv.second, it->second, 1e-10);
  }
}

BOOST_AUTO_TEST_SUITE_END();


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef PENNCNVSNPDATASINK_HPP
#define PENNCNVSNPDATASINK_HPP

#include <iostream>
#include <string>
#include <Eigen/Core>
#include "genfile/snp_data_utils.hpp"
#include "genfile/gen.hpp"
#include "genfile/SNPDataSink.hpp"

namespace genfile {
	// A SNPDataSink which writes its data in a format suitable for input
	// to PennCNV [1,2] or QuantiSNP [3,4].
	// [1] http://penncnv.openbioinformatics.org
	// [2] http://genome.cshlp.org/content/17/11/1665.short
	// [3] http://www.ncbi.nlm.nih.gov/pubmed/17341461
	// [4] https://sites.google.com/site/quantisnp/
	//
	class PennCNVSNPDataSink: public SNPDataSink
	{
	public:
		PennCNVSNPDataSink( std::string const& filename, double const threshhold = 0.9 ) ;
		SinkPos get_stream_pos() const ;
		std::string get_spec() const ;

	private:

		void set_sample_names_impl( std::size_t number_of_samples, SampleNameGetter ) ;
		void set_metadata_impl( Metadata const& ) {} ;
		void write_variant_data_impl(
			VariantIdentifyingData const& id_data,
			VariantDataReader& data_reader,
			Info const& info
		) ;
		void finalise_impl() {}

		operator bool() const ;
		std::ostream& stream() { return *m_stream_ptr ; }
		std::string const& filename() const { return m_filename ; }

	private:
		void setup( std::string const& filename ) ;

		std::string m_filename ;
		double const m_threshhold ;
		std::auto_ptr< std::ostream > m_stream_ptr ;
		Eigen::MatrixXd m_intensities ;
		Eigen::MatrixXd m_nonmissingness ;
		std::vector< std::size_t > m_genotypes ;
		std::vector< std::string > m_genotype_alleles ;
	} ;
}

#endif