#include <functional>
#include <mutex>
#include <thread>
#include <string>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include "bspline.cpp"
#include <queue>
#include <unordered_map>
// #include <boost>
#include <time.h>
#include "KDTree/KDTree.hpp"
#include "dynamic_voronoi/dynamicvoronoi.h"

#define INF INT_MAX
using namespace std;

namespace global_path_planning
{

    struct Node{
    float cost;
    int index;
    bool operator<(const Node &a) const {
        return cost < a.cost;
    }
    };
    class global_path_planning_
    {
        public:
            global_path_planning_(ros::NodeHandle private_nh)
            {
                getCostmap = false;
                NODE = private_nh;
                costmap_sub_global_ = private_nh.subscribe<nav_msgs::OccupancyGrid>("/map_pub_cyclic", 1, &global_path_planning_::CostmapSubCallback_global, this);
                goal_sub_global_ = private_nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &global_path_planning_::goal_callback, this);
                costmap_with_path_ = private_nh.advertise<nav_msgs::OccupancyGrid>("/costmap_with_path", 1);
                robot_position_pub_=private_nh.advertise<geometry_msgs::PoseStamped>("/robot_position", 1);
                global_path_pub_ = private_nh.advertise<nav_msgs::Path>("/global_path", 1);
            }

        private:
            void find_closest_voronoi_point(int& robot_id, int& closest_voronoi_id, std::vector<bool> is_voronoi)
            {

                int a[4]={1,(int)global_costmap.info.width,-1,-(int)global_costmap.info.width};
                std::unordered_map<int, int> visited, unvisited;
                std::queue<int> id;
                int current;
                std::cout<<"start finding closest voronoi point"<<std::endl;
                if(is_voronoi[robot_id])
                {
                    closest_voronoi_id = robot_id;
                    std::cout<<"find closest voronoi point:"<<closest_voronoi_id<<std::endl;
                    return;
                }
                int count=10000;
                if(!is_voronoi[robot_id]&&count>0)
                {
                    count--;
                    id.push(robot_id);
                    while (id.size())
                    {
                        current = id.front();
                        id.pop();
                        for(int i=0;i<4;i++)
                        {
                            int neighbor_id=current+a[i];
                            // std::cout<<"neighbor_id:"<<neighbor_id<<std::endl;
                            if(!(visited[neighbor_id])&&neighbor_id>0&&neighbor_id<is_voronoi.size())
                            {
                                if(is_voronoi[current])
                                {
                                    closest_voronoi_id=current;
                                    std::cout<<"find closest voronoi point:"<<closest_voronoi_id<<std::endl;
                                    // id=queue<int>();
                                    return;
                                }
                                    id.push(neighbor_id);
                                    ++visited[neighbor_id];
                            }
                        }
                    }
                }
            }
            void CostmapSubCallback_global(const nav_msgs::OccupancyGrid::ConstPtr& map_msg)
            {

                global_costmap_for_voronoi = *map_msg;
                voronoi_sizeX = global_costmap_for_voronoi.info.width;
                voronoi_sizeY = global_costmap_for_voronoi.info.height;
                bool **map1=NULL;
                map1 = new bool*[voronoi_sizeX];

                for (int x=0; x<voronoi_sizeX; x++)
                {
                    (map1)[x] = new bool[voronoi_sizeY];
                }
                for (int x=0; x<voronoi_sizeX; x++)
                {
                    for (int y=0; y<voronoi_sizeY; y++) {
                        if (((int)global_costmap_for_voronoi.data[x+y*voronoi_sizeX]<100) && ((int)global_costmap_for_voronoi.data[x+y*voronoi_sizeX]>=0))
                            map1[x][y]=false; //cell is free
                        else map1[x][y] = true; // cell is occupied
                        }
                }
                if(!voronoi_generated)
                {
                    voronoi.initializeMap(voronoi_sizeX, voronoi_sizeY, map1);
                    voronoi.update(); // update distance map and Voronoi diagram
                    bool doPrune = false, doPruneAlternative = false;
                    if (doPrune) voronoi.prune();  // prune the Voronoi
                    if (doPruneAlternative) voronoi.updateAlternativePrunedDiagram();  // prune the Voronoi
                    voronoi_generated = true;
                }

                for(int i = 0; i < voronoi_sizeX ; i++)
                {
                    for(int j = 0; j < voronoi_sizeY ; j++)
                    {
                        if(voronoi.isVoronoi(i,j)==1)
                        {
                            global_costmap_for_voronoi.data[i+j*voronoi_sizeX]=50;
                        }
                    }
                }
                for (int x=0; x<voronoi_sizeX; x++)
                {
                    delete [] map1[x];
                }
                delete [] map1;

                global_costmap = *map_msg;
                OGM.resize(global_costmap_for_voronoi.data.size());
                for(int i=0;i<global_costmap_for_voronoi.data.size();i++)
                {
                    if(global_costmap_for_voronoi.data[i]==50)
                        OGM[i]=true;
                    else
                        OGM[i]=false;
                }

                std::vector<int> path;
                std::vector<std::vector<double> > path_separted;
                if(goal_index!=0)
                {
                    path_planning(robot_index, goal_index, global_costmap, path);
                    // for(auto i:path)
                    // {
                    //     std::cout<<"path:"<<i<<std::endl;
                    //     global_costmap_for_voronoi.data[i]=100;
                    // }
                }
                for(int i=10; i<100; i++)
                {
                    global_costmap_for_voronoi.data[i+robot_index]=100;
                }
                int closest_robot_in_voronoi_idx=0;
                int closest_goal_in_vorono_idx=0;
                std::cout<<"robot_index:"<<robot_index<<std::endl;
                find_closest_voronoi_point(robot_index, closest_robot_in_voronoi_idx, OGM);
                if(goal_index!=0)
                    find_closest_voronoi_point(goal_index, closest_goal_in_vorono_idx, OGM);
                for(int i=10; i<100; i++)
                {
                    global_costmap_for_voronoi.data[i+closest_robot_in_voronoi_idx]=100;
                }
                if(goal_index!=0)
                {
                    path_planning(closest_robot_in_voronoi_idx, closest_goal_in_vorono_idx, global_costmap_for_voronoi, path);
                    if(!path.empty())
                    {
                        for(int i=0;i<path.size();i+=5)
                        {
                            std::vector<double> path_temp;
                            path_temp.push_back((double)(path[i]%voronoi_sizeX));
                            path_temp.push_back((double)(path[i]/voronoi_sizeX));
                            path_separted.push_back(path_temp);
                        }

                        nav_msgs::Path Path_published;
                        geometry_msgs::PoseStamped this_pose_stamped;
                        std::vector<int> refined_path = Path_smooth(path_separted);
                        refined_path.push_back(goal_index);
                        for(int i = 0; i < refined_path.size(); i++){
                            int temp_path_id=path[i];
                            Path.push_back(temp_path_id);
                            this_pose_stamped.pose.position.x = temp_path_id%global_costmap.info.width;
                            this_pose_stamped.pose.position.y = global_costmap.info.height-temp_path_id/global_costmap.info.width-1;
                            Path_published.header=global_costmap.header;
                            // std::cout<<"u:"<<this_pose_stamped.pose.position.x<<" v:"<<this_pose_stamped.pose.position.y<<std::endl;
                            Path_published.poses.push_back(this_pose_stamped);

                        }
                        global_path_pub_.publish(Path_published);

                        for(auto i: refined_path)
                        {
                            global_costmap_for_voronoi.data[i]=100;
                        }
                    }

                    }
                    costmap_with_path_.publish(global_costmap_for_voronoi);
            }

