/*
 * Collision sphere file
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#include "SceneGraph/Collision/spCollisionCapsule.hpp"
#include "SceneGraph/Collision/spCollisionSphere.hpp"
#include "SceneGraph/Collision/spCollisionBox.hpp"
#include "SceneGraph/Collision/spCollisionPlane.hpp"
#include "SceneGraph/Collision/spCollisionMesh.hpp"

#include <boost/foreach.hpp>

namespace sp
{
namespace scene
{

CollisionCapsule::CollisionCapsule(
    CollisionMaterial* Material, SceneNode* Node, f32 Radius, f32 Height) :
    CollisionLineBased(Material, Node, COLLISION_CAPSULE, Radius, Height)
{
}
CollisionCapsule::~CollisionCapsule()
{
}

s32 CollisionCapsule::getSupportFlags() const
{
    return COLLISIONSUPPORT_SPHERE | COLLISIONSUPPORT_CAPSULE | COLLISIONSUPPORT_BOX | COLLISIONSUPPORT_PLANE | COLLISIONSUPPORT_MESH;
}

bool CollisionCapsule::checkIntersection(const dim::line3df &Line, SIntersectionContact &Contact) const
{
    dim::vector3df PointP, PointQ;
    const dim::line3df CapsuleLine(getLine());

    /* Make an intersection test with both lines */
    const f32 DistanceSq = math::CollisionLibrary::getLineLineDistanceSq(CapsuleLine, Line, PointP, PointQ);

    if (DistanceSq < math::pow2(getRadius()))
    {
        Contact.Normal  = (PointQ - PointP).normalize();
        Contact.Point   = PointP + Contact.Normal * getRadius();
        Contact.Object  = this;
        return true;
    }

    return false;
}

bool CollisionCapsule::checkIntersection(const dim::line3df &Line, bool ExcludeCorners) const
{
    /* Make an intersection test with both lines */
    dim::vector3df PointP, PointQ;

    if (math::CollisionLibrary::getLineLineDistanceSq(getLine(), Line, PointP, PointQ) < math::pow2(getRadius()))
    {
        if (ExcludeCorners)
        {
            dim::vector3df Dir(PointQ - PointP);
            Dir.setLength(getRadius());
            return checkCornerExlusion(Line, PointP + Dir);
        }
        return true;
    }

    return false;
}

/*
 * ======= Private: =======
 */

bool CollisionCapsule::checkCollisionToSphere(const CollisionSphere* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(Rival->getPosition());
    const dim::line3df CapsuleLine(getLine());

    const f32 MaxRadius = Radius_ + Rival->getRadius();

    /* Get the closest point from this sphere to the capsule */
    const dim::vector3df ClosestPoint = CapsuleLine.getClosestPoint(SpherePos);

    /* Check if this object and the other collide with each other */
    if (math::getDistanceSq(SpherePos, ClosestPoint) < math::pow2(MaxRadius))
        return setupCollisionContact(ClosestPoint, SpherePos, MaxRadius, Rival->getRadius(), Contact);

    return false;
}

bool CollisionCapsule::checkCollisionToCapsule(const CollisionCapsule* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::vector3df SpherePos(Rival->getPosition());
    const dim::line3df CapsuleLine(getLine());

    const f32 MaxRadius = Radius_ + Rival->getRadius();

    /* Get the closest points between this and the rival capsule */
    dim::vector3df PointP, PointQ;
    const f32 DistanceSq = math::CollisionLibrary::getLineLineDistanceSq(getLine(), Rival->getLine(), PointP, PointQ);

    /* Check if this object and the other collide with each other */
    if (DistanceSq < math::pow2(MaxRadius))
        return setupCollisionContact(PointP, PointQ, MaxRadius, Rival->getRadius(), Contact);

    return false;
}

bool CollisionCapsule::checkCollisionToBox(const CollisionBox* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::matrix4f Mat(Rival->getTransformation().getPositionRotationMatrix());
    const dim::matrix4f InvMat(Mat.getInverse());

    const dim::aabbox3df Box(Rival->getBox().getScaled(Rival->getScale()));
    const dim::line3df CapsuleLine(getLine());
    const dim::line3df CapsuleLineInv(
        InvMat * CapsuleLine.Start, InvMat * CapsuleLine.End
    );

    /* Get the closest point from this capsule and the box */
    if (Box.isPointInside(CapsuleLineInv.Start) || Box.isPointInside(CapsuleLineInv.End))
        return false;

    const dim::line3df Line = math::CollisionLibrary::getClosestLine(Box, CapsuleLineInv);

    /* Check if this object and the other collide with each other */
    if (math::getDistanceSq(Line.Start, Line.End) < math::pow2(getRadius()))
    {
        Contact.Point = Mat * Line.Start;

        /* Compute normal and impact together to avoid calling square-root twice */
        Contact.Normal = (Mat * Line.End) - Contact.Point;
        Contact.Impact = Contact.Normal.getLength();

        if (Contact.Impact < math::ROUNDING_ERROR)
            return false;

        Contact.Normal *= (1.0f / Contact.Impact);
        Contact.Impact = getRadius() - Contact.Impact;

        return true;
    }

    return false;
}

bool CollisionCapsule::checkCollisionToPlane(const CollisionPlane* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Store transformation */
    const dim::line3df CapsuleLine(getLine());
    const dim::plane3df RivalPlane(
        Rival->getTransformation().getPositionRotationMatrix() * Rival->getPlane()
    );

    /* Check if this object and the other collide with each other */
    const f32 DistA = RivalPlane.getPointDistance(CapsuleLine.Start);
    const f32 DistB = RivalPlane.getPointDistance(CapsuleLine.End);

    if ( DistA > 0.0f && DistB > 0.0f && ( DistA < getRadius() || DistB < getRadius() ) )
    {
        Contact.Normal  = RivalPlane.Normal;
        Contact.Point   = Contact.Normal;

        if (DistA <= DistB)
        {
            Contact.Point *= -DistA;
            Contact.Point += CapsuleLine.Start;

            Contact.Impact = getRadius() - DistA;
        }
        else
        {
            Contact.Point *= -DistB;
            Contact.Point += CapsuleLine.End;

            Contact.Impact = getRadius() - DistB;
        }

        return true;
    }

    return false;
}

bool CollisionCapsule::checkCollisionToMesh(const CollisionMesh* Rival, SCollisionContact &Contact) const
{
    if (!Rival)
        return false;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return false;

    /* Store transformation */
    const dim::line3df CapsuleLine(getLine());
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    const dim::line3df CapsuleLineInv(
        RivalMatInv * CapsuleLine.Start, RivalMatInv * CapsuleLine.End
    );

    f32 DistanceSq = math::pow2(getRadius());
    SCollisionFace* ClosestFace = 0;
    dim::vector3df ClosestPoint;

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, CapsuleLineInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangles of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, CapsuleLineInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::line3df CurClosestLine(
                math::CollisionLibrary::getClosestLine(RivalMat * Face->Triangle, CapsuleLine)
            );

            /* Check if this is a potentially new closest face */
            const f32 CurDistSq = math::getDistanceSq(CurClosestLine.Start, CurClosestLine.End);

            if (CurDistSq < DistanceSq)
            {
                /* Store link to new closest face */
                DistanceSq      = CurDistSq;
                ClosestPoint    = CurClosestLine.Start;
                ClosestFace     = Face;
            }
        }
    }

    /* Check if a collision has been detected */
    if (ClosestFace)
    {
        Contact.Normal  = (RivalMat * ClosestFace->Triangle).getNormal();
        Contact.Point   = ClosestPoint;
        Contact.Face    = ClosestFace;
        return true;
    }

    return false;
}

bool CollisionCapsule::checkAnyCollisionToMesh(const CollisionMesh* Rival) const
{
    if (!Rival)
        return false;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return false;

    /* Store transformation */
    const dim::line3df CapsuleLine(getLine());
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    const dim::line3df CapsuleLineInv(
        RivalMatInv * CapsuleLine.Start, RivalMatInv * CapsuleLine.End
    );

    const f32 RadiusSq = math::pow2(getRadius());

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, CapsuleLineInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangles of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, CapsuleLineInv))
                continue;

            /* Make sphere-triangle collision test */
            const dim::line3df CurClosestLine(
                math::CollisionLibrary::getClosestLine(Face->Triangle, CapsuleLineInv)
            );

            /* Check if the first collision has been detected and return on succeed */
            if (math::getDistanceSq(CurClosestLine.Start, CurClosestLine.End) < RadiusSq)
                return true;
        }
    }

    return false;
}