            std::vector<int> Path_smooth(std::vector<std::vector<double> >& pair_path)
            {
                int refinded_path_size=pair_path.size();
                std::vector<std::vector<double> > refined_path;
                refined_path=pair_path;
                double tolerance_=0.5;
                double change=10000;
                int count=1000;
                double d1, d2;
                while(change>tolerance_&&count>0)
                {
                    count--;
                    change=0;
                    for(int i=1; i<refined_path.size()-1; i++)
                    {
                        for(int j=0; j<refined_path[i].size(); j++)
                        {
                            d1=weight_data * (refined_path[i][j] - pair_path[i][j]);
                            d2=weight_smooth * (refined_path[i-1][j]-refined_path[i+1][j]-2*refined_path[i][j]);
                            change = abs(d1+d2);
                            std::cout<<"change"<<change<<std::endl;
                            refined_path[i][j] += d1+d2;
                        }
                    }
                }
                std::vector<int> result;
                int temp=0;
                for(int i=0; i<refined_path.size(); i++)
                {
                    for(int j=0; j<refined_path[i].size(); j++)
                    {
                        std::cout<<"refined_path:"<<refined_path[i][j];
                    }
                    temp=(int)(refined_path[i][0]+refined_path[i][1]*global_costmap.info.width);
                    result.push_back(temp);
                    std::cout<<std::endl;
                }
                return result;
            }
            point_t get_kdtree_index(point_t point, KDTree tree)
            {
                point_t point_temp;
                point_temp.push_back(point[0]);
                point_temp.push_back(point[1]);
                point_t closest_point = tree.nearest_point(point_temp);
                return closest_point;
            }
            void indextocell(int index, int &x, int &y)
            {
                x = index % global_costmap.info.width;
                y = index / global_costmap.info.width;
            }
            void maptoworld(int x, int y, double &wx, double &wy)
            {
                // wx = origin_x_ + (x + 0.5) * resolution_;
                // wy = origin_y_ + (y + 0.5) * resolution_;
                int u=x, v=global_costmap.info.height-y-1;
                wx = (741-v)/20;
                wy = (1165-u)/20;
            }
            bool isInBounds(int x, int y)
            {
                if( x < 0 || y < 0 || x >= global_costmap.info.height || y >= global_costmap.info.width)
                    return false;
                return true;
            }
            vector<int> get_neighbors(int current_cell)
            {
                vector<int> neighborIndexes;

                for (int i = -1; i <= 1; i++)
                {
                    for (int j = -1; j <= 1; j++)
                    {
                        unsigned tmp1, tmp2;
                        // costmap_->indexToCells(current_cell, tmp1, tmp2);
                        tmp1 = current_cell%global_costmap.info.width;
                        tmp2 = current_cell/global_costmap.info.width;
                        int nextX = tmp1 + i;
                        int nextY = tmp2 + j;
                        // int nextIndex = costmap_->getIndex(nextX, nextY);
                        int nextIndex = nextX+nextY*global_costmap.info.width;

                        if(!( i == 0 && j == 0) && isInBounds(nextX, nextY) && OGM[nextIndex])
                        {
                            neighborIndexes.push_back(nextIndex);
                        }
                    }
                }
                return neighborIndexes;
            }
            double getMoveCost(int firstIndex, int secondIndex)
            {
                unsigned int tmp1, tmp2;
                // costmap_->indexToCells(firstIndex, tmp1, tmp2);
                tmp1 = firstIndex%global_costmap.info.width;
                tmp2 = firstIndex/global_costmap.info.width;
                int firstXCord = tmp1,firstYCord = tmp2;
                // costmap_->indexToCells(secondIndex, tmp1, tmp2);
                tmp1 = secondIndex%global_costmap.info.width;
                tmp2 = secondIndex/global_costmap.info.width;
                int secondXCord = tmp1, secondYCord = tmp2;

                int difference = abs(firstXCord - secondXCord) + abs(firstYCord - secondYCord);
                // Error checking
                if(difference != 1 && difference != 2){
                    ROS_ERROR("Astar global planner: Error in getMoveCost - difference not valid");
                    return 1.0;
                }
                if(difference == 1)
                    return 1.0;
                else
                    return 1.4;
            }
            double getHeuristic(int cell_index, int goal_index)
            {
                unsigned int tmp1, tmp2;
                tmp1 = cell_index%global_costmap.info.width;
                tmp2 = cell_index/global_costmap.info.width;
                int startX = tmp1, startY = tmp2;
                tmp1 = goal_index%global_costmap.info.width;
                tmp2 = goal_index/global_costmap.info.width;
                int goalX = tmp1, goalY = tmp2;

                return abs(goalY - startY) + abs(goalX - startX);
            }

            bool path_planning(int start_index, int goal_index, nav_msgs::OccupancyGrid global_costmap, std::vector<int>& plan_result)
            {
                // ROS_INFO("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,
                // goal.pose.position.x,goal.pose.position.y);
                // ros::Time time_1 = ros::Time::now();
                // double wx = start.pose.position.x;
                // double wy = start.pose.position.y;
                // unsigned int start_x, start_y;
                // costmap_->worldToMap(wx, wy, start_x, start_y);
                // int start_index = costmap_->getIndex(start_x, start_y);

                // wx = goal.pose.position.x;
                // wy = goal.pose.position.y;

                // unsigned int goal_x, goal_y;
                // costmap_->worldToMap(wx, wy, goal_x, goal_y);
                // int goal_index = costmap_->getIndex(goal_x, goal_y);
                int map_size = global_costmap.data.size();
                vector<float> gCosts(map_size, INF);
                vector<int> cameFrom(map_size, -1);

                multiset<Node> priority_costs;

                gCosts[start_index] = 0;

                Node currentNode;
                currentNode.index = start_index;
                currentNode.cost = gCosts[start_index] + 0;
                priority_costs.insert(currentNode);
                vector<geometry_msgs::PoseStamped> plan;
                vector<geometry_msgs::PoseStamped> replan;

                plan.clear();
                replan.clear();
                std::cout<<"begin planning"<<std::endl;

                while(!priority_costs.empty())
                {
                    // Take the element from the top
                    currentNode = *priority_costs.begin();
                    //Delete the element from the top
                    priority_costs.erase(priority_costs.begin());
                    if (currentNode.index == goal_index){
                        break;
                    }
                    // Get neighbors
                    vector<int> neighborIndexes = get_neighbors(currentNode.index);

                    for(int i = 0; i < neighborIndexes.size(); i++){
                        if(cameFrom[neighborIndexes[i]] == -1){
                        gCosts[neighborIndexes[i]] = gCosts[currentNode.index] + getMoveCost(currentNode.index, neighborIndexes[i]);
                        Node nextNode;
                        nextNode.index = neighborIndexes[i];
                        //nextNode.cost = gCosts[neighborIndexes[i]];    //Dijkstra Algorithm
                        nextNode.cost = gCosts[neighborIndexes[i]] + getHeuristic(neighborIndexes[i], goal_index);    //A* Algorithm
                        cameFrom[neighborIndexes[i]] = currentNode.index;
                        priority_costs.insert(nextNode);
                        }
                    }
                }

                if(cameFrom[goal_index] == -1){
                    cout << "Goal not reachable, failed making a global path." << endl;
                    return false;
                }

                if(start_index == goal_index)
                    return false;
                //Finding the best path
                vector<int> bestPath;
                currentNode.index = goal_index;
                while(currentNode.index != start_index){
                    bestPath.push_back(cameFrom[currentNode.index]);
                    currentNode.index = cameFrom[currentNode.index];
                }
                reverse(bestPath.begin(), bestPath.end());
                std::cout<<"before: "<<Path.size()<<std::endl;
                Path.clear();
                int path;
                for(int i = 0; i < bestPath.size(); i=i+3){
                    path = bestPath[i];
                    Path.push_back(path);
                }
                std::cout<<"after: "<<Path.size()<<std::endl;
                cout << "/***********/" << "bestPath.size():" << bestPath.size() << "*****" <<"Path.size():" << Path.size() << endl;
                plan_result=Path;
                return true;

            }
            void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& goal_pose)
            {
                global_goal_pose=*goal_pose;
                std::cout<<"goal_data:"<<global_costmap.data.at(123)<<std::endl;
                goal_index = (global_costmap.info.height-global_goal_pose.pose.position.y-1)*global_costmap.info.width+global_goal_pose.pose.position.x;
                if(global_goal_pose.pose.orientation.x!=-9999&&global_goal_pose.pose.orientation.x!=-9999)
                    robot_index =(global_costmap.info.height-global_goal_pose.pose.orientation.y-1)*global_costmap.info.width+global_goal_pose.pose.orientation.x;

            }

            bool getCostmap;
            ros::NodeHandle NODE;
            ros::Subscriber costmap_sub_constructing_voronoi_;
            ros::Subscriber costmap_sub_global_;
            ros::Subscriber goal_sub_global_;
            ros::Subscriber position_sub_global_;
            ros::Publisher costmap_with_path_;
            ros::Publisher robot_position_pub_;
            ros::Publisher global_path_pub_;
            nav_msgs::OccupancyGrid global_costmap;
            nav_msgs::OccupancyGrid global_costmap_for_voronoi;
            nav_msgs::OccupancyGrid local_costmap;
            geometry_msgs::PoseStamped global_local_pose;
            geometry_msgs::PoseStamped global_goal_pose;
            geometry_msgs::PoseStamped closest_frontier_point;
            std::vector<bool> OGM;//check whether the cell is known
            vector<int> Path;
            vector<int> Path_smoothed;
            int robot_index_x=0;
            int robot_index_y=0;
            DynamicVoronoi voronoi;
            bool voronoi_generated=false;//generate voronoi once
            int voronoi_sizeX, voronoi_sizeY;
            double weight_data=0.005;//the two argument below is for path smooth
            double weight_smooth=0.0000001;

            std::mutex lock_costmap;
            std::thread tf_thread_;
            tf::TransformListener tf_listener_;
            int robot_index=8073724;
            int goal_index=0;
            int temp_goal_index;
            int unknown_digit=100;
            int known_digit=0;
    };
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "global_path_planning");
    ros::NodeHandle private_nh1("~");
    global_path_planning::global_path_planning_ ta(private_nh1);
    ros::MultiThreadedSpinner spinner;
    spinner.spin();
    return 0;
}


#include "hmc/nuts/gauss3D.hpp"
#include <stan/services/callbacks/stream_writer.hpp>
#include <stan/algorithms/hmc/nuts/unit_e_nuts.hpp>
#include <stan/algorithms/hmc/nuts/diag_e_nuts.hpp>
#include <stan/algorithms/hmc/nuts/dense_e_nuts.hpp>
#include <stan/algorithms/hmc/nuts/adapt_unit_e_nuts.hpp>
#include <stan/algorithms/hmc/nuts/adapt_diag_e_nuts.hpp>
#include <stan/algorithms/hmc/nuts/adapt_dense_e_nuts.hpp>
#include <boost/random/additive_combine.hpp>
#include <stan/util/io/dump.hpp>
#include <fstream>

#include <gtest/gtest.h>

typedef boost::ecuyer1988 rng_t;

TEST(McmcNuts, instantiaton_test) {
  rng_t base_rng(4839294);

  std::stringstream output;
  stan::callbacks::stream_writer writer(output);
  std::stringstream error_stream;
  stan::callbacks::stream_writer error_writer(error_stream);

  std::fstream empty_stream("", std::fstream::in);
  stan::io::dump data_var_context(empty_stream);
  gauss3D_model_namespace::gauss3D_model model(data_var_context);

  stan::mcmc::unit_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    unit_e_sampler(model, base_rng);

  stan::mcmc::diag_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    diag_e_sampler(model, base_rng);

  stan::mcmc::dense_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    dense_e_sampler(model, base_rng);

  stan::mcmc::adapt_unit_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    adapt_unit_e_sampler(model, base_rng);

  stan::mcmc::adapt_diag_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    adapt_diag_e_sampler(model, base_rng);

  stan::mcmc::adapt_dense_e_nuts<gauss3D_model_namespace::gauss3D_model, rng_t>
    adapt_dense_e_sampler(model, base_rng);
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_NeutronMaterial.hpp
//! \author Alex Robinson
//! \brief  Neutron material class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_NEUTRON_MATERIAL_HPP
#define MONTE_CARLO_NEUTRON_MATERIAL_HPP

// Boost Includes
#include <boost/unordered_map.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ModuleTraits.hpp"
#include "MonteCarlo_Nuclide.hpp"
#include "MonteCarlo_SAlphaBeta.hpp"
#include "Utility_Tuple.hpp"

namespace MonteCarlo{

//! The neutron material class
class NeutronMaterial
{

public:

  //! Constructor
  NeutronMaterial(
		const ModuleTraits::InternalMaterialHandle id,
		const double density,
		const boost::unordered_map<std::string,Teuchos::RCP<Nuclide> >&
		nuclide_name_map,
		const Teuchos::Array<double>& nuclide_fractions,
		const Teuchos::Array<std::string>& nuclide_names );

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

  //! Return the material id
  ModuleTraits::InternalMaterialHandle getId() const;

  //! Return the material number density (atom/b-cm)
  double getNumberDensity() const;

  //! Return the macroscopic total cross section (1/cm)
  double getMacroscopicTotalCrossSection( const double energy ) const;

  //! Return the macroscopic absorption cross section (1/cm)
  double getMacroscopicAbsorptionCrossSection( const double energy ) const;

  //! Return the survival probability
  double getSurvivalProbability( const double energy ) const;

  //! Return the macroscopic cross section (1/cm) for a specific reaction
  double getMacroscopicReactionCrossSection(
				     const double energy,
				     const NuclearReactionType reaction) const;

  //! Collide with a neutron
  void collideAnalogue( NeutronState& neutron, ParticleBank& bank ) const;

  //! Collide with a neutron and survival bias
  void collideSurvivalBias( NeutronState& neutron, ParticleBank& bank ) const;

private:

  // Get the atomic weight ratio from a nuclide pointer
  static double getNuclideAWR(
		    const Utility::Pair<double,Teuchos::RCP<Nuclide> >& pair );

  // Sample the nuclide that is collided with
  unsigned sampleCollisionNuclide( const double energy ) const;

  // The material id
  ModuleTraits::InternalMaterialHandle d_id;

  // The number density of the material
  double d_number_density;

  // The nuclides that make up the material
  // (FIRST = nuclide_number_density, SECOND = nuclide pointer)
  Teuchos::Array<Utility::Pair<double,Teuchos::RCP<Nuclide> > > d_nuclides;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_NEUTRON_MATERIAL_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_NeutronMaterial.hpp
//---------------------------------------------------------------------------//


/**
 * @file trust_bamberger_2010.cpp
 * Implements the trust model class vanet::TrustBamberger2010.
 *
 * @author Walter Bamberger
 *
 * $Revision: 636 $
 * $HeadURL: https://shawn.svn.sourceforge.net/svnroot/shawn/src/apps/vanet/trust/trust_bamberger_2010.cpp $
 * $Id: trust_bamberger_2010.cpp 636 2011-11-15 12:03:53Z simsiem $
 *
 * @par License
 * This file is part of the network simulator Shawn.

 * Copyright (C) 2010, 2011 Technische Unversitaet Muenchen (www.tum.de)

 * All rights reserved.

 * Shawn is free software; you can redistribute it and/or modify it
 * under the terms of the BSD License. Refer to the shawn-licence.txt
 * file in the root of the Shawn source tree for further details.
 */

#include "trust_bamberger_2010.h"
#ifdef ENABLE_VANET

#include "../knowledge_base/knowledge_base.h"
#include "../utility.h"
#include "sys/simulation/simulation_controller.h"
#include <boost/tokenizer.hpp>
#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <numeric>

using namespace boost;
using namespace shawn;
using namespace std;

namespace vanet
{

struct TrustBamberger2010::ComputationParameters
{
   BinaryOpinion trust;
   BinaryOpinion competence;
   BinaryOpinion predictability;
   double c;
   double errorSum;

   ComputationParameters() :
      c(0.75 / (1.0 - 0.75)), errorSum(0.0)
   {
      /*
       * 0.75 is the default trust, for the case that no evidence is available.
       * This seems more realistic than 0.5, because most vehicle are usually
       * quite reliable.
       */
      float baseTrust[1] = { 0.75 };
      trust.setBaseRate(baseTrust);
      competence.setBaseRate(baseTrust);
      predictability.setBaseRate(baseTrust);

   }
};

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

TrustBamberger2010::TrustBamberger2010( const EvidenceIntegrator& integrator,
         const KnowledgeBase& kb, const string& vehicleLabel,
         const SimulationController& sc ) :
            integrator_(integrator),
            knowledgeBase_(kb),
            logStream_(0),
            reputationComputer_(0),
            simulationController_(sc),
            withPredictability_(
                     sc.environment().optional_bool_param(
                              "trust_with_predictability", false))
{
   static const tokenizer<> logLabels(
            sc.environment().optional_string_param("logLabels", ""));
   if ( find(logLabels.begin(), logLabels.end(), vehicleLabel)
            != logLabels.end() )
   {
      ofstream* logFStream = new ofstream();
      logFStream->open((vehicleLabel + ".trust.log").c_str());
      logFStream->setf(ios_base::fixed, ios_base::floatfield);
      logFStream->precision(4);
      logStream_ = logFStream;
   }
}

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

TrustBamberger2010::~TrustBamberger2010()
{
   if ( logStream_ )
      delete logStream_;
}

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

ostream&
TrustBamberger2010::computeCsvRecord( ostream& os, const Report* report )
{
   computeTrustValueAndCsv(report, &os);
   return os;
}

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

pair<float, float>
TrustBamberger2010::computeTrustValue( const Report* report )
{
   return computeTrustValueAndCsv(report, 0);
}

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

BinaryOpinion
TrustBamberger2010::combineTrustInTypes( const Report* reportToTrust,
         const vector<ComputationParameters>& parameterSet ) const
{
   // Index of the parameters that belong to the information type of the
   // given report.
   size_t indexReportParameters = static_cast<size_t> (reportToTrust->type % 4);
   BinaryOpinion trustInThisType = parameterSet[indexReportParameters].trust;

   // Fusion of the trust parameter computed for all the information types
   // different from the one of the given report.
   BinaryOpinion trustInOtherTypes;
   for ( int i = 0; i < 4; ++i )
   {
      if ( i != indexReportParameters )
      {
         trustInOtherTypes.cumulativeFusionToSelf(parameterSet[i].trust);
      }
   }

   // Log debugging information.
   if ( logStream_ )
   {
      const ComputationParameters& psToTrust =
               parameterSet[indexReportParameters];
      *logStream_ << "    --> type:" << reportToTrust->type << ", competence:"
               << psToTrust.competence;
      if ( withPredictability_ )
         *logStream_ << ", predictability:" << psToTrust.predictability;
      *logStream_ << ", trust:" << psToTrust.trust << endl;
   }

   // Final trust opinion: Fusion of the trust parameter of the wanted type
   // and the unwanted types.
   return trustInThisType.priorityFusion(trustInOtherTypes);
}

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

pair<float, float>
TrustBamberger2010::computeTrustValueAndCsv( const Report* reportToTrust,
         ostream* csvStream )
{
   /*
    * Get all reports and process them one after the other in temporal order.
    * Each type of information is treated separately in this step. So
    * separate computation parameters are necessary for every information
    * type.
    */

   vector<ComputationParameters> parameterSet(4);

   // All reports of the sender. They represent the whole experience
   // with the sender.
   const ReportList::index<Report::receptionTimeTag>::type& reports =
            knowledgeBase_.findAllReports(reportToTrust->sender).get<
                     Report::receptionTimeTag> ();

   if ( logStream_ )
      *logStream_ << "Sender:" << reportToTrust->sender << ", type:"
               << reportToTrust->type << "\n";

   for ( ReportList::index<Report::receptionTimeTag>::type::const_iterator
            oldReportIt = reports.begin(); oldReportIt != reports.end(); ++oldReportIt )
   {
      // Do not use the report I judge about for the judgment
      if ( reportToTrust == *oldReportIt )
         continue;

      const IntReport* oldIntReport = dynamic_cast<IntReport*> (*oldReportIt);
      if ( oldIntReport != 0 && oldIntReport->type > 0 && oldIntReport->type
               < 5 )
      {
         /*
          * Process this old report. It represents one piece of evidence.
          * updateTrustInType does the whole processing of each report.
          */
         ComputationParameters& psOldReport = parameterSet[oldIntReport->type
                  % 4];
         updateTrustInType(oldIntReport, psOldReport);
      }
      else
      {
         throw SimulationFailed(
                  string("TrustBamberger2010 cannot handle reports of class ")
                           + typeid(*oldReportIt).name());
      }
   }

   /*
    * Combine the parameters for every information type in an overall
    * trust value for the wanted information type (the type of the given
    * report).
    *
    * Note: In case that the report list above was empty, all intermediate
    * trust opinion contain zero evidence. The resulting trust opinion below
    * will have zero evidence as well then. This is exactly as intended.
    */

   // Final trust opinion.
   BinaryOpinion trust = combineTrustInTypes(reportToTrust, parameterSet);
   // Final trust value.
   float trustValue = trustToTrustValue(trust);

   // Print out debug information.
   int sumEvidence = trust.evidence(0) + trust.evidence(1);
   if ( csvStream )
   {
      ComputationParameters& psToTrust = parameterSet[reportToTrust->type % 4];
      *csvStream << trustValue << "," << sumEvidence << ",\""
               << psToTrust.competence << "\",\"" << psToTrust.predictability
               << "\",\"" << psToTrust.trust << "\"";
   }

   // Package the trust value for the method's return value.
   return make_pair(trustValue, 1 - trust.ignorance());
}

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

string
TrustBamberger2010::getCsvHeader() const
{
   return "\"trust value\",\"evidence\",\"competence for type\",\"predictability for type\",\"trust for type\"";
}

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

pair<bool, IntOpinion>
TrustBamberger2010::makeDecision( vector<pair<float, IntOpinion> > opinionList ) throw ()
{
   static const float threshold = 1.1f;

   /*
    * Discard the third with the lowest trust values.
    * Here in the trust model, all three opinions are the same. So, I can simply
    * discard the last one.
    */
   opinionList.pop_back();

   /*
    * Discount and fuse the opinions
    */
   IntOpinion resultingOpinion;
   for ( vector<pair<float, IntOpinion> >::const_iterator it =
            opinionList.begin(); it != opinionList.end(); ++it )
   {
      IntOpinion discountedRecommendation = it->second.beliefDiscountingBy(
               it->first);
      resultingOpinion.cumulativeFusionToSelf(discountedRecommendation);
   }

   /*
    * Find the expected value and apply the threshold
    */
   float maxExpectation = 0.0;
   for ( size_t i = 0; i < resultingOpinion.size(); ++i )
   {
      float expectation = resultingOpinion.probabilityExpectation(i);
      if ( maxExpectation < expectation )
         maxExpectation = expectation;
   }
   if ( maxExpectation > threshold )
   {
      return make_pair(true, resultingOpinion);
   }
   else
   {
      return make_pair(false, resultingOpinion);
   }
}

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

void
TrustBamberger2010::updateCompetence( const IntReport* r,
         ComputationParameters& ps ) throw ()
{
   const IntInformationKnowledge* i = r->associatedInformation();
   /// @todo For the reference value, I may not use values of the same sender. Right?
   float
            relativeError =
                     static_cast<float> (abs(r->value - i->value))
                              / static_cast<float> (InformationDescription::maxIntValues[r->type]);
   float ownBelief = i->opinion.belief(i->value);
   ps.competence.addEvidence(0, ownBelief * (1 - relativeError));
   ps.competence.addEvidence(1, ownBelief * relativeError);
}

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

void
TrustBamberger2010::updatePredictability( const IntReport* r,
         ComputationParameters& ps ) throw ()
{
   // Make the prediction
   vector<pair<float, IntOpinion> > opinionList(3,
            make_pair(trustToTrustValue(ps.trust), r->opinion));
   pair<bool, IntOpinion> prediction = makeDecision(opinionList);

   // Compute the prediction error
   const IntInformationKnowledge* i = r->associatedInformation();
   /*
    * Simplification: For our decision maker, the predicted value is always the
    * same as the value in the report. With this simplification, I can handle
    * the case of a predicted opinion with several equal maximal elements
    * (e.g. 0.0, 0.0, 0.0, 0.0, 0.0).
    */
   int predictedValue = r->value;
   bool equalValues = r->value == i->value;
   double predictedBelief = static_cast<double> (prediction.second.belief(
            predictedValue));
   double ownBelief = static_cast<double> (i->opinion.belief(predictedValue));
   double error = ownBelief - predictedBelief;
   if ( (prediction.first && (!equalValues) && error < 0)
            || ((!prediction.first) && equalValues && error > 0) )
   {
      // Prediction error, i.e., wrong decision.
      ps.errorSum += error;
      double KP = prediction.first ? 0.2 : 0.1;
      double y = KP * error + 0.1 * ps.errorSum;
      if ( y >= 0 )
      {
         ps.c *= 1.0 + y;
      }
      else
      {
         ps.c /= (1.0 - y);
      }

      if ( logStream_ )
      {
         *logStream_ << "    – accepted:" << prediction.first << ", vM:"
                  <value << ", vA:" << predictedValue << ", error:"
                  << error << ", c:" << ps.c << ", errorSum:" << ps.errorSum
                  << ", y:" << y << "\n";
         *logStream_ << "      oM:" <opinion << ", oA:"
                  << prediction.second << "\n";
      }
   }
   else
   {
      // No prediction error. c remains unchanged.
      if ( logStream_ )
         *logStream_ << "    + accepted:" << prediction.first << ", vM:"
                  <value << ", vA:" << predictedValue << ", c:" << ps.c
                  << "\n";
   }

   /* Compute the new predictability opinion from c.
    *
    * I do this in evidence notation. From c = b+ / b- follows r+ / r- = c
    * as well. Things are quite simple then. Just obtain the new amount of
    * evidence and split it on r+ and r- according to c. Simple and fast.
    *
    * The amount of evidence should always be the same as that of the
    * competence. This is important for the averaging fusion. */
   double newEvidenceSum = static_cast<double> (accumulate(
            ps.competence.beginEvidence(), ps.competence.endEvidence(), 0.0f));
   double rminus = newEvidenceSum / (1.0 + ps.c);
   ps.predictability.setEvidence(1, static_cast<float> (rminus));
   ps.predictability.setEvidence(0, static_cast<float> (rminus * ps.c));
}

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

void
TrustBamberger2010::updateTrustInType( const IntReport* r,
         ComputationParameters& ps ) throw ()
{
   updateCompetence(r, ps);
   if ( withPredictability_ )
   {
      float f = .0;
      updatePredictability(r, ps);
      ps.trust = ps.competence.averagingFusion(ps.predictability);
   }
   else
   {
      ps.trust = ps.competence;
   }

   if ( logStream_ )
   {
      *logStream_ << "      type:" << r->type << ", competence:"
               << ps.competence << " / " << ps.competence.evidence(0)
               / ps.competence.evidence(1) << " / " << trustToTrustValue(
               ps.competence);
      if ( withPredictability_ )
         *logStream_ << ", predictability:" << ps.predictability << " / "
                  << ps.predictability.evidence(0)
                           / ps.predictability.evidence(1) << " / "
                  << trustToTrustValue(ps.predictability);
      *logStream_ << ", trust:" << ps.trust << " / " << ps.trust.evidence(0)
               / ps.trust.evidence(1) << " / " << trustToTrustValue(ps.trust)
               << "\n";
   }
}

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

float
TrustBamberger2010::trustToTrustValue( const BinaryOpinion& trust ) throw ()
{
   static bool withChannelCapacity =
            simulationController_.environment().optional_bool_param(
                     "trust_value_with_channel_capacity", false);
   if ( withChannelCapacity )
   {
      // In evidence representation with W = 2
      const double W = 2.0; // Weight of the base rate
      double rplus = static_cast<double> (trust.evidence(0));
      double rminus = static_cast<double> (trust.evidence(1));

      double S = rplus + rminus;
      if ( S <= 0.0 )
      {
         return 0.5;
      }
      else
      {
         double x = rplus / S;

         double Hb = 0.0;
         if ( x > 0.0 && x < 1.0 )
            Hb = -x * log2(x) - (1.0 - x) * log2(1.0 - x);
         // else: For x=0 and x=1, Hb=0. x cannot be <0 or >1.

         if ( rplus > rminus )
            return static_cast<float> ((S / (S + W) * (1.0 - Hb) + 1.0) / 2.0);
         else
            return static_cast<float> ((-S / (S + W) * (1.0 - Hb) + 1.0) / 2.0);
      }
   }
   else
   {
      return trust.probabilityExpectation(0);
   }
}

}

#endif /* ENABLE_VANET */


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_IMPL_TRIGO_F_TRIG_REDUCTION_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_IMPL_TRIGO_F_TRIG_REDUCTION_HPP_INCLUDED

#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/rem_pio2_straight.hpp>
#include <nt2/include/functions/simd/rem_pio2.hpp>
#include <nt2/include/functions/simd/toint.hpp>
#include <nt2/include/functions/simd/inrad.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/is_odd.hpp>
#include <nt2/include/functions/simd/is_not_less.hpp>
#include <nt2/include/functions/simd/is_not_greater.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/bitwise_andnot.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/is_invalid.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/all.hpp>
#include <nt2/include/functions/simd/split.hpp>
#include <nt2/include/functions/simd/group.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/bool.hpp>

namespace nt2
{
  namespace details
  {
    namespace internal
    {
      // This class exposes the public static member:
      // reduce:                to provide range reduction
      //
      // unit_tag allows to choose statically the scaling  among radian_tag, pi_tag, degree_tag
      // meaning that the cosa function will (for example) define respectively
      // x-->cos(x)          (radian_tag),
      // x-->cos(pi*x)       (pi_tag)
      // x-->cos((pi/180)*x) (degree_tag)
      //

      // trigonometric reduction strategies to the [-pi/4, pi/4] range.
      // these reductions are used in the accurate and fast
      // trigonometric functions with different policies

      template<class A0, class mode>
      struct trig_reduction < A0, radian_tag,  tag::simd_type, mode, float>
      {
        typedef typename meta::as_logical<A0>::type              bA0;
        typedef typename meta::as_integer<A0, signed>::type int_type;
        static inline bA0 isalreadyreduced(const A0&a0) { return is_ngt(a0, Pio_4<A0>()); }
        static inline bA0 ismedium (const A0&a0)  { return le(a0,single_constant<A0,0x43490fdb>()); }
        static inline bA0 issmall  (const A0&a0)  { return le(a0,single_constant<A0,0x427b53d1>()); }
        static inline bA0 islessthanpi_2  (const A0&a0)  { return le(a0,Pio_2<A0>()); }
        typedef typename boost::mpl::not_<boost::is_same<A0,typename meta::upgrade<A0>::type> >::type conversion_allowed;

        static inline bA0 cot_invalid(const A0&) { return False<bA0>(); }
        static inline bA0 tan_invalid(const A0&) { return False<bA0>(); }
        static inline int_type reduce(const A0& x, A0& xr, A0& xc){ return inner_reduce(x, xr, xc, mode()); }
      private:
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const big_& b)
        {
          const A0 x = x_n;
          // x is always positive here
          if (nt2::all(isalreadyreduced(x))) // all of x are in [0, pi/4], no reduction
            {
              xr = x;
              xc = Zero<A0>();
              return Zero<int_type>();
            }
          else if (nt2::all(islessthanpi_2(x))) // all of x are in [pi/4, pi/2],  straight algorithm is sufficient for 1 ulp
              return rem_pio2_straight(x, xr, xc);
          else if (nt2::all(issmall(x))) // all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
              return rem_pio2_cephes(x, xr, xc);
          else if (nt2::all(ismedium(x))) // all of x are in [0, 2^7*pi/2],  fdlibm medium_ way
              return rem_pio2_medium(x, xr, xc);
          else
              return inner_reduce_big(x, xr, xc, b, conversion_allowed());
        }

        template<class m>
        static inline int_type inner_reduce_big(const A0& x, A0& xr, A0& xc, const m&, boost::mpl::true_)
        {
          //if (nt2::all(isnotsobig(x))) // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
          typedef typename meta::upgrade<A0>::type uA0;
          typedef typename meta::upgrade<int_type>::type uint_type;
          typedef trig_reduction< uA0, radian_tag,  tag::simd_type, mode, double> aux_reduction;
          uA0 ux1, ux2, uxr1, uxr2, uxc1, uxc2;
          nt2::split(x, ux1, ux2);
          uint_type n1 = aux_reduction::reduce(ux1, uxr1, uxc1);
          uint_type n2 = aux_reduction::reduce(ux2, uxr2, uxc2);
          xr = nt2::group(uxr1, uxr2);
          nt2::split(xr, ux1, ux2);
          xc = nt2::group((uxr1-ux1)+uxc1, (uxr2-ux2)+uxc2);
          return nt2::group(n1, n2);
        }

        static inline int_type inner_reduce_big(const A0& x, A0& xr, A0& xc, const big_&, boost::mpl::false_)
        {
          // all of x are in [0, inf],  standard big_ way
          return rem_pio2(x, xr, xc);
        }

        static inline int_type inner_reduce_big(const A0& x, A0& xr, A0& xc, const direct_big_&, boost::mpl::false_)
        {
          return rem_pio2_big(x, xr, xc);
        }

        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const medium_&)
        {
          const A0 x = x_n;
          // x is always positive here
          if (nt2::all(isalreadyreduced(x))) // all of x are in [0, pi/4], no reduction
            {
              xr = x;
              xc = Zero<A0>();
              return Zero<int_type>();
            }
          else if (nt2::all(islessthanpi_2(x))) // all of x are in [pi/4, pi/2],  straight algorithm is sufficient for 1 ulp
              return rem_pio2_straight(x, xr, xc);
          else if (nt2::all(issmall(x))) // all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
              return rem_pio2_cephes(x, xr, xc);
          else // correct only if all of x are in [0, 2^7*pi/2],  fdlibm medium_ way
              return rem_pio2_medium(x, xr, xc);
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const small_&)
        {
          const A0 x = x_n;
          // x is always positive here
          if (nt2::all(isalreadyreduced(x))) // all of x are in [0, pi/4], no reduction
            {
              xr = x;
              xc = Zero<A0>();
              return Zero<int_type>();
            }
          else if (nt2::all(islessthanpi_2(x))) // all of x are in [pi/4, pi/2],  straight algorithm is sufficient for 1 ulp
              return rem_pio2_straight(x, xr, xc);
          else //  correct only if all of x are in [0, 20*pi],  cephes algorithm is sufficient for 1 ulp
              return rem_pio2_cephes(x, xr, xc);
         }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const direct_small_&)
        {
          const A0 x = x_n;
          return rem_pio2_cephes(x, xr, xc);
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const direct_medium_&)
        {
          const A0 x = x_n;
          return rem_pio2_medium(x, xr, xc);
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const direct_big_& b)
        {
          const A0 x = x_n;
          inner_reduce_big(x, xr, xc, b);
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const clipped_pio4_&)
        {
          const A0 x = x_n;
          xr = sel(isalreadyreduced(nt2::abs(x)), x, Nan<A0>());
          xc = Zero<A0>();
          return Zero<int_type>();
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const clipped_small_&)
        {
          const A0 x = x_n;
          xr = sel(issmall(nt2::abs(x)), x, Nan<A0>());
          return inner_reduce(xr, xr, xc, small_());
        }
        static inline int_type inner_reduce(const typename A0::native_type x_n, A0& xr, A0& xc, const clipped_medium_&)
        {
          const A0 x = x_n;
          xr = sel(ismedium(nt2::abs(x)), x, Nan<A0>());
          return inner_reduce(xr, xr, xc, medium_());
        }

      };

      template<class A0>
      struct trig_reduction<A0,degree_tag, tag::simd_type,big_, float>
      {
        typedef typename meta::as_integer<A0, signed>::type int_type;
        typedef typename meta::as_logical<A0>::type              bA0;

        static inline bA0 cot_invalid(const A0& x) { return logical_and(is_nez(x), is_flint(x/_180<A0>())); }
        static inline bA0 tan_invalid(const A0& x) { return is_flint((x-_90<A0>())/_180<A0>()); }

        static inline int_type reduce(const typename A0::native_type x_n, A0& xr, A0& xc)
        {
          const A0 x = x_n;
          A0 xi = round(x*single_constant<A0,0x3c360b61>()); //  1.111111111111111e-02f
          A0 x2 = x - xi * _90<A0>();//90.0f
          xr =  x2*single_constant<A0,0x3c8efa35>(); //0.0174532925199432957692f
          xc = Zero<A0>();
          return toint(xi);
        }
      };

      // TODO put a medium_ case with a fast_round ?
       template < class A0>
       struct trig_reduction < A0, pi_tag,  tag::simd_type, big_, float>
      {
        typedef typename meta::as_integer<A0, signed>::type int_type;
        typedef typename meta::as_logical<A0>::type              bA0;

        static inline bA0 cot_invalid(const A0& x) { return logical_and(is_nez(x), is_flint(x)); }
        static inline bA0 tan_invalid(const A0& x) { return is_flint(x-Half<A0>()) ; }

        static inline int_type reduce(const typename A0::native_type x_n,  A0& xr, A0&xc)
        {
          const A0 x = x_n;
          A0 xi = round(x*Two<A0>());
          A0 x2 = x - xi * Half<A0>();
          xr = x2*Pi<A0>();
          xc = Zero<A0>();
          return toint(xi);
        }
      };
    }
  }
}