void CollisionCapsule::performCollisionResolvingToSphere(const CollisionSphere* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToSphere(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionCapsule::performCollisionResolvingToCapsule(const CollisionCapsule* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToCapsule(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionCapsule::performCollisionResolvingToBox(const CollisionBox* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToBox(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionCapsule::performCollisionResolvingToPlane(const CollisionPlane* Rival)
{
    SCollisionContact Contact;
    if (checkCollisionToPlane(Rival, Contact))
        performDetectedContact(Rival, Contact);
}

void CollisionCapsule::performCollisionResolvingToMesh(const CollisionMesh* Rival)
{
    if (!Rival)
        return;

    /* Check if rival mesh has a tree-hierarchy */
    KDTreeNode* RootTreeNode = Rival->getRootTreeNode();

    if (!RootTreeNode)
        return;

    /* Store transformation */
    const video::EFaceTypes CollFace(Rival->getCollFace());

    const dim::matrix4f RivalMat(Rival->getTransformation());
    const dim::matrix4f RivalMatInv(RivalMat.getInverse());

    dim::line3df CapsuleLine(getLine());
    dim::line3df CapsuleLineInv(
        RivalMatInv * CapsuleLine.Start, RivalMatInv * CapsuleLine.End
    );

    dim::line3df ClosestLine;
    const f32 RadiusSq = math::pow2(getRadius());

    #ifndef _DEB_NEW_KDTREE_
    std::map<SCollisionFace*, bool> FaceMap, EdgeFaceMap;
    #endif

    /* Get tree node list */
    std::list<const TreeNode*> TreeNodeList;

    RootTreeNode->findLeafList(
        TreeNodeList, CapsuleLineInv, (RivalMatInv.getScale() * getRadius()).getMax()
    );

    /* Check collision with triangle faces of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle face */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (FaceMap.find(Face) != FaceMap.end())
                continue;

            FaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, CapsuleLineInv))
                continue;

            /* Make capsule-triangle collision test */
            const dim::triangle3df Triangle(RivalMat * Face->Triangle);

            if (!math::CollisionLibrary::getClosestLineStraight(Triangle, CapsuleLine, ClosestLine))
                continue;

            /* Check if this is a potentially new closest face */
            if (math::getDistanceSq(ClosestLine.Start, ClosestLine.End) < RadiusSq)
            {
                /* Perform detected collision contact */
                SCollisionContact Contact;
                {
                    Contact.Point       = ClosestLine.Start;
                    Contact.Normal      = Triangle.getNormal();
                    Contact.Impact      = getRadius() - (ClosestLine.End - Contact.Point).getLength();
                    Contact.Triangle    = Triangle;
                    Contact.Face        = Face;
                }
                performDetectedContact(Rival, Contact);

                if (getFlags() & COLLISIONFLAG_RESOLVE)
                {
                    /* Update capsule position */
                    CapsuleLine             = getLine();
                    CapsuleLineInv.Start    = RivalMatInv * CapsuleLine.Start;
                    CapsuleLineInv.End      = RivalMatInv * CapsuleLine.End;
                }
            }
        }
    }

    /* Check collision with triangle edges of each tree-node */
    foreach (const TreeNode* Node, TreeNodeList)
    {
        /* Get tree node data */
        CollisionMesh::TreeNodeDataType* TreeNodeData = static_cast<CollisionMesh::TreeNodeDataType*>(Node->getUserData());

        if (!TreeNodeData)
            continue;

        /* Check collision with each triangle edge */
        #ifndef _DEB_NEW_KDTREE_
        foreach (SCollisionFace* Face, *TreeNodeData)
        #else
        foreach (SCollisionFace &NodeFace, *TreeNodeData)
        #endif
        {
            #ifndef _DEB_NEW_KDTREE_
            /* Check for unique usage */
            if (EdgeFaceMap.find(Face) != EdgeFaceMap.end())
                continue;

            EdgeFaceMap[Face] = true;
            #else
            SCollisionFace* Face = &NodeFace;
            #endif

            /* Check for face-culling */
            if (Face->isBackFaceCulling(CollFace, CapsuleLineInv))
                continue;

            /* Make capsule-triangle collision test */
            const dim::triangle3df Triangle(RivalMat * Face->Triangle);

            ClosestLine = math::CollisionLibrary::getClosestLine(Triangle, CapsuleLine);

            /* Check if this is a potentially new closest face */
            if (math::getDistanceSq(ClosestLine.Start, ClosestLine.End) < RadiusSq)
            {
                /* Perform detected collision contact */
                SCollisionContact Contact;
                {
                    Contact.Point = ClosestLine.Start;

                    Contact.Normal = ClosestLine.End;
                    Contact.Normal -= ClosestLine.Start;
                    Contact.Normal.normalize();

                    Contact.Impact      = getRadius() - (ClosestLine.End - Contact.Point).getLength();
                    Contact.Triangle    = Triangle;
                    Contact.Face        = Face;
                }
                performDetectedContact(Rival, Contact);

                if (getFlags() & COLLISIONFLAG_RESOLVE)
                {
                    /* Update capsule position */
                    CapsuleLine             = getLine();
                    CapsuleLineInv.Start    = RivalMatInv * CapsuleLine.Start;
                    CapsuleLineInv.End      = RivalMatInv * CapsuleLine.End;
                }
            }
        }
    }
}

bool CollisionCapsule::setupCollisionContact(
    const dim::vector3df &PointP, const dim::vector3df &PointQ,
    f32 MaxRadius, f32 RivalRadius, SCollisionContact &Contact) const
{
    /* Compute normal and impact together to avoid calling square-root twice */
    Contact.Normal = PointP;
    Contact.Normal -= PointQ;

    Contact.Impact = Contact.Normal.getLength();

    if (Contact.Impact < math::ROUNDING_ERROR)
        return false;

    Contact.Normal *= (1.0f / Contact.Impact);
    Contact.Impact = MaxRadius - Contact.Impact;

    Contact.Point = Contact.Normal;
    Contact.Point *= RivalRadius;
    Contact.Point += PointQ;

    return true;
}

} // /namespace scene

} // /namespace sp

// ================================================================================


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include "arc_utilities/eigen_helpers.hpp"
#include "arc_utilities/eigen_helpers_conversions.hpp"
#include "arc_utilities/pretty_print.hpp"
#include "arc_utilities/voxel_grid.hpp"
#include "arc_utilities/simple_rrt_planner.hpp"
#include "uncertainty_planning_core/simple_pid_controller.hpp"
#include "uncertainty_planning_core/simple_uncertainty_models.hpp"
#include "uncertainty_planning_core/uncertainty_contact_planning.hpp"
#include "uncertainty_planning_core/simple_robot_models.hpp"
#include "uncertainty_planning_core/simple_samplers.hpp"
#include "uncertainty_planning_core/uncertainty_planning_core.hpp"
#include "fast_kinematic_simulator/fast_kinematic_simulator.hpp"
#include "fast_kinematic_simulator/simulator_environment_builder.hpp"
#include "uncertainty_planning_examples/config_common.hpp"

#ifndef SE3_COMMON_CONFIG_HPP
#define SE3_COMMON_CONFIG_HPP

namespace se3_common_config
{
    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetDefaultOptions()
    {
        uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options;
        options.clustering_type = uncertainty_contact_planning::CONVEX_REGION_SIGNATURE;
        options.planner_time_limit = 300.0;
        options.goal_bias = 0.1;
        options.step_size = 24.0 * 0.125;
        options.step_duration = 10.0;
        options.goal_probability_threshold = 0.51;
        options.goal_distance_threshold = 5.0 * 0.125;
        options.connect_after_first_solution = 0.0;
        options.signature_matching_threshold = 0.75;
        options.distance_clustering_threshold = 15.0 * 0.125;
        options.feasibility_alpha = 0.75;
        options.variance_alpha = 0.75;
        options.edge_attempt_count = 50u;
        options.num_particles = 24u;
        options.use_contact = true;
        options.use_reverse = true;
        options.use_spur_actions = true;
        options.max_exec_actions = 1000u;
        options.max_policy_exec_time = 300.0;
        options.num_policy_simulations = 0u;
        options.num_policy_executions = 1u;
        options.policy_action_attempt_count = 100u;
        options.debug_level = 0;
        options.planner_log_file = "/tmp/se3_planner_log.txt";
        options.policy_log_file = "/tmp/se3_policy_log.txt";
        options.planned_policy_file = "/tmp/se3_planned_policy.policy";
        options.executed_policy_file = "/dev/null";
        return options;
    }

    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetOptions()
    {
        return uncertainty_planning_core::GetOptions(GetDefaultOptions());
    }

    inline config_common::TASK_CONFIG_PARAMS GetDefaultExtraOptions()
    {
        return config_common::TASK_CONFIG_PARAMS(0.125, 10.0, 0.0, 0.125, "peg_in_hole");
    }

    inline config_common::TASK_CONFIG_PARAMS GetExtraOptions()
    {
        return config_common::GetOptions(GetDefaultExtraOptions());
    }

    inline simple_robot_models::SE3_ROBOT_CONFIG GetDefaultRobotConfig(const config_common::TASK_CONFIG_PARAMS& options)
    {
        const double kp = 1.0; //0.1
        const double ki = 0.0;
        const double kd = 0.0; //0.01;
        const double i_clamp = 0.0;
        const double velocity_limit = 1.0; //0.25; // 1.0;
        const double angular_velocity_limit = velocity_limit * 0.25;
        const double max_sensor_noise = options.sensor_error;
        const double max_angular_sensor_noise = max_sensor_noise * 0.25;
        const double max_actuator_noise = options.actuator_error;
        const double max_angular_actuator_noise = max_actuator_noise * 0.25;
        const simple_robot_models::SE3_ROBOT_CONFIG robot_config(kp, ki, kd, i_clamp, velocity_limit, max_sensor_noise, max_actuator_noise, kp, ki, kd, i_clamp, angular_velocity_limit, max_angular_sensor_noise, max_angular_actuator_noise);
        return robot_config;
    }

    inline Eigen::Isometry3d MakeConfig(const Eigen::Translation3d& translation, const Eigen::Quaterniond& rotation)
    {
        const Eigen::Isometry3d config = translation * rotation;
        return config;
    }

    inline std::pair<Eigen::Isometry3d, Eigen::Isometry3d> GetStartAndGoal()
    {
        // Define the goals of the plan
        const Eigen::Isometry3d start = MakeConfig(Eigen::Translation3d(9.0, 9.0, 9.0), Eigen::Quaterniond::Identity());
        const Eigen::Isometry3d goal = MakeConfig(Eigen::Translation3d(2.25, 2.25, 0.625), Eigen::Quaterniond::Identity());
        return std::make_pair(start, goal);
    }

    inline std::shared_ptr<EigenHelpers::VectorVector4d> GetRobotPoints()
    {
        std::shared_ptr<EigenHelpers::VectorVector4d> robot_points(new EigenHelpers::VectorVector4d());
        const std::vector<double> x_pos = {-0.1875, -0.0625, 0.0625, 0.1875};
        const std::vector<double> y_pos = {-0.1875, -0.0625, 0.0625, 0.1875};
        const std::vector<double> z_pos = {-0.4375, -0.3125, -0.1875, -0.0625, 0.0625, 0.1875, 0.3125, 0.4375};
        for (size_t xpdx = 0; xpdx < x_pos.size(); xpdx++)
        {
            for (size_t ypdx = 0; ypdx < y_pos.size(); ypdx++)
            {
                for (size_t zpdx = 0; zpdx < z_pos.size(); zpdx++)
                {
                    robot_points->push_back(Eigen::Vector4d(x_pos[xpdx], y_pos[ypdx], z_pos[zpdx], 1.0));
                }
            }
        }
        return robot_points;
    }

    inline simple_robot_models::SimpleSE3Robot GetRobot(const simple_robot_models::SE3_ROBOT_CONFIG& robot_config)
    {
        // Make the actual robot
        const Eigen::Isometry3d initial_config = Eigen::Isometry3d::Identity();
        simple_robot_models::SimpleSE3Robot robot(GetRobotPoints(), initial_config, robot_config);
        return robot;
    }

    inline uncertainty_planning_core::SE3SamplerPtr GetSampler()
    {
        const double env_resolution = 0.125;
        const double env_min_x = 0.0 + (env_resolution);
        const double env_max_x = 10.0 - (env_resolution);
        const double env_min_y = 0.0 + (env_resolution);
        const double env_max_y = 10.0 - (env_resolution);
        const double env_min_z = 0.0 + (env_resolution);
        const double env_max_z = 10.0 - (env_resolution);
        // Make the sampler
        return uncertainty_planning_core::SE3SamplerPtr(new simple_samplers::SimpleSE3BaseSampler<uncertainty_planning_core::PRNG>(std::pair<double, double>(env_min_x, env_max_x), std::pair<double, double>(env_min_y, env_max_y), std::pair<double, double>(env_min_z, env_max_z)));
    }