#endif

// /////////////////////////////////////////////////////////////////////////////
// End of f_trig_reduction.hpp
// /////////////////////////////////////////////////////////////////////////////


#include "PCAModelBuilder.h"
#include "StatisticalModel.h"
#include "vtkPolyData.h"
#include <boost/scoped_ptr.hpp>
#include <iostream>
#include "vtkStandardMeshRepresenter.h"
#include "DataManager.h"
#include "vtkPolyDataReader.h"
#include "saveModelCLP.h"
#include <fstream>
#include "vtkCellArray.h"

using namespace statismo;
typedef vtkStandardMeshRepresenter RepresenterType;
typedef DataManager<vtkPolyData> DataManagerType;
typedef PCAModelBuilder<vtkPolyData> ModelBuilderType;
typedef StatisticalModel<vtkPolyData> StatisticalModelType;

vtkSmartPointer<vtkPolyData> loadVTKPolyData(const std::string& filename) {
    vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(filename.c_str());
    reader->Update();
    vtkSmartPointer<vtkPolyData> pd = vtkSmartPointer<vtkPolyData>::New();
    pd->ShallowCopy(reader->GetOutput());
    return pd;
}

int main(int argc, char ** argv)
{
    PARSE_ARGS;

    if(argc < 7)
    {
        std::cout << "Usage " << argv[0] << " [--groupnumber <int>] [--vtkfilelist <std::vector<std::string>>] [--resultdir <std::string>]" << std::endl;
        return 1;
    }

    // Average of all the VTK meshes
    //   Mean of the 3 coordinates
    vtkSmartPointer<vtkPolyData> polydata0 = loadVTKPolyData(vtkfilelist[0]);
    int numPts = loadVTKPolyData(vtkfilelist[0])->GetPoints()->GetNumberOfPoints();
    vtkSmartPointer<vtkCellArray> verts = polydata0->GetVerts();
    vtkSmartPointer<vtkCellArray> lines = polydata0->GetLines();
    vtkSmartPointer<vtkCellArray> polys = polydata0->GetPolys();
    vtkSmartPointer<vtkCellArray> strips = polydata0->GetStrips();

    vtkSmartPointer<vtkPolyData> polydata_MeanGroup = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    points = polydata0->GetPoints();
    for(int meshID = 1; meshID < vtkfilelist.size(); meshID++)
    {
        vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
        polydata = loadVTKPolyData(vtkfilelist[meshID]);
        for(int ptID = 0; ptID < numPts; ptID++)
        {
            double coord[3];
            polydata->GetPoint(ptID, coord);
            double sum[3];
            points->GetPoint(ptID, sum);
            for(int dim = 0; dim < 3; dim++)
            {
                sum[dim] = sum[dim] + coord[dim];
            }
            points->InsertPoint(ptID, sum);
        }
    }
    double mean[3];
    for(int ptID = 0; ptID < numPts; ptID++)
    {
        for(int dim = 0; dim < 3; dim++)
        {
            double sum[3];
            points->GetPoint(ptID, sum);
            mean[dim] = sum[dim]/vtkfilelist.size();
        }
        points->InsertPoint(ptID, mean);
    }

    //   Creation of the polydata of the mean of the given group
    polydata_MeanGroup->SetPoints(points);
    polydata_MeanGroup->SetVerts(verts);
    polydata_MeanGroup->SetLines(lines);
    polydata_MeanGroup->SetPolys(polys);
    polydata_MeanGroup->SetStrips(strips);

    // Creation of the shape model
    vtkSmartPointer<vtkPolyData> reference = polydata_MeanGroup;

    boost::scoped_ptr<RepresenterType> representer(RepresenterType::Create(reference));
    boost::scoped_ptr<DataManagerType> dataManager(DataManagerType::Create(representer.get()));
    for(int j = 0; j < vtkfilelist.size(); j++)
    {
        dataManager->AddDataset(loadVTKPolyData(vtkfilelist[j]), vtkfilelist[j]);
    }
    boost::scoped_ptr<ModelBuilderType> modelBuilder(ModelBuilderType::Create());
    boost::scoped_ptr<StatisticalModelType> model(modelBuilder->BuildNewModel(dataManager->GetData(), 0.01));

    // Once we have built the model, we can save in the directory choosen by the user.
    std::string H5File = resultdir + "/G" + std::to_string(groupnumber) + ".h5";
    model->Save(H5File);
    std::cout << "Successfully saved shape model as " << "G" << groupnumber << ".h5" << std::endl;

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::model::key::prior_model_dataset_spec.hpp                    //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_KEY_PRIOR_MODEL_PRIOR_MODEL_DATASET_SPEC_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_KEY_PRIOR_MODEL_PRIOR_MODEL_DATASET_SPEC_HPP_ER_2009
#include <boost/parameter/parameters.hpp>
#include <boost/statistics/detail/distribution/model/key/prior.hpp>
#include <boost/statistics/detail/distribution/model/key/model.hpp>
#include <boost/statistics/detail/distribution/model/key/dataset.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace model{

    struct prior_model_dataset_spec{
        typedef boost::parameter::parameters<
            boost::parameter::required<tag::prior>,
            boost::parameter::required<tag::model>,
            boost::parameter::required<tag::dataset>
        > type;
    };

}// model
}// distribution
}// detail
}// statistics
}// boost

#endif


/*
 boost/numeric/odeint/stepper/detail/pid_step_adjuster_coefficients.hpp

 [begin_description]
 Coefficients for the PID stepsize controller.
 [end_description]

 Copyright 2017 Valentin Noah Hartmann

 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 BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_PID_STEP_ADJUSTER_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_PID_STEP_ADJUSTER_COEFFICIENTS_HPP_INCLUDED

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

enum adjuster_type{
    BASIC,
    H0211,
    H211b,
    H211PI,
    H0312,
    H312b,
    H312PID,
    H0321,
    H321
};

template<int Type>
class pid_step_adjuster_coefficients;

template<>
class pid_step_adjuster_coefficients<BASIC> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0;
        (*this)[1] = 0.0;
        (*this)[2] = 0.0;
        (*this)[3] = 0.0;
        (*this)[4] = 0.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H0211> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 2.0;
        (*this)[1] = 1.0 / 2.0;
        (*this)[2] = 0.0;
        (*this)[3] = 1.0 / 2.0;
        (*this)[4] = 0.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H211b> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 5.0;
        (*this)[1] = 2.0 / 5.0;
        (*this)[2] = 0.0;
        (*this)[3] = 1.0 / 5.0;
        (*this)[4] = 0.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H211PI> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 6.0;
        (*this)[1] = 2.0 / 6.0;
        (*this)[2] = 0.0;
        (*this)[3] = 0.0;
        (*this)[4] = 0.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H0312> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 4.0;
        (*this)[1] = 2.0 / 2.0;
        (*this)[2] = 1.0 / 4.0;
        (*this)[3] = 3.0 / 4.0;
        (*this)[4] = 1.0 / 4.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H312b> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 6.0;
        (*this)[1] = 2.0 / 6.0;
        (*this)[2] = 1.0 / 6.0;
        (*this)[3] = 3.0 / 6.0;
        (*this)[4] = 1.0 / 6.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H312PID> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] = 1.0 / 18.0;
        (*this)[1] = 2.0 / 9.0;
        (*this)[2] = 1.0 / 18.0;
        (*this)[3] = 0.0;
        (*this)[4] = 0.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H0321> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] =  5.0 / 4.0;
        (*this)[1] =  1.0 / 2.0;
        (*this)[2] = -3.0 / 4.0;
        (*this)[3] = -1.0 / 4.0;
        (*this)[4] = -3.0 / 4.0;
    }
};

template<>
class pid_step_adjuster_coefficients<H321> : public boost::array<double, 5>
{
public:
    pid_step_adjuster_coefficients()
    : boost::array<double, 5>()
    {
        (*this)[0] =  1.0 / 3.0;
        (*this)[1] =  1.0 / 18.0;
        (*this)[2] = -5.0 / 18.0;
        (*this)[3] = -5.0 / 16.0;
        (*this)[4] = -1.0 / 6.0;
    }
};

} // detail
} // odeint
} // numeric
} // boost

#endif


#include "M2Scorer.h"

#include <algorithm>
#include <fstream>
#include <stdexcept>
#include <sstream>
#include <cstdlib>

#include <boost/lexical_cast.hpp>

using namespace std;

namespace MosesTuning
{

M2Scorer::M2Scorer(const string& config)
  : StatisticsBasedScorer("M2Scorer", config),
    beta_(Scan<float>(getConfig("beta", "0.5"))),
    max_unchanged_words_(Scan<int>(getConfig("max_unchanged_words", "2"))),
    truecase_(Scan<bool>(getConfig("truecase", "false"))),
    verbose_(Scan<bool>(getConfig("verbose", "false"))),
    m2_(max_unchanged_words_, beta_, truecase_)
{}

void M2Scorer::setReferenceFiles(const vector<string>& referenceFiles)
{
  for(size_t i = 0; i < referenceFiles.size(); ++i) {
    m2_.ReadM2(referenceFiles[i]);
    break;
  }
}

void M2Scorer::prepareStats(size_t sid, const string& text, ScoreStats& entry)
{
  string sentence = trimStr(this->preprocessSentence(text));
  std::vector<ScoreStatsType> stats(4, 0);
  m2_.SufStats(sentence, sid, stats);
  entry.set(stats);
}

float M2Scorer::calculateScore(const vector<ScoreStatsType>& comps) const
{

  if (comps.size() != NumberOfScores()) {
    throw runtime_error("Size of stat vector for M2Scorer is not " + NumberOfScores());
  }

  float beta = beta_;

  float p = 0.0;
  float r = 0.0;
  float f = 0.0;

  if(comps[1] != 0)
    p = comps[0] / (double)comps[1];
  else
    p = 1.0;

  if(comps[2] != 0)
    r = comps[0] / (double)comps[2];
  else
    r = 1.0;

  float denom = beta * beta * p + r;
  if(denom != 0)
    f = (1.0 + beta * beta) * p * r / denom;
  else
    f = 0.0;

  if(verbose_)
    std::cerr << comps[0] << " " << comps[1] << " " << comps[2] << std::endl;

  if(verbose_)
    std::cerr << p << " " << r << " " << f << std::endl;

  return f;
}

float M2Scorer::getReferenceLength(const vector<ScoreStatsType>& comps) const
{
  return comps[3];
}

std::vector<ScoreStatsType> randomStats(float decay, int max)
{
  int gold = rand() % max;
  int prop = rand() % max;
  int corr = 0.0;

  if(std::min(prop, gold) > 0)
    corr = rand() % std::min(prop, gold);

  //std::cerr << corr << " " << prop << " " << gold << std::endl;

  std::vector<ScoreStatsType> stats(3, 0.0);
  stats[0] = corr * decay;
  stats[1] = prop * decay;
  stats[2] = gold * decay;

  return stats;
}

float sentenceM2(const std::vector<ScoreStatsType>& stats)
{
  float beta = 0.5;

  std::vector<ScoreStatsType> smoothStats(3, 0.0); // = randomStats(0.001, 5);
  smoothStats[0] += stats[0];
  smoothStats[1] += stats[1];
  smoothStats[2] += stats[2];

  float p = 0.0;
  float r = 0.0;
  float f = 0.0;

  if(smoothStats[1] != 0)
    p = smoothStats[0] / smoothStats[1];
  else
    p = 1.0;

  if(smoothStats[2] != 0)
    r = smoothStats[0] / smoothStats[2];
  else
    r = 1.0;

  float denom = beta * beta * p + r;
  if(denom != 0)
    f = (1.0 + beta * beta) * p * r / denom;
  else
    f = 0.0;

  return f;
}

}


#include <boost/multi_index_container.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index/sequenced_index.hpp>

#include <algorithm>
#include <iostream>
#include <random>
#include <chrono>
#include <set>
#include <unordered_set>

class Matrix {
 public:
  /* ///////////////////////////////////////////////////////////////////
  basic types
  */ ///////////////////////////////////////////////////////////////////
  using Index = uint32_t;
  using Value = std::complex<double>;
  /* ///////////////////////////////////////////////////////////////////
  type T to store in container
  */ ///////////////////////////////////////////////////////////////////
  class T {
   friend class Matrix;
   public:
    Index x_; Index y_; mutable Value v_;
    T() {}
    T(const Index& x, const Index& y, const Value& v ) : x_(x), y_(y), v_(v) {}
    std::string to_string() const {
      std::ostringstream oss;
      oss.precision(2);
      oss << std::fixed ;
      oss << "(" << x_ << "," << y_ << ")" << " ";
      oss << "(" << v_.real() << "," << v_.imag() << ")";
      return oss.str();
    }
  };
  /* ///////////////////////////////////////////////////////////////////
  Tags
  */ ///////////////////////////////////////////////////////////////////
  struct order_xy {};
  struct order_yx {};
  struct random_access {};
  /* ///////////////////////////////////////////////////////////////////
  Type for Boost's Multi-Index Container
  */ ///////////////////////////////////////////////////////////////////
  using Container = boost::multi_index_container<
    T, // the data type stored
    boost::multi_index::indexed_by<
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<order_xy>,
        boost::multi_index::composite_key<
          T,
          boost::multi_index::member<T,Index,&T::x_>,
          boost::multi_index::member<T,Index,&T::y_>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >,
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<order_yx>,
        boost::multi_index::composite_key<
          T,
          boost::multi_index::member<T,Index,&T::y_>,
          boost::multi_index::member<T,Index,&T::x_>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >,
      boost::multi_index::random_access<
        boost::multi_index::tag<random_access>
      >
    >
  >;
  /* ///////////////////////////////////////////////////////////////////
  variables
  */ ///////////////////////////////////////////////////////////////////
  Container container_;
  Index index_max_ = UINT32_MAX;
  /* ///////////////////////////////////////////////////////////////////
  implicit methods
  */ ///////////////////////////////////////////////////////////////////
  void insert(const Matrix::Index& x, const Matrix::Index& y,
    const Matrix::Value& v);
  std::string to_string() const;
  void xy_sort();
  void yx_sort();
  Container::index<order_xy>::type::iterator
    xy_find(const Matrix::Index& x, const Matrix::Index& y);
  void pesAB(const Value& s, Matrix& A, Matrix& B, bool sort);
  std::pair< Container::index<random_access>::type::iterator,
    Container::index<random_access>::type::iterator
  > random_access_equal_range_xy(const Index& x);
  std::pair< Container::index<random_access>::type::iterator,
    Container::index<random_access>::type::iterator
  > random_access_equal_range_yx(const Index& y);
  void add(const Index& x, const Index& y, const Value& v, bool sort);
  Container::index<Matrix::order_xy>::type::iterator
    xy_begin();
  Container::index<Matrix::order_yx>::type::iterator
    yx_begin();
  Container::index<Matrix::random_access>::type::iterator
    random_access_begin();
  Container::index<Matrix::random_access>::type::iterator
    random_access_end();
  Value getCoeff(const Index x, const Index y);
  void clear();
  void reserve(Index m);
};