    inline uncertainty_planning_core::SE3SimulatorPtr GetSimulator(const config_common::TASK_CONFIG_PARAMS& options, const int32_t debug_level)
    {
        const int32_t real_debug_level = std::max(0, debug_level - 1);
        const simulator_environment_builder::EnvironmentComponents environment_components = simulator_environment_builder::BuildCompleteEnvironment(options.environment_id, options.environment_resolution);
        const fast_kinematic_simulator::SolverParameters solver_params = fast_kinematic_simulator::GetDefaultSolverParameters();
        return fast_kinematic_simulator::MakeSE3Simulator(environment_components.GetEnvironment(), environment_components.GetEnvironmentSDF(), environment_components.GetSurfaceNormalsGrid(), solver_params, options.simulation_controller_frequency, real_debug_level);
    }
}

#endif // SE3_COMMON_CONFIG_HPP


#include <idmlib/resys/context/FactorizationMachine.h>

#include <am/graphchi/engine/dynamic_graphs/graphchi_dynamicgraph_engine.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/random.hpp>

#include <util/ClockTimer.h>
#include <boost/timer.hpp>

#include <list>
#include <string>

#include <cmath>

using namespace graphchi;
using namespace idmlib::recommender;

namespace bfs = boost::filesystem;

const char* TEST_DIR_STR = "fm";

BOOST_AUTO_TEST_SUITE(FactorizationMachineTest)

BOOST_AUTO_TEST_CASE(factorInmemTest)
{
    bfs::path fmPath(TEST_DIR_STR);
    boost::filesystem::remove_all(fmPath);
    bfs::create_directories(fmPath);

    std::string filename = fmPath.string() + "/fmdb";
    typedef vertex_data<20> VertexDataType;
    typedef FactorContainerInMem<VertexDataType> FactorContainerType;
    unsigned size = 100;
    {
        FactorContainerType db(filename);
        db.Init(size);
        for(unsigned i = 0; i < size; ++i)
        {
            VertexDataType& v = db[i];
            v.bias = i;
        }
    }
    FactorContainerType db(filename);
    db.Init(size);
    for(unsigned i = 0; i < size; ++i)
    {
        VertexDataType& v = db[i];
        BOOST_CHECK_EQUAL(v.bias, i);
    }
}

BOOST_AUTO_TEST_CASE(factorDBTest)
{
    bfs::path fmPath(TEST_DIR_STR);
    boost::filesystem::remove_all(fmPath);
    bfs::create_directories(fmPath);

    std::string filename = fmPath.string() + "/fmdb";
    typedef vertex_data<20> VertexDataType;
    typedef FactorContainerDB<VertexDataType> FactorContainerType;
    unsigned size = 100;
    {
        FactorContainerType db(filename, sizeof(VertexDataType)*10);
        db.Init(size);
        for(unsigned i = 0; i < size; ++i)
        {
            VertexDataType& v = db[i];
            v.bias = i;
        }
    }
    FactorContainerType db(filename, sizeof(VertexDataType)*10);
    db.Init(size);
    for(unsigned i = 0; i < size; ++i)
    {
        VertexDataType& v = db[i];
        BOOST_CHECK_EQUAL(v.bias, i);
    }
}

BOOST_AUTO_TEST_CASE(smokeTest)
{
#if 0
    bfs::path fmPath(TEST_DIR_STR);
    boost::filesystem::remove_all(fmPath);
    bfs::create_directories(fmPath);

    metrics m("fm");

    typedef vertex_data<20> VertexDataType;
    typedef edge_data<0> EdgeDataType;

    typedef FactorizationMachine<20, VertexDataType, EdgeDataType > FMType;
    typedef FactorizationMachine<20, VertexDataType, EdgeDataType, true > ImplicitFMType;

    std::string filename = fmPath.string() + "/graphchi_fm";

    int niters = 4; // Number of iterations
    bool scheduler = false;                       // Whether to use selective scheduling

    /* Detect the number of shards or preprocess an input to creae them */
    int nshards = convert_if_notexists<EdgeDataType >(filename,
                                                              get_option_string("nshards", "auto"));

    /* Run */
    graphchi_engine<VertexDataType, EdgeDataType > engine(filename, nshards, scheduler, m);
    engine.set_disable_vertexdata_storage();
    engine.set_modifies_outedges(false);
    engine.set_modifies_inedges(false); // Improves I/O performance.
    std::string factor_db = filename + "/fmdb";
    FMType fm(factor_db,1,0.1,0.1,0.1);
    engine.run(fm, niters);
    ImplicitFMType fm2(factor_db,1,0.1,0.1,0.1);

    metrics_report(m);
#endif
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2014-2015 DiMS dev-team
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <math.h>
#include "util.h"

#include <boost/foreach.hpp>

#include "common/actionHandler.h"
#include "common/authenticationProvider.h"

#include "monitor/rankingDatabase.h"
#include "monitor/controller.h"
#include "monitor/admitTrackerAction.h"
#include "monitor/reputationTracer.h"
#include "monitor/activityControllerAction.h"
#include "monitor/pingAction.h"
#include "monitor/updateNetworkDataAction.h"
#include "monitor/reputationControlAction.h"

namespace common
{
std::vector< uint256 > deleteList;
}

namespace monitor
{
double const PreviousReptationRatio = 0.99;// I want  to preserve  a lot

double const TriggerExtendRatio = 0.1;
double const RelativeToMax = 0.3;
unsigned int const OneTransactionGain = 10;
unsigned int const BoostForServicesAlone = 10;

// allow some  deviation for  other monitors

uint64_t const CReputationTracker::m_recalculateTime = 60;// seconds, this  time is  vital how frequent it should be???

CReputationTracker::CReputationTracker()
{
	std::map< uint160, common::CTrackerData > trackers;
	CRankingDatabase::getInstance()->loadIdentificationDatabase( trackers );

	std::map< uint160, common::CTrackerData >::const_iterator iterator = trackers.begin();

	while( iterator != trackers.end() )
	{
		addTracker( iterator->second );
		addNodeToSynch( iterator->second.m_publicKey.GetID() );
		iterator++;
	}
}

CReputationTracker*
CReputationTracker::getInstance()
{
	if ( !ms_instance )
	{
		ms_instance = new CReputationTracker();
	};
	return dynamic_cast<CReputationTracker *>( ms_instance );
}

void
CReputationTracker::calculateReputation()
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	unsigned int  maxTransactionNumber = 0;
	BOOST_FOREACH( TransactionsAddmited::value_type & transactionIndicator, m_transactionsAddmited )
	{
		if ( maxTransactionNumber < transactionIndicator.second )
		{
			maxTransactionNumber = transactionIndicator.second;
		}
	}

	unsigned int boostForAll = ( maxTransactionNumber + BoostForServicesAlone )* OneTransactionGain * RelativeToMax;

	BOOST_FOREACH( PAIRTYPE( uint160 const, common::CTrackerData ) & tracker, m_registeredTrackers )
	{
		tracker.second.m_reputation *= PreviousReptationRatio;
		if ( m_presentNodes.find( tracker.first ) != m_presentNodes.end() )
		{
			tracker.second.m_reputation += boostForAll;
		}
	}

	BOOST_FOREACH( TransactionsAddmited::value_type & transactionIndicator, m_transactionsAddmited )
	{
		std::map< uint160, common::CTrackerData >::iterator iterator = m_registeredTrackers.find( transactionIndicator.first );

		if ( m_registeredTrackers.end() != iterator )
		{
			iterator->second.m_reputation += transactionIndicator.second * OneTransactionGain;
		}
	}

	m_transactionsAddmited.clear();
}

void
CReputationTracker::loop()
{
	while( 1 )
	{
		std::list< uint160 > toBeRemoved;
		{
			boost::lock_guard<boost::mutex> lock( m_lock );

			BOOST_FOREACH( PAIRTYPE( uint160 const, common::CTrackerData ) & tracker, m_registeredTrackers )
			{
				int64_t timePassed = GetTime() - tracker.second.m_contractTime;
				int64_t timeLeft = tracker.second.m_networkTime - timePassed;

				if ( timeLeft < 0 )
				{
					CRankingDatabase::getInstance()->eraseTrackerData( tracker.second.m_publicKey );
					toBeRemoved.push_back( tracker.second.m_publicKey.GetID() );
					m_allowSynchronization.erase( tracker.second.m_publicKey.GetID() );
				}
				else if ( timeLeft < CController::getInstance()->getPeriod() * TriggerExtendRatio )
				{
					if ( !isExtendInProgress( tracker.second.m_publicKey ) )
					{
						setExtendInProgress( tracker.second.m_publicKey );
						common::CActionHandler::getInstance()->executeAction( new CAdmitTrackerAction(tracker.second.m_publicKey) );
					}
				}
				else
				{
					if ( isExtendInProgress( tracker.second.m_publicKey ) )
						m_extendInProgress.erase( tracker.second.m_publicKey );
				}
			}

			BOOST_FOREACH( uint160 const & id, toBeRemoved )
			{
				m_registeredTrackers.erase( id );
			}

		}

		// outside of  lock  scope
		common::CRankingFullInfo rankingFullInfo(
					getAllyTrackers()
					, getAllyMonitors()
					, getTrackers()
					, getSynchronizedTrackers()
					, getMeasureReputationTime()
					, CReputationControlAction::getInstance() ? CReputationControlAction::getInstance()->getActionKey() : uint256() );

		if ( !toBeRemoved.empty() )
		{
			common::CActionHandler::getInstance()->executeAction( new CUpdateNetworkDataAction( rankingFullInfo, common::CMediumKinds::DimsNodes ) );
		}

		MilliSleep( 1000 );
	}
}

void
CReputationTracker::storeCurrentRanking()
{
	BOOST_FOREACH( PAIRTYPE( uint160 const, common::CTrackerData ) & tracker, m_registeredTrackers )
	{
		CRankingDatabase::getInstance()->writeTrackerData( tracker.second );
	}
}

void
CReputationTracker::loadCurrentRanking()
{
	CRankingDatabase::getInstance()->loadIdentificationDatabase( m_registeredTrackers );
}

void
CReputationTracker::setNodeInfo( common::CValidNodeInfo const & _validNodeInfo, common::CRole::Enum _role )
{
	switch( _role )
	{
	case common::CRole::Seed:
		break;
	case common::CRole::Tracker:
		m_knownTrackers.insert( _validNodeInfo );
		break;
	case common::CRole::Monitor:
		m_knownMonitors.insert( _validNodeInfo );
		break;
	default:
		break;
	}
}

void
CReputationTracker::clearTransactions()
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_transactionsAddmited.clear();
}

void
CReputationTracker::recalculateReputation()
{
	m_measureReputationTime += m_recalculateTime;
	calculateReputation();
}

void
CReputationTracker::checkValidity( common::CAllyTrackerData const & _allyTrackerData )
{
}

std::set< common::CTrackerData >
CReputationTracker::getTrackers() const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	std::set< common::CTrackerData >trackers;

	BOOST_FOREACH( PAIRTYPE( uint160, common::CTrackerData ) const & tracker, m_registeredTrackers )
	{
		trackers.insert( tracker.second );
	}
	return trackers;
}

std::set< common::CAllyMonitorData >
CReputationTracker::getAllyMonitors() const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	std::set< common::CAllyMonitorData > monitors;

	BOOST_FOREACH( PAIRTYPE( uint160, common::CAllyMonitorData ) const & monitor, m_allyMonitors )
	{
		monitors.insert( monitor.second );
	}

	return monitors;
}

std::set< common::CAllyTrackerData >
CReputationTracker::getAllyTrackers() const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	std::set< common::CAllyTrackerData > trackers;

	BOOST_FOREACH( PAIRTYPE( uint160, common::CAllyTrackerData ) const & tracker, m_allyTrackersRankings )
	{
		trackers.insert( tracker.second );
	}

	return trackers;
}

std::list< common::CMedium *>
CReputationTracker::provideConnection( common::CMediumFilter const & _mediumFilter )
{
	std::list< common::CMedium*> mediums = common::CNodesManager::provideConnection( _mediumFilter );

	if ( !mediums.empty() )
		return mediums;

	return _mediumFilter.getMediums( this );
}

std::list< common::CMedium *>
CReputationTracker::getNodesByClass( common::CMediumKinds::Enum _nodesClass ) const
{
	std::list< common::CMedium *> mediums;

	uintptr_t nodeIndicator;

	if ( !CController::getInstance()->isAdmitted() )
	{
		if ( _nodesClass == common::CMediumKinds::DimsNodes
			 || _nodesClass == common::CMediumKinds::Monitors
			 || _nodesClass == common::CMediumKinds::Trackers
			 )
		{
			if ( _nodesClass != common::CMediumKinds::Trackers )
			{
				BOOST_FOREACH( common::CValidNodeInfo const & validNode, m_knownMonitors )
				{
					// in case  of  fail  do  something ??
					if ( getKeyToNode( validNode.m_publicKey.GetID(), nodeIndicator) )
					{
						common::CMedium * medium = findNodeMedium( nodeIndicator );
						if ( medium )
							mediums.push_back( medium );
					}
				}
			}

			if ( _nodesClass != common::CMediumKinds::Monitors )
			{
				BOOST_FOREACH( common::CValidNodeInfo const & validNode, m_knownTrackers )
				{
					if ( getKeyToNode( validNode.m_publicKey.GetID(), nodeIndicator) )
					{
						common::CMedium * medium = findNodeMedium( nodeIndicator );
						if ( medium )
							mediums.push_back( medium );
					}
				}
			}
			return mediums;
		}
	}
	else
	{
		if ( _nodesClass == common::CMediumKinds::DimsNodes
			 || _nodesClass == common::CMediumKinds::Monitors
			 || _nodesClass == common::CMediumKinds::Trackers
			 )
		{
			if ( _nodesClass != common::CMediumKinds::Monitors )
			{

				BOOST_FOREACH( PAIRTYPE( uint160, common::CTrackerData ) const & trackerData, m_registeredTrackers )
				{
					if ( m_presentNodes.find( trackerData.first ) != m_presentNodes.end() && m_synchronizedTrackers.find( trackerData.first ) != m_synchronizedTrackers.end() )
					{
						if ( !getKeyToNode( trackerData.second.m_publicKey.GetID(), nodeIndicator) )
							assert( !"something wrong" );

						common::CMedium * medium = findNodeMedium( nodeIndicator );

						if ( !medium )
							assert( !"something wrong" );
						mediums.push_back( medium );
					}
				}

				BOOST_FOREACH( PAIRTYPE( uint160, common::CAllyTrackerData ) const & trackerData, m_allyTrackersRankings )
				{
					if ( m_presentNodes.find( trackerData.first ) != m_presentNodes.end() && m_synchronizedTrackers.find( trackerData.first ) != m_synchronizedTrackers.end() )
					{
						if ( !getKeyToNode( trackerData.second.m_publicKey.GetID(), nodeIndicator) )
							assert( !"something wrong" );

						common::CMedium * medium = findNodeMedium( nodeIndicator );

						if ( !medium )
							assert( !"something wrong" );
						mediums.push_back( medium );
					}
				}
			}

			if ( _nodesClass != common::CMediumKinds::Trackers )
			{
				BOOST_FOREACH( PAIRTYPE( uint160, common::CAllyMonitorData ) const & monitorData, m_allyMonitors )
				{
					if ( m_presentNodes.find( monitorData.first ) != m_presentNodes.end() )
					{
						if ( !getKeyToNode( monitorData.second.m_publicKey.GetID(), nodeIndicator) )
							assert( !"something wrong" );

						common::CMedium * medium = findNodeMedium( nodeIndicator );

						if ( !medium )
							assert( !"something wrong" );
						mediums.push_back( medium );
					}
				}
			}
		}
	}
	return mediums;
}

void
CReputationTracker::setKeyToNode( CPubKey const & _pubKey, uintptr_t _nodeIndicator)
{
	m_pubKeyToNodeIndicator.erase(_pubKey);
	m_pubKeyToNodeIndicator.insert( std::make_pair( _pubKey, _nodeIndicator ) );
}

bool
CReputationTracker::getKeyToNode( uint160 const & _pubKeyId, uintptr_t & _nodeIndicator) const
{
	std::map< CPubKey, uintptr_t >::const_iterator iterator =
			m_pubKeyToNodeIndicator.begin();

	while( iterator != m_pubKeyToNodeIndicator.end() )
	{
		if ( iterator->first.GetID() == _pubKeyId )
			break;
		iterator++;
	}

	if ( iterator != m_pubKeyToNodeIndicator.end() )
		_nodeIndicator = iterator->second;

	return iterator != m_pubKeyToNodeIndicator.end();
}

bool
CReputationTracker::getNodeToKey( uintptr_t _nodeIndicator, CPubKey & _pubKey )const
{
	BOOST_FOREACH( PAIRTYPE(CPubKey, uintptr_t) const & keyToIndicator, m_pubKeyToNodeIndicator )
	{
		if ( keyToIndicator.second == _nodeIndicator )
		{
			_pubKey = keyToIndicator.first;
			return true;
		}
	}

	return false;
}

std::set< uint160 >
CReputationTracker::getPresentAndSynchronizedTrackers() const
{
	std::set< common::CTrackerData > trackers = getTrackers();

	std::set< uint160 > presentTrackers;
	BOOST_FOREACH( common::CTrackerData const & trackerData, trackers )
	{
		if ( isPresentNode( trackerData.m_publicKey.GetID() ) && isTrackerSynchronized( trackerData.m_publicKey.GetID() ) )
			presentTrackers.insert( trackerData.m_publicKey.GetID() );
	}

	std::set< common::CAllyTrackerData > allyTrackers = getAllyTrackers();

	BOOST_FOREACH( common::CAllyTrackerData const & allyTracker, allyTrackers )
	{
		if ( isPresentNode( allyTracker.m_publicKey.GetID() ) && isTrackerSynchronized( allyTracker.m_publicKey.GetID() ) )
			presentTrackers.insert( allyTracker.m_publicKey.GetID() );
	}

	return presentTrackers;
}

void
CReputationTracker::eraseMedium( uintptr_t _nodePtr )
{
	CPubKey key;

	boost::lock_guard<boost::mutex> lock( m_lock );

	if ( !getNodeToKey( _nodePtr, key ) )
		return;

	CKeyID keyId = key.GetID();

	m_candidates.erase( keyId );

	m_transactionsAddmited.erase( keyId );

	m_presentNodes.erase( keyId );

	m_pubKeyToNodeIndicator.erase( key );

	CAddress address;
	if ( getAddress( _nodePtr, address ) )
	{
		erasePubKey( address );

		common::CNodesManager::eraseMedium( _nodePtr );

		common::CActionHandler::getInstance()->executeAction( new CActivityControllerAction( key, address, CActivitySatatus::Inactive ) );
	}
}

void
CReputationTracker::evaluateNode( common::CSelfNode * _selfNode )
{
	CPubKey pubKey;
	if ( getPublicKey( _selfNode->addr, pubKey ) )
	{
		common::CActionHandler::getInstance()->executeAction( new CPingAction( _selfNode ) );
	}
	else
	{
		_selfNode->fDisconnect = true;  // for seed  and unknown
	}
}
std::set< common::CValidNodeInfo > const
CReputationTracker::getNodesInfo( common::CRole::Enum _role ) const
{
	std::set< common::CValidNodeInfo > nodesInfo;
	if ( _role == common::CRole::Tracker )
	{
		BOOST_FOREACH( PAIRTYPE( uint160 const, common::CTrackerData ) const & tracker, m_registeredTrackers )
		{
			uintptr_t nodePtr;
			getKeyToNode( tracker.second.m_publicKey.GetID(), nodePtr );

			CAddress address;
			getAddress( nodePtr, address );

			nodesInfo.insert( common::CValidNodeInfo( tracker.second.m_publicKey, address ) );
		}
	}
	else if ( _role == common::CRole::Monitor )
	{
		BOOST_FOREACH( PAIRTYPE( uint160, common::CAllyMonitorData ) const & monitor, m_allyMonitors )
		{
			uintptr_t nodePtr;
			getKeyToNode( monitor.second.m_publicKey.GetID(), nodePtr );

			CAddress address;
			getAddress( nodePtr, address );

			nodesInfo.insert( common::CValidNodeInfo( monitor.second.m_publicKey, address ) );
		}
	}
	return nodesInfo;
}

bool
CReputationTracker::checkForTracker( uint160 const & _pubKeyId, common::CTrackerData & _trackerData, CPubKey & _controllingMonitor )const
{
	std::map< uint160, common::CTrackerData >::const_iterator iterator = m_registeredTrackers.find( _pubKeyId );

	if ( iterator != m_registeredTrackers.end() )
	{
		_trackerData = iterator->second;
		_controllingMonitor = common::CAuthenticationProvider::getInstance()->getMyKey();

		return true;
	}

	std::map< uint160, common::CAllyTrackerData >::const_iterator allyIterator = m_allyTrackersRankings.find( _pubKeyId );

	if ( allyIterator == m_allyTrackersRankings.end() )
		return  false;

	_trackerData = dynamic_cast<common::CTrackerData const&>( allyIterator->second );

	_controllingMonitor = allyIterator->second.m_allyMonitorKey;

	return true;
}

void
CReputationTracker::setExtendInProgress( CPubKey const & _pubKey )
{
		m_extendInProgress.insert( _pubKey );
}

bool
CReputationTracker::isExtendInProgress( CPubKey const & _pubKey )
{
	return m_extendInProgress.find(_pubKey) != m_extendInProgress.end();
}

bool
CReputationTracker::eraseExtendInProgress( CPubKey const & _pubKey )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_extendInProgress.erase(_pubKey);

	return true;
}

bool
CReputationTracker::isAddmitedMonitor( uint160 const & _pubKeyId )
{
	return m_allyMonitors.find( _pubKeyId ) != m_allyMonitors.end();
}