/* ///////////////////////////////////////////////////////////////////
explicit methods
*/ ///////////////////////////////////////////////////////////////////
void Matrix::reserve(Index m) {
  container_.get<random_access>().reserve(m);
}

void Matrix::clear() {
  container_.clear();
}

Matrix::Value Matrix::getCoeff(const Matrix::Index x, const Matrix::Index y) {
  auto it = xy_find(x,y);
  return it->v_;
}

void Matrix::pesAB(const Matrix::Value& s, Matrix& A, Matrix& B, bool sort) {

  if(sort) {
    A.xy_sort();
    B.yx_sort();
  }

  auto iA = A.random_access_begin();
  auto iA_old = A.random_access_begin();
  Index xA;

  auto iB = B.random_access_begin();
  Index yB;

  Value v = 0;

  while(iA != A.random_access_end()) {
    xA = iA->x_;
    iA_old = iA;
    iB = B.random_access_begin();
if(false) {
    while(iB != B.random_access_end()) {
      iA = iA_old;
      yB = iB->y_;
      v = 0;
      while(xA == iA->x_ && iA != A.random_access_end()
        && yB == iB->y_ && iB != B.random_access_end()) {
        if(iA->y_ == iB->x_) {
          v += iA->v_ * iB->v_;
          iA++; iB++;
        } else if(iA->y_ < iB->x_) { iA++; } else { iB++; }
      }
      add(xA, yB, s*v, sort);
      while(yB == iB->y_ && iB != B.random_access_end()) {iB++;}
    }
}
    while(xA == iA->x_ && iA != A.random_access_end()) {iA++;}
  }
}