bool
CReputationTracker::isRegisteredTracker( uint160 const & _pubKeyId )
{
	if ( m_registeredTrackers.find( _pubKeyId ) != m_registeredTrackers.end() )
		return true;

	return m_allyTrackersRankings.find( _pubKeyId ) != m_allyTrackersRankings.end();

}

void
CReputationTracker::addTracker( common::CTrackerData const & _trackerData )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_registeredTrackers.erase( _trackerData.m_publicKey.GetID() );
	m_registeredTrackers.insert( std::make_pair( _trackerData.m_publicKey.GetID(), _trackerData ) );
}

bool
CReputationTracker::getTracker( uint160 const & _pubKeyId, common::CTrackerData & _trackerData ) const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	std::map< uint160, common::CTrackerData >::const_iterator iterator =
			m_registeredTrackers.find( _pubKeyId );

	if ( iterator != m_registeredTrackers.end() )
	{
		_trackerData = (*iterator).second;
		return true;
	}

	return false;
}

void
CReputationTracker::removeTracker( uint160 const & _pubKeyId )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_registeredTrackers.find( _pubKeyId );
}

void
CReputationTracker::addAllyTracker( common::CAllyTrackerData const & _trackerData )
{
	boost::lock_guard<boost::mutex> lock( m_lock );

	m_allyTrackersRankings.erase( _trackerData.m_publicKey.GetID() );
	m_allyTrackersRankings.insert( make_pair( _trackerData.m_publicKey.GetID(), _trackerData ) );

	m_trackerToMonitor.erase( _trackerData.m_publicKey.GetID() );
	m_trackerToMonitor.insert( std::make_pair( _trackerData.m_publicKey.GetID(), _trackerData.m_allyMonitorKey.GetID() ) );
}

void
CReputationTracker::removeAllyTracker( uint160 const & _pubKeyId )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_allyTrackersRankings.erase( _pubKeyId );
	m_trackerToMonitor.erase( _pubKeyId );
}

void
CReputationTracker::addAllyMonitor( common::CAllyMonitorData const & _monitorData )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	if ( !(common::CAuthenticationProvider::getInstance()->getMyKey() == _monitorData.m_publicKey) )
	{
		m_allyMonitors.insert( std::make_pair( _monitorData.m_publicKey.GetID(), _monitorData ) );
	}
}

void
CReputationTracker::removeAllyMonitor( uint160 const & _pubKeyId )
{
	boost::lock_guard<boost::mutex> lock( m_lock );

	m_allyMonitors.erase( _pubKeyId );
}

void
CReputationTracker::clearRankingData()
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_trackerToMonitor.clear();
	m_registeredTrackers.clear();
	m_allyTrackersRankings.clear();
	m_allyMonitors.clear();
	m_presentNodes.clear();
	m_extendInProgress.clear();

	CRankingDatabase::getInstance()->resetDb();
}

bool
CReputationTracker::isSynchronizationAllowed( uint160 const & _pubKeyId )const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	return m_allowSynchronization.find( _pubKeyId ) != m_allowSynchronization.end();
}

void
CReputationTracker::removeNodeFromSynch( uint160 const & _pubKeyId )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_allowSynchronization.erase( _pubKeyId );
}

void
CReputationTracker::addNodeToSynch( uint160 const & _pubKeyId )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	m_allowSynchronization.insert( _pubKeyId );
}

bool
CReputationTracker::getAddresFromKey( uint160 const & _pubKeyId, CAddress & _address )const
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	uintptr_t nodeIndicator;
	if ( !getKeyToNode( _pubKeyId, nodeIndicator ) )
		return false;

	return getAddress( nodeIndicator, _address );
}

void
CReputationTracker::updateRankingInfo( CPubKey const & _pubKey, common::CRankingFullInfo const & _rankingFullInfo )
{
	boost::lock_guard<boost::mutex> lock( m_lock );
	// sanity, TODO: later react if wrong

	if ( m_allyMonitors.find( _pubKey.GetID() ) == m_allyMonitors.end() )
		return;

	BOOST_FOREACH( common::CAllyTrackerData const & allyTrackerData, _rankingFullInfo.m_allyTrackers )
	{
		if (
					m_registeredTrackers.find( allyTrackerData.m_publicKey.GetID() ) == m_registeredTrackers.end()
				&& m_allyTrackersRankings.find( allyTrackerData.m_publicKey.GetID() ) == m_allyTrackersRankings.end()
					)
		{
			//assert( !"react  to  this" );
			//  problem  here  but  work on it  another  day
		}
	}

	BOOST_FOREACH( common::CAllyMonitorData const & allyMonitorData, _rankingFullInfo.m_allyMonitors )
	{
		if ( m_knownMonitors.find( common::CValidNodeInfo( allyMonitorData.m_publicKey, allyMonitorData.m_address ) ) == m_knownMonitors.end() )
			;//create  connect  action ??

		if ( m_allyMonitors.find( allyMonitorData.m_publicKey.GetID() ) == m_allyMonitors.end() )
		{
			if ( !(common::CAuthenticationProvider::getInstance()->getMyKey() == allyMonitorData.m_publicKey) )
			{
				m_allyMonitors.insert( std::make_pair( allyMonitorData.m_publicKey.GetID(), allyMonitorData ) );
			}
		}
	}

	//add new tracker data, if any

	std::set< uint160 > presentTrackers;

	BOOST_FOREACH( common::CTrackerData const & trackerData, _rankingFullInfo.m_trackers )
	{
		presentTrackers.insert( trackerData.m_publicKey.GetID() );

		if ( m_allyTrackersRankings.find( trackerData.m_publicKey.GetID() ) == m_allyTrackersRankings.end() )
		{
			if ( m_knownTrackers.find( common::CValidNodeInfo( trackerData.m_publicKey, trackerData.m_address ) ) == m_knownTrackers.end() )
				;//create  connect  action ??

			m_allyTrackersRankings.insert( make_pair( trackerData.m_publicKey.GetID(), common::CAllyTrackerData( trackerData, _pubKey ) ) );
			m_trackerToMonitor.insert( std::make_pair( trackerData.m_publicKey.GetID(), _pubKey.GetID() ) );
		}
	}

	std::list< uint160 > deleteTrackersList;

	// gather  all tracker   with  this  monitor
	BOOST_FOREACH( PAIRTYPE( uint160, uint160 ) const & trackerToMonitor, m_trackerToMonitor )
	{
		if ( trackerToMonitor.second == _pubKey.GetID() )
		{
			if ( presentTrackers.find( trackerToMonitor.first ) == presentTrackers.end() )
			{
				deleteTrackersList.push_back( trackerToMonitor.first );
			}
		}
	}

	BOOST_FOREACH( uint160 const & trackerKey, deleteTrackersList )
	{
			m_allyTrackersRankings.erase( trackerKey );
			m_trackerToMonitor.erase( trackerKey );
			m_synchronizedTrackers.erase( trackerKey );
	}

	BOOST_FOREACH( uint160 const & keyId, _rankingFullInfo.m_synchronizedTrackers )
	{
		m_synchronizedTrackers.insert( keyId );
	}
	// popagate  ranking ??
}

}


/*

CandyPoker
https://github.com/sweeterthancandy/CandyPoker

MIT License

Copyright (c) 2019 Gerry Candy

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 "ps/base/cards.h"
#include "ps/support/command.h"
#include "ps/detail/print.h"
#include "ps/eval/class_cache.h"
#include "ps/eval/holdem_class_vector_cache.h"
#include "app/pretty_printer.h"
#include "app/serialization_util.h"
#include "ps/detail/graph.h"

#include "ps/sim/computer.h"
#include "ps/sim/game_tree.h"
#include "ps/sim/computer_factory.h"
#include "ps/sim/_extra.h"
#include "ps/sim/solver.h"

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

namespace bpt = boost::property_tree;

#include <numeric>

#include <boost/timer/timer.hpp>

namespace ps{
namespace sim{
        struct DeltaSequence : Solver{
                DeltaSequence(std::shared_ptr<GameTree> gt_, GraphColouring<AggregateComputer> AG_, StateType state0_, double factor_, double epsilon_, size_t stride_)
                        :gt(gt_),
                        AG(AG_),
                        state0(state0_),
                        factor(factor_),
                        epsilon(epsilon_),
                        stride(stride_)
                {}
                virtual boost::optional<StateType> Execute(SolverContext& ctx)override
                {
                        auto root   = gt->Root();

                        double delta = 0.1;

                        auto S = state0;

                        SequenceConsumer sc;

                        std::vector<StateType> ledger;
                        enum{ MaxFails = 1 };
                        for(size_t fails=0;fails!=MaxFails;){
                                PS_LOG(trace) << "delta => " << delta;
                                size_t counter = 0;
                                size_t max_counter = 1000;
                                for(;counter<max_counter;++counter){
                                        auto S_counter = computation_kernel::CounterStrategy(gt, AG, S, delta);
                                        computation_kernel::InplaceLinearCombination(S, S_counter, 1 - factor );
                                        if( S_counter == S ){
                                                break;
                                        }
                                        if( counter % stride == 0 ){
                                                computation_kernel::InplaceClamp(S, ClampEpsilon);
                                        }
                                }
                                if( counter != max_counter ){
                                        if( ledger.size() > 4 ){
                                                if( ledger[ledger.size()-1] == S &&
                                                    ledger[ledger.size()-2] == S &&
                                                    ledger[ledger.size()-3] == S ){
                                                        return S;
                                                }
                                        }
                                        ledger.push_back(S);
                                        delta /= 2.0;
                                        continue;
                                }
                                PS_LOG(trace) << "delta=" << delta << " failed";
                                ++fails;
                                delta /= 2.0;

                        }
                        return S;
                        if( ledger.size() ){
                                return ledger.back();
                        }
                        ctx.Message("Failed to converge :(889yh");
                        return {};
                }
        private:
                std::shared_ptr<GameTree> gt;
                GraphColouring<AggregateComputer> AG;
                StateType state0;
                double factor;
                double epsilon;
                size_t stride;
                double ClampEpsilon{1e-6};
        };

        struct DeltaSequenceDecl : SolverDecl{
                virtual void Accept(ArgumentVisitor& V)const override{
                        double const default_factor  = 0.05;
                        double const default_epsilon = 0.00001;
                        size_t const default_stride  = 10;

                        V.DeclArgument("factor" , default_factor, "used for taking linear product");
                        V.DeclArgument("epsilon", default_epsilon, "used for stoppage condition");
                        V.DeclArgument("stride" , default_stride,
                                       "used for how many iterations before checking stoppage condition");
                }
                virtual std::shared_ptr<Solver> Make( std::shared_ptr<GameTree> gt,
                                                      GraphColouring<AggregateComputer> AG,
                                                      StateType const& inital_state,
                                                      bpt::ptree const& args)const override
                {
                        double factor  = args.get<double>("factor");
                        double epsilon = args.get<double>("epsilon");
                        size_t stride  = args.get<size_t>("stride");

                        return std::make_shared<DeltaSequence>(gt, AG, inital_state, factor, epsilon, stride);
                }
        };

        static SolverRegister<DeltaSequenceDecl> DeltaSequenceReg("delta-seq");

} // end namespace sim
} // end namespace ps


/*
 Copyright (C) 2016 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.
*/

/*! \file ored/utilities/indexparser.cpp
    \brief
    \ingroup utilities
*/

#include <boost/algorithm/string.hpp>
#include <boost/make_shared.hpp>
#include <boost/regex.hpp>
#include <map>
#include <ored/configuration/conventions.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ql/errors.hpp>
#include <ql/indexes/all.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/all.hpp>
#include <qle/indexes/bmaindexwrapper.hpp>
#include <qle/indexes/cacpi.hpp>
#include <qle/indexes/commodityindex.hpp>
#include <qle/indexes/dkcpi.hpp>
#include <qle/indexes/equityindex.hpp>
#include <qle/indexes/fxindex.hpp>
#include <qle/indexes/genericiborindex.hpp>
#include <qle/indexes/ibor/audbbsw.hpp>
#include <qle/indexes/ibor/brlcdi.hpp>
#include <qle/indexes/ibor/chfsaron.hpp>
#include <qle/indexes/ibor/chftois.hpp>
#include <qle/indexes/ibor/clpcamara.hpp>
#include <qle/indexes/ibor/copibr.hpp>
#include <qle/indexes/ibor/corra.hpp>
#include <qle/indexes/ibor/czkpribor.hpp>
#include <qle/indexes/ibor/demlibor.hpp>
#include <qle/indexes/ibor/dkkcibor.hpp>
#include <qle/indexes/ibor/dkkois.hpp>
#include <qle/indexes/ibor/ester.hpp>
#include <qle/indexes/ibor/hkdhibor.hpp>
#include <qle/indexes/ibor/hufbubor.hpp>
#include <qle/indexes/ibor/idridrfix.hpp>
#include <qle/indexes/ibor/idrjibor.hpp>
#include <qle/indexes/ibor/ilstelbor.hpp>
#include <qle/indexes/ibor/inrmiborois.hpp>
#include <qle/indexes/ibor/inrmifor.hpp>
#include <qle/indexes/ibor/krwcd.hpp>
#include <qle/indexes/ibor/krwkoribor.hpp>
#include <qle/indexes/ibor/mxntiie.hpp>
#include <qle/indexes/ibor/myrklibor.hpp>
#include <qle/indexes/ibor/noknibor.hpp>
#include <qle/indexes/ibor/nowa.hpp>
#include <qle/indexes/ibor/nzdbkbm.hpp>
#include <qle/indexes/ibor/phpphiref.hpp>
#include <qle/indexes/ibor/plnpolonia.hpp>
#include <qle/indexes/ibor/plnwibor.hpp>
#include <qle/indexes/ibor/primeindex.hpp>
#include <qle/indexes/ibor/rubmosprime.hpp>
#include <qle/indexes/ibor/saibor.hpp>
#include <qle/indexes/ibor/seksior.hpp>
#include <qle/indexes/ibor/sekstibor.hpp>
#include <qle/indexes/ibor/sgdsibor.hpp>
#include <qle/indexes/ibor/sgdsor.hpp>
#include <qle/indexes/ibor/skkbribor.hpp>
#include <qle/indexes/ibor/thbbibor.hpp>
#include <qle/indexes/ibor/tonar.hpp>
#include <qle/indexes/ibor/twdtaibor.hpp>
#include <qle/indexes/secpi.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace std;
using ore::data::Convention;

namespace ore {
namespace data {

// Helper base class to build an IborIndex with a specific period and term structure given an instance of the same
// IborIndex
class IborIndexParser {
public:
    virtual ~IborIndexParser() {}
    virtual boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const = 0;
    virtual string family() const = 0;
};

// General case
template <class T> class IborIndexParserWithPeriod : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        return boost::make_shared<T>(p, h);
    }
    string family() const override { return T(3 * Months).familyName(); }
};

// MXN TIIE
// If tenor equates to 28 Days, i.e. tenor is 4W or 28D, ensure that the index is created
// with a tenor of 4W under the hood. Things work better this way especially cap floor stripping.
// We do the same with 91D -> 3M (a la KRW CD below)
template <> class IborIndexParserWithPeriod<MXNTiie> : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        if (p.units() == Days && p.length() == 28) {
            return boost::make_shared<MXNTiie>(4 * Weeks, h);
        } else if (p.units() == Days && p.length() == 91) {
            return boost::make_shared<MXNTiie>(3 * Months, h);
        } else {
            return boost::make_shared<MXNTiie>(p, h);
        }
    }

    string family() const override { return MXNTiie(4 * Weeks).familyName(); }
};

// KRW CD
// If tenor equates to 91 Days, ensure that the index is created with a tenor of 3M under the hood.
template <> class IborIndexParserWithPeriod<KRWCd> : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        if (p.units() == Days && p.length() == 91) {
            return boost::make_shared<KRWCd>(3 * Months, h);
        } else {
            return boost::make_shared<KRWCd>(p, h);
        }
    }

    string family() const override { return KRWCd(3 * Months).familyName(); }
};

// Helper function to check that index name to index object is a one-to-one mapping
void checkOneToOne(const map<string, boost::shared_ptr<OvernightIndex>>& onIndices,
                   const map<string, boost::shared_ptr<IborIndexParser>>& iborIndices) {

    // Should not attempt to add the same family name to the set if the provided mappings are one to one
    set<string> familyNames;

    for (const auto& kv : onIndices) {
        auto p = familyNames.insert(kv.second->familyName());
        QL_REQUIRE(p.second, "Duplicate mapping for overnight index family " << *p.first << " not allowed");
    }

    for (const auto& kv : iborIndices) {
        auto p = familyNames.insert(kv.second->family());
        QL_REQUIRE(p.second, "Duplicate mapping for ibor index family " << *p.first << " not allowed");
    }
}

boost::shared_ptr<FxIndex> parseFxIndex(const string& s, const Handle<Quote>& fxSpot,
                                        const Handle<YieldTermStructure>& sourceYts,
                                        const Handle<YieldTermStructure>& targetYts) {
    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 4, "four tokens required in " << s << ": FX-TAG-CCY1-CCY2");
    QL_REQUIRE(tokens[0] == "FX", "expected first token to be FX");
    return boost::make_shared<FxIndex>(tokens[0] + "/" + tokens[1], 0, parseCurrency(tokens[2]),
                                       parseCurrency(tokens[3]), NullCalendar(), fxSpot, sourceYts, targetYts);
}

boost::shared_ptr<EquityIndex> parseEquityIndex(const string& s) {
    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 2, "two tokens required in " << s << ": EQ-NAME");
    QL_REQUIRE(tokens[0] == "EQ", "expected first token to be EQ");
    return boost::make_shared<EquityIndex>(tokens[1], NullCalendar(), Currency());
}

bool tryParseIborIndex(const string& s, boost::shared_ptr<IborIndex>& index, const boost::shared_ptr<Convention>& c) {
    try {
        index = parseIborIndex(s, Handle<YieldTermStructure>(), c);
    } catch (...) {
        return false;
    }
    return true;
}

boost::shared_ptr<IborIndex> parseIborIndex(const string& s, const Handle<YieldTermStructure>& h,
                                            const boost::shared_ptr<Convention>& c) {
    string dummy;
    return parseIborIndex(s, dummy, h, c);
}