void Matrix::
add(const Matrix::Index& x, const Matrix::Index& y, const Matrix::Value& v, bool sort) {
  auto it = xy_find(x,y);
  if(it != container_.get<order_xy>().end()) {
    it->v_ += v;
  } else {
    container_.insert(T(x,y,v)); // very time consuming
  }
}

std::pair<
  Matrix::Container::index<Matrix::random_access>::type::iterator,
  Matrix::Container::index<Matrix::random_access>::type::iterator
> Matrix::
random_access_equal_range_xy(const Matrix::Index& x) {
  std::pair<
    Matrix::Container::index<Matrix::random_access>::type::iterator,
    Matrix::Container::index<Matrix::random_access>::type::iterator
  > res;
  auto rangeXY = container_.get<order_xy>().equal_range(x);
  res.first = container_.project<random_access>(rangeXY.first);
  res.second = container_.project<random_access>(rangeXY.second);
  return res;
}

std::pair<
  Matrix::Container::index<Matrix::random_access>::type::iterator,
  Matrix::Container::index<Matrix::random_access>::type::iterator
> Matrix::
random_access_equal_range_yx(const Matrix::Index& y) {
  std::pair<
    Matrix::Container::index<Matrix::random_access>::type::iterator,
    Matrix::Container::index<Matrix::random_access>::type::iterator
  > res;
  auto rangeYX = container_.get<order_yx>().equal_range(y);
  res.first = container_.project<random_access>(rangeYX.first);
  res.second = container_.project<random_access>(rangeYX.second);
  return res;
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_begin() {
  return container_.get<random_access>().begin();
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_end() {
  return container_.get<random_access>().end();
}

Matrix::Container::index<Matrix::order_xy>::type::iterator
Matrix::xy_begin() {
  return container_.get<order_xy>().begin();
}

Matrix::Container::index<Matrix::order_yx>::type::iterator
Matrix::yx_begin() {
  return container_.get<order_yx>().begin();
}

Matrix::Container::index<Matrix::order_xy>::type::iterator
Matrix::xy_find(const Matrix::Index& x, const Matrix::Index& y) {
  return container_.get<order_xy>().find(std::make_tuple(x,y));
}

std::string Matrix::to_string() const {
  auto it_xy = container_.get<order_xy>().cbegin();
  auto it_yx = container_.get<order_yx>().cbegin();
  auto it_random_access = container_.get<random_access>().cbegin();
  std::string tmp = "";
  tmp += "order_xy | ";
  tmp += "order_yx | ";
  tmp += "random_access\n";
  while(it_xy != container_.get<order_xy>().cend()) {
    tmp += it_xy->to_string() + " | ";
    tmp += it_yx->to_string() + " | ";
    tmp += it_random_access->to_string() + "\n";
    it_xy++;
    it_yx++;
    it_random_access++;
  }
  return tmp;
}

void Matrix::xy_sort() {
  container_.get<random_access>().rearrange(
    container_.get<order_xy>().begin()
  );
}

void Matrix::yx_sort() {
  container_.get<random_access>().rearrange(
    container_.get<order_yx>().begin()
  );
}

void Matrix::insert(const Matrix::Index& x, const Matrix::Index& y,
  const Matrix::Value& v) {
  container_.insert(T(x,y,v));
}
/*

*/