boost::shared_ptr<IborIndex> parseIborIndex(const string& s, string& tenor, const Handle<YieldTermStructure>& h,
                                            const boost::shared_ptr<Convention>& c) {

    // Check the index string is of the required form before doing anything
    vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 2 || tokens.size() == 3,
               "Two or three tokens required in " << s << ": CCY-INDEX or CCY-INDEX-TERM");

    // Variables used below
    string indexStem = tokens[0] + "-" + tokens[1];
    if (tokens.size() == 3) {
        tenor = tokens[2];
    } else {
        tenor = "";
    }

    // if we have a convention given, set up the index using this convention, this overrides the parsing from
    // hardcoded strings below if there is an overlap
    if (c) {
        QL_REQUIRE(c->id() == s, "ibor index convention id ('"
                                     << c->id() << "') not matching ibor index string to parse ('" << s << "'");
        Currency ccy = parseCurrency(tokens[0]);
        if (auto conv = boost::dynamic_pointer_cast<OvernightIndexConvention>(c)) {
            QL_REQUIRE(tenor.empty(), "no tenor allowed for convention based overnight index ('" << s << "')");
            return boost::make_shared<OvernightIndex>(tokens[1], conv->settlementDays(), ccy,
                                                      parseCalendar(conv->fixingCalendar()),
                                                      parseDayCounter(conv->dayCounter()), h);

        } else if (auto conv = boost::dynamic_pointer_cast<IborIndexConvention>(c)) {
            QL_REQUIRE(!tenor.empty(), "no tenor given for convention based Ibor index ('" << s << "'");
            return boost::make_shared<IborIndex>(tokens[1], parsePeriod(tenor), conv->settlementDays(), ccy,
                                                 parseCalendar(conv->fixingCalendar()),
                                                 parseBusinessDayConvention(conv->businessDayConvention()),
                                                 conv->endOfMonth(), parseDayCounter(conv->dayCounter()), h);
        } else {
            QL_FAIL("invalid convention passed to parseIborIndex(): expected OvernightIndexConvention or "
                    "IborIndexConvention");
        }
    }

    // if we do not have a convention, look up the index in the hardcoded maps below

    // Map from our _unique internal name_ to an overnight index
    static map<string, boost::shared_ptr<OvernightIndex>> onIndices = {
        {"EUR-EONIA", boost::make_shared<Eonia>()},        {"EUR-ESTER", boost::make_shared<Ester>()},
        {"GBP-SONIA", boost::make_shared<Sonia>()},        {"JPY-TONAR", boost::make_shared<Tonar>()},
        {"CHF-TOIS", boost::make_shared<CHFTois>()},       {"CHF-SARON", boost::make_shared<CHFSaron>()},
        {"USD-FedFunds", boost::make_shared<FedFunds>()},  {"USD-SOFR", boost::make_shared<Sofr>()},
        {"USD-Prime", boost::make_shared<PrimeIndex>()},   {"AUD-AONIA", boost::make_shared<Aonia>()},
        {"CAD-CORRA", boost::make_shared<CORRA>()},        {"DKK-DKKOIS", boost::make_shared<DKKOis>()},
        {"SEK-SIOR", boost::make_shared<SEKSior>()},       {"COP-IBR", boost::make_shared<COPIbr>()},
        {"BRL-CDI", boost::make_shared<BRLCdi>()},         {"NOK-NOWA", boost::make_shared<Nowa>()},
        {"CLP-CAMARA", boost::make_shared<CLPCamara>()},   {"NZD-OCR", boost::make_shared<Nzocr>()},
        {"PLN-POLONIA", boost::make_shared<PLNPolonia>()}, {"INR-MIBOROIS", boost::make_shared<INRMiborOis>()}};

    // Map from our _unique internal name_ to an ibor index (the period does not matter here)
    static map<string, boost::shared_ptr<IborIndexParser>> iborIndices = {
        {"AUD-BBSW", boost::make_shared<IborIndexParserWithPeriod<AUDbbsw>>()},
        {"AUD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<AUDLibor>>()},
        {"EUR-EURIBOR", boost::make_shared<IborIndexParserWithPeriod<Euribor>>()},
        {"EUR-EURIBOR365", boost::make_shared<IborIndexParserWithPeriod<Euribor365>>()},
        {"CAD-CDOR", boost::make_shared<IborIndexParserWithPeriod<Cdor>>()},
        {"CNY-SHIBOR", boost::make_shared<IborIndexParserWithPeriod<Shibor>>()},
        {"CZK-PRIBOR", boost::make_shared<IborIndexParserWithPeriod<CZKPribor>>()},
        {"EUR-LIBOR", boost::make_shared<IborIndexParserWithPeriod<EURLibor>>()},
        {"USD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<USDLibor>>()},
        {"GBP-LIBOR", boost::make_shared<IborIndexParserWithPeriod<GBPLibor>>()},
        {"JPY-LIBOR", boost::make_shared<IborIndexParserWithPeriod<JPYLibor>>()},
        {"JPY-TIBOR", boost::make_shared<IborIndexParserWithPeriod<Tibor>>()},
        {"CAD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<CADLibor>>()},
        {"CHF-LIBOR", boost::make_shared<IborIndexParserWithPeriod<CHFLibor>>()},
        {"SEK-LIBOR", boost::make_shared<IborIndexParserWithPeriod<SEKLibor>>()},
        {"SEK-STIBOR", boost::make_shared<IborIndexParserWithPeriod<SEKStibor>>()},
        {"NOK-NIBOR", boost::make_shared<IborIndexParserWithPeriod<NOKNibor>>()},
        {"HKD-HIBOR", boost::make_shared<IborIndexParserWithPeriod<HKDHibor>>()},
        {"SAR-SAIBOR", boost::make_shared<IborIndexParserWithPeriod<SAibor>>()},
        {"SGD-SIBOR", boost::make_shared<IborIndexParserWithPeriod<SGDSibor>>()},
        {"SGD-SOR", boost::make_shared<IborIndexParserWithPeriod<SGDSor>>()},
        {"DKK-CIBOR", boost::make_shared<IborIndexParserWithPeriod<DKKCibor>>()},
        {"DKK-LIBOR", boost::make_shared<IborIndexParserWithPeriod<DKKLibor>>()},
        {"HUF-BUBOR", boost::make_shared<IborIndexParserWithPeriod<HUFBubor>>()},
        {"IDR-IDRFIX", boost::make_shared<IborIndexParserWithPeriod<IDRIdrfix>>()},
        {"IDR-JIBOR", boost::make_shared<IborIndexParserWithPeriod<IDRJibor>>()},
        {"ILS-TELBOR", boost::make_shared<IborIndexParserWithPeriod<ILSTelbor>>()},
        {"INR-MIFOR", boost::make_shared<IborIndexParserWithPeriod<INRMifor>>()},
        {"MXN-TIIE", boost::make_shared<IborIndexParserWithPeriod<MXNTiie>>()},
        {"PLN-WIBOR", boost::make_shared<IborIndexParserWithPeriod<PLNWibor>>()},
        {"SKK-BRIBOR", boost::make_shared<IborIndexParserWithPeriod<SKKBribor>>()},
        {"NZD-BKBM", boost::make_shared<IborIndexParserWithPeriod<NZDBKBM>>()},
        {"TRY-TRLIBOR", boost::make_shared<IborIndexParserWithPeriod<TRLibor>>()},
        {"TWD-TAIBOR", boost::make_shared<IborIndexParserWithPeriod<TWDTaibor>>()},
        {"MYR-KLIBOR", boost::make_shared<IborIndexParserWithPeriod<MYRKlibor>>()},
        {"KRW-CD", boost::make_shared<IborIndexParserWithPeriod<KRWCd>>()},
        {"KRW-KORIBOR", boost::make_shared<IborIndexParserWithPeriod<KRWKoribor>>()},
        {"ZAR-JIBAR", boost::make_shared<IborIndexParserWithPeriod<Jibar>>()},
        {"RUB-MOSPRIME", boost::make_shared<IborIndexParserWithPeriod<RUBMosprime>>()},
        {"THB-BIBOR", boost::make_shared<IborIndexParserWithPeriod<THBBibor>>()},
        {"PHP-PHIREF", boost::make_shared<IborIndexParserWithPeriod<PHPPhiref>>()},
        {"RON-ROBOR", boost::make_shared<IborIndexParserWithPeriod<Robor>>()},
        {"DEM-LIBOR", boost::make_shared<IborIndexParserWithPeriod<DEMLibor>>()}};

    // Check (once) that we have a one-to-one mapping
    static bool checked = false;
    if (!checked) {
        checkOneToOne(onIndices, iborIndices);
        checked = true;
    }

    // Simple single case for USD-SIFMA (i.e. BMA)
    if (indexStem == "USD-SIFMA") {
        QL_REQUIRE(tenor.empty(), "A tenor is not allowed with USD-SIFMA as it is implied");
        return boost::make_shared<BMAIndexWrapper>(boost::make_shared<BMAIndex>(h));
    }

    // Overnight indices
    auto onIt = onIndices.find(indexStem);
    if (onIt != onIndices.end()) {
        QL_REQUIRE(tenor.empty(),
                   "A tenor is not allowed with the overnight index " << indexStem << " as it is implied");
        return onIt->second->clone(h);
    }

    // Ibor indices with a tenor
    auto it = iborIndices.find(indexStem);
    if (it != iborIndices.end()) {
        Period p = parsePeriod(tenor);
        return it->second->build(p, h);
    }

    // GENERIC indices
    if (tokens[1] == "GENERIC") {
        Period p = parsePeriod(tenor);
        auto ccy = parseCurrency(tokens[0]);
        return boost::make_shared<GenericIborIndex>(p, ccy, h);
    }

    QL_FAIL("parseIborIndex \"" << s << "\" not recognized");
}

bool isGenericIndex(const string& indexName) { return indexName.find("-GENERIC-") != string::npos; }

bool isInflationIndex(const string& indexName) {
    try {
        // Currently, only way to have an inflation index is to have a ZeroInflationIndex
        parseZeroInflationIndex(indexName);
    } catch (...) {
        return false;
    }
    return true;
}

// Swap Index Parser base
class SwapIndexParser {
public:
    virtual ~SwapIndexParser() {}
    virtual boost::shared_ptr<SwapIndex> build(Period p, const Handle<YieldTermStructure>& f,
                                               const Handle<YieldTermStructure>& d) const = 0;
};

// We build with both a forwarding and discounting curve
template <class T> class SwapIndexParserDualCurve : public SwapIndexParser {
public:
    boost::shared_ptr<SwapIndex> build(Period p, const Handle<YieldTermStructure>& f,
                                       const Handle<YieldTermStructure>& d) const override {
        return boost::make_shared<T>(p, f, d);
    }
};

boost::shared_ptr<SwapIndex> parseSwapIndex(const string& s, const Handle<YieldTermStructure>& f,
                                            const Handle<YieldTermStructure>& d,
                                            boost::shared_ptr<Convention> convention) {

    boost::shared_ptr<data::IRSwapConvention> irSwapConvention;
    if (convention) {
        irSwapConvention = boost::dynamic_pointer_cast<IRSwapConvention>(convention);
        QL_REQUIRE(irSwapConvention, "expected swap convention in parseSwapIndex() for '" << convention->id() << "'");
    }

    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));

    QL_REQUIRE(tokens.size() == 3 || tokens.size() == 4,
               "three or four tokens required in " << s << ": CCY-CMS-TENOR or CCY-CMS-TAG-TENOR");
    QL_REQUIRE(tokens[0].size() == 3, "invalid currency code in " << s);
    QL_REQUIRE(tokens[1] == "CMS", "expected CMS as middle token in " << s);

    Period p = parsePeriod(tokens.back());

    // if no tag is given use LiborSwapIsdaFix as an (arbitrary) standard name
    string familyName = tokens.size() == 4 ? tokens[0] + "-CMS-" + tokens[2] : tokens[0] + "LiborSwapIsdaFix";
    Currency ccy = parseCurrency(tokens[0]);

    boost::shared_ptr<IborIndex> index =
        f.empty() || !convention ? boost::shared_ptr<IborIndex>() : irSwapConvention->index()->clone(f);
    QuantLib::Natural settlementDays = index ? index->fixingDays() : 0;
    QuantLib::Calendar calender = convention ? irSwapConvention->fixedCalendar() : NullCalendar();
    Period fixedLegTenor = convention ? Period(irSwapConvention->fixedFrequency()) : Period(1, Months);
    BusinessDayConvention fixedLegConvention = convention ? irSwapConvention->fixedConvention() : ModifiedFollowing;
    DayCounter fixedLegDayCounter = convention ? irSwapConvention->fixedDayCounter() : ActualActual();

    if (d.empty())
        return boost::make_shared<SwapIndex>(familyName, p, settlementDays, ccy, calender, fixedLegTenor,
                                             fixedLegConvention, fixedLegDayCounter, index);
    else
        return boost::make_shared<SwapIndex>(familyName, p, settlementDays, ccy, calender, fixedLegTenor,
                                             fixedLegConvention, fixedLegDayCounter, index, d);
}

// Zero Inflation Index Parser
class ZeroInflationIndexParserBase {
public:
    virtual ~ZeroInflationIndexParserBase() {}
    virtual boost::shared_ptr<ZeroInflationIndex> build(bool isInterpolated,
                                                        const Handle<ZeroInflationTermStructure>& h) const = 0;
};

template <class T> class ZeroInflationIndexParser : public ZeroInflationIndexParserBase {
public:
    boost::shared_ptr<ZeroInflationIndex> build(bool isInterpolated,
                                                const Handle<ZeroInflationTermStructure>& h) const override {
        return boost::make_shared<T>(isInterpolated, h);
    }
};

boost::shared_ptr<ZeroInflationIndex> parseZeroInflationIndex(const string& s, bool isInterpolated,
                                                              const Handle<ZeroInflationTermStructure>& h) {

    static map<string, boost::shared_ptr<ZeroInflationIndexParserBase>> m = {
        {"EUHICP", boost::make_shared<ZeroInflationIndexParser<EUHICP>>()},
        {"EU HICP", boost::make_shared<ZeroInflationIndexParser<EUHICP>>()},
        {"EUHICPXT", boost::make_shared<ZeroInflationIndexParser<EUHICPXT>>()},
        {"EU HICPXT", boost::make_shared<ZeroInflationIndexParser<EUHICPXT>>()},
        {"FRHICP", boost::make_shared<ZeroInflationIndexParser<FRHICP>>()},
        {"FR HICP", boost::make_shared<ZeroInflationIndexParser<FRHICP>>()},
        {"UKRPI", boost::make_shared<ZeroInflationIndexParser<UKRPI>>()},
        {"UK RPI", boost::make_shared<ZeroInflationIndexParser<UKRPI>>()},
        {"USCPI", boost::make_shared<ZeroInflationIndexParser<USCPI>>()},
        {"US CPI", boost::make_shared<ZeroInflationIndexParser<USCPI>>()},
        {"ZACPI", boost::make_shared<ZeroInflationIndexParser<ZACPI>>()},
        {"ZA CPI", boost::make_shared<ZeroInflationIndexParser<ZACPI>>()},
        {"SECPI", boost::make_shared<ZeroInflationIndexParser<SECPI>>()},
        {"DKCPI", boost::make_shared<ZeroInflationIndexParser<DKCPI>>()},
        {"CACPI", boost::make_shared<ZeroInflationIndexParser<CACPI>>()}};

    auto it = m.find(s);
    if (it != m.end()) {
        return it->second->build(isInterpolated, h);
    } else {
        QL_FAIL("parseZeroInflationIndex: \"" << s << "\" not recognized");
    }
}

boost::shared_ptr<BondIndex> parseBondIndex(const string& s) {
    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 2, "two tokens required in " << s << ": BOND-SECURITY");
    QL_REQUIRE(tokens[0] == "BOND", "expected first token to be BOND");
    return boost::make_shared<BondIndex>(tokens[1]);
}

boost::shared_ptr<QuantExt::CommodityIndex> parseCommodityIndex(const string& name, const Calendar& cal,
                                                                const Handle<PriceTermStructure>& ts) {

    // Make sure the prefix is correct
    string prefix = name.substr(0, 5);
    QL_REQUIRE(prefix == "COMM-", "A commodity index string must start with 'COMM-' but got " << prefix);

    // Now take the remainder of the string
    // for spot indices, this should just be the commodity name (possibly containing hyphens)
    // for future indices, this is of the form NAME-YYYY-MM or NAME-YYYY-MM-DD where NAME is the commodity name
    // (possibly containing hyphens) and YYYY-MM(-DD) is the expiry date of the futures contract
    Date expiry;
    string nameWoPrefix = name.substr(5);
    string commName = nameWoPrefix;

    // Check for form NAME-YYYY-MM-DD
    if (nameWoPrefix.size() > 10) {
        string test = nameWoPrefix.substr(nameWoPrefix.size() - 10);
        if (boost::regex_match(test, boost::regex("\\d{4}-\\d{2}-\\d{2}"))) {
            expiry = parseDate(test);
            commName = nameWoPrefix.substr(0, nameWoPrefix.size() - test.size() - 1);
        }
    }

    // Check for form NAME-YYYY-MM if NAME-YYYY-MM-DD failed
    if (expiry == Date() && nameWoPrefix.size() > 7) {
        string test = nameWoPrefix.substr(nameWoPrefix.size() - 7);
        if (boost::regex_match(test, boost::regex("\\d{4}-\\d{2}"))) {
            expiry = parseDate(test + "-01");
            commName = nameWoPrefix.substr(0, nameWoPrefix.size() - test.size() - 1);
        }
    }

    // Create and return the required future index
    if (expiry != Date()) {
        return boost::make_shared<CommodityFuturesIndex>(commName, expiry, cal, ts);
    } else {
        return boost::make_shared<CommoditySpotIndex>(commName, cal, ts);
    }
}

boost::shared_ptr<Index> parseIndex(const string& s, const data::Conventions& conventions) {
    boost::shared_ptr<QuantLib::Index> ret_idx;
    // if we have an ibor index convention we parse s using this (and throw if we don't succeed)
    if (conventions.has(s, Convention::Type::IborIndex) || conventions.has(s, Convention::Type::OvernightIndex)) {
        ret_idx = parseIborIndex(s, Handle<YieldTermStructure>(), conventions.get(s));
    } else {
        // otherwise we try to parse the ibor index without conventions
        try {
            ret_idx = parseIborIndex(s, Handle<YieldTermStructure>(), nullptr);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        // if we have a swap index convention we parse s using this (and throw if we don't succeed)
        if (conventions.has(s, Convention::Type::SwapIndex)) {
            auto c = boost::dynamic_pointer_cast<SwapIndexConvention>(conventions.get(s));
            QL_REQUIRE(conventions.has(c->conventions(), Convention::Type::Swap),
                       "do not have swap conventions for '" << c->conventions()
                                                            << "', required from swap index convention '" << s << "'");
            ret_idx = parseSwapIndex(s, Handle<YieldTermStructure>(), Handle<YieldTermStructure>(),
                                     conventions.get(c->conventions()));
        } else {
            // otherwise try to parse a swap index without conventions
            try {
                ret_idx = parseSwapIndex(s, Handle<YieldTermStructure>(), Handle<YieldTermStructure>(), nullptr);
            } catch (...) {
            }
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseZeroInflationIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseFxIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseEquityIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseBondIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseCommodityIndex(s);
        } catch (...) {
        }
    }
    QL_REQUIRE(ret_idx, "parseIndex \"" << s << "\" not recognized");
    return ret_idx;
}

bool isOvernightIndex(const string& indexName, const Conventions& conventions) {

    boost::shared_ptr<IborIndex> index;
    if (tryParseIborIndex(indexName, index,
                          conventions.has(indexName, Convention::Type::OvernightIndex) ? conventions.get(indexName)
                                                                                : nullptr)) {
        auto onIndex = boost::dynamic_pointer_cast<OvernightIndex>(index);
        if (onIndex)
            return true;
    }

    return false;
}

string internalIndexName(const string& indexName) {

    // Check that the indexName string is of the required form
    vector<string> tokens;
    split(tokens, indexName, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 2 || tokens.size() == 3,
               "Two or three tokens required in " << indexName << ": CCY-INDEX or CCY-INDEX-TERM");

    // Static map of allowable alternative external names to our unique internal name
    static map<string, string> m = {{"DKK-TNR", "DKK-DKKOIS"}, {"EUR-EURIB", "EUR-EURIBOR"}, {"CAD-BA", "CAD-CDOR"}};

    // Is start of indexName covered by the map? If so, update it.
    string tmpName = tokens[0] + "-" + tokens[1];
    if (m.count(tmpName) == 1) {
        tmpName = m.at(tmpName);
    }

    // If there were only two tokens, return the possibly updated two tokens.
    if (tokens.size() == 2) {
        return tmpName;
    }

    // Check if we have an overnight index
    // This covers cases like USD-FedFunds-1D and returns USD-FedFunds
    // (no need to check convention based overnight indices, they are always of the form CCY-INDEX)
    if (isOvernightIndex(tmpName)) {
        Period p = parsePeriod(tokens[2]);
        QL_REQUIRE(p == 1 * Days,
                   "The period " << tokens[2] << " is not compatible with the overnight index " << tmpName);
        return tmpName;
    }

    // Allow USD-SIFMA-1W or USD-SIFMA-7D externally. USD-SIFMA is used internally.
    if (tmpName == "USD-SIFMA" && (tokens[2] == "1W" || tokens[2] == "7D")) {
        return tmpName;
    }

    return tmpName + "-" + tokens[2];
}

bool isFxIndex(const std::string& indexName) {
    std::vector<string> tokens;
    split(tokens, indexName, boost::is_any_of("-"));
    return tokens.size() == 4 && tokens[0] == "FX";
}

std::string inverseFxIndex(const std::string& indexName) {
    std::vector<string> tokens;
    split(tokens, indexName, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 4 && tokens[0] == "FX", "no fx index given (" << indexName << ")");
    return "FX-" + tokens[1] + "-" + tokens[3] + "-" + tokens[2];
}

} // namespace data
} // namespace ore


#ifndef BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INEXACT_HPP
#define BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INEXACT_HPP
#include <boost/safe_float/policy/check_base_policy.hpp>

#ifdef FENV_AVAILABLE
#pragma STDC FENV_ACCESS ON
#include <fenv.h>
#endif

namespace boost {
namespace safe_float{
namespace policy{

template<class FP>
class check_addition_inexact : public check_policy<FP> {
#ifndef FENV_AVAILABLE
    FP prev_l=0;
    FP prev_r=0;
#endif
public:
    bool pre_addition_check(const FP& lhs, const FP& rhs){
#ifndef FENV_AVAILABLE
        prev_l = lhs;
        prev_r = rhs;
        return true;
#else
        return ! std::feclearexcept(FE_INEXACT);
#endif
    }

    bool post_addition_check(const FP& rhs){
#ifndef FENV_AVAILABLE
        return ((rhs - prev_r) == prev_l) && ((rhs - prev_l) == prev_r); //this check is not completely safe, need to do some math to get a proper implementation...
#else
        return ! std::fetestexcept(FE_INEXACT);
#endif
    }

    std::string addition_failure_message(){
        return std::string("Non reversible addition applied");
    }

};

}
}
}
#endif // BOOST_SAFE_FLOAT_POLICY_CHECK_ADDITION_INEXACT_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BOOLEAN_FUNCTIONS_SIMD_COMMON_IF_ZERO_ELSE_ONE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BOOLEAN_FUNCTIONS_SIMD_COMMON_IF_ZERO_ELSE_ONE_HPP_INCLUDED

#include <boost/simd/toolbox/boolean/functions/if_zero_else_one.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::if_zero_else_one_
                                      , tag::cpu_
                                      , (A0)(X)
                                      , ((simd_<fundamental_<A0>,X>))
                                )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else(a0, Zero<A0>(), One<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::if_zero_else_one_
                                      , tag::cpu_
                                      , (A0)(X)
                                      , ((simd_<logical_<A0>,X>))
                                )
  {
    typedef typename A0::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else(a0, Zero<A0>(), One<A0>());
    }
  };
} } }

#endif


/*
*   Copyright 2017-2018 Simon Raschke
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*       http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*/

#pragma once

#include "definitions.hpp"
#include "enhance/math_utility.hpp"
#include <cmath>
#include <tbb/tbb.h>
#if __has_include(<Eigen/Core>)
#include <Eigen/Core>
#elif __has_include(<eigen3/Eigen/Core>)
#include <eigen3/Eigen/Core>
#endif
#if __has_include(<Eigen/Core>)
#include <Eigen/Geometry>
#elif __has_include(<eigen3/Eigen/Core>)
#include <eigen3/Eigen/Geometry>
#endif

struct Geometry
{
    typedef PARTICLERANGE::value_type::element_type::cartesian cartesian;

    virtual ~Geometry() = default;
    virtual void generate() = 0;
    virtual void scale(const cartesian&) = 0;
    virtual void shift(const cartesian&) = 0;

    tbb::concurrent_vector<cartesian> points {};

protected:
    Geometry() = default;
};