//==============================================================================
//         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 NT2_CORE_FUNCTIONS_SIMD_OUTER_FOLD_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_SIMD_OUTER_FOLD_HPP_INCLUDED

#include <nt2/core/functions/outer_fold.hpp>
#include <boost/simd/sdk/simd/meta/is_vectorizable.hpp>
#include <boost/fusion/include/pop_back.hpp>
#include <nt2/sdk/config/cache.hpp>

#ifndef BOOST_SIMD_NO_SIMD
namespace nt2 { namespace ext
{
  //============================================================================
  // Generates outer_fold
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION_IF( nt2::tag::outer_fold_, boost::simd::tag::simd_, (A0)(S0)(T0)(N0)(A1)(A2)(A3)(A4)
                               , (boost::simd::meta::is_vectorizable<typename A0::value_type, BOOST_SIMD_DEFAULT_EXTENSION>)
                               , ((expr_< table_< unspecified_<A0>, S0 >
                                        , T0
                                        , N0
                                        >
                                 ))
                                 ((ast_< A1, nt2::container::domain>))
                                 (unspecified_<A2>)
                                 (unspecified_<A3>)
                                 (unspecified_<A4>)
                               )
  {
    typedef void                                                              result_type;
    typedef typename A0::value_type                                           value_type;
    typedef typename A1::extent_type                                          extent_type;
    typedef boost::simd::native<value_type,BOOST_SIMD_DEFAULT_EXTENSION>      target_type;

    BOOST_FORCEINLINE result_type operator()(A0& out, A1& in, A2 const& neutral, A3 const& bop, A4 const&) const
    {
      extent_type ext = in.extent();
      static const std::size_t N = boost::simd::meta::cardinal_of<target_type>::value;
      std::size_t ibound  = boost::fusion::at_c<0>(ext);
      std::size_t mbound =  boost::fusion::at_c<1>(ext);
      std::size_t obound =  boost::fusion::at_c<2>(ext);
      std::size_t id;

      std::size_t cache_line_size = nt2::config::top_cache_line_size(2); // in byte
      std::size_t nb_vec = cache_line_size/(sizeof(value_type)*N);
      std::size_t cache_bound = (nb_vec)*N;
      std::size_t bound  =  ((ibound)/cache_bound) * cache_bound;

      for(std::size_t o = 0, o_ = 0; o < obound; ++o, o_+=ibound)
      {
        for(std::size_t i = 0; i < bound; i+=cache_bound)
        {
          id = i+o_;
          for (std::size_t k = 0, k_ = id; k < nb_vec; ++k, k_+=N)
            nt2::run(out, k_, neutral(nt2::meta::as_<target_type>()));

          for(std::size_t m = 0, m_ = 0; m < mbound; ++m, m_+=ibound)
          {
            for (std::size_t k = 0, k_ = id; k < nb_vec; ++k, k_+=N)
              nt2::run( out, k_
                      , bop( nt2::run(out, k_, meta::as_<target_type>())
                           , nt2::run(in, k_+m_, meta::as_<target_type>())
                           )
                      );
          }
        }

        // scalar part
        for(std::size_t i = bound; i < ibound; ++i)
        {
          id = i+o_;
          nt2::run(out, id, neutral(nt2::meta::as_<value_type>()));
          for(std::size_t m = 0, m_ = 0; m < mbound; ++m, m_+=ibound)
          {
            nt2::run( out, id
                    , bop( nt2::run(out, id,meta::as_<value_type>())
                         , nt2::run(in, id+m_,meta::as_<value_type>())
                         )
                    );
          }
        }
      }
    }
  };

  } }

#endif
#endif


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_scenario_contact_phase_humanoid_hpp__
#define __multicontact_api_scenario_contact_phase_humanoid_hpp__

#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/serialization/aligned-vector.hpp"

#include "multicontact-api/trajectories/cubic-hermite-spline.hpp"

#include <pinocchio/container/aligned-vector.hpp>
#include <pinocchio/spatial/force.hpp>

#include <boost/serialization/vector.hpp>

#include <vector>
#include <Eigen/StdVector>

namespace multicontact_api
{
  namespace scenario
  {

    template<typename _Scalar>
    struct ContactPhaseHumanoidTpl
    : public ContactPhaseTpl<_Scalar,4>
    , public serialization::Serializable< ContactPhaseHumanoidTpl<_Scalar> >
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      typedef ContactPhaseTpl<_Scalar,4> Base;
      typedef _Scalar Scalar;
      typedef pinocchio::ForceTpl<Scalar> Force;
      enum {
        state_dim = 9,
        control_dim = 6,
        com_position_id = 0,
        com_velocity_id = 3,
        com_acceleration_id = 3,
        am_id = 6,
        am_variations_id = 6,
        linear_control_id = 0,
        angular_control_id = 3
      };

      typedef Eigen::Matrix<Scalar,state_dim,1> StateVector;
      typedef Eigen::Matrix<Scalar,control_dim,1> ControlVector;
      typedef Eigen::Matrix<Scalar,3,1> Vector3;
      typedef Eigen::Map<Vector3> MapVector3;

      typedef Eigen::Map<StateVector> MapStateVector;
      typedef Eigen::Map<ControlVector> MapControlVector;

      typedef pinocchio::container::aligned_vector<StateVector> VectorStateVector;

      //      typedef pinocchio::container::aligned_vector<Force> VectorForce;
      typedef std::vector<Force, Eigen::aligned_allocator<Force> > VectorForce;
      typedef pinocchio::container::aligned_vector<ControlVector> VectorControlVector;
      typedef std::vector<Scalar> VectorScalar;
      typedef VectorScalar TimeVector;
      typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,1> ConfigurationVector;
      typedef pinocchio::container::aligned_vector<ConfigurationVector> VectorConfigurationVector;

      typedef boost::array<VectorForce,4> ForceVectorArray;

      typedef trajectories::CubicHermiteSplineTpl<Scalar,3> CubicHermiteSpline3;
      typedef trajectories::CubicHermiteSplineTpl<Scalar,24> CubicHermiteSpline24;

      using Base::dim;
      using Base::operator==;
      using typename Base::ContactPatch;
      using Base::m_contact_patches;

      /// \brief default constructor
      ContactPhaseHumanoidTpl()
      : Base()
      , RF_patch(m_contact_patches[0])
      , LF_patch(m_contact_patches[1])
      , RH_patch(m_contact_patches[2])
      , LH_patch(m_contact_patches[3])
      , m_init_state()
      , m_final_state()
      , m_state_trajectory(0)
      , m_dot_state_trajectory(0)
      , m_control_trajectory(0)
      , m_time_trajectory(0)
      , m_objective_trajectory(0)
      , m_raw_control_trajectory(0)
      , m_angular_momentum_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero()))
      , m_com_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero()))
      , m_vcom_ref(CubicHermiteSpline3::Constant(CubicHermiteSpline3::VectorD::Zero()))
      , m_forces_ref(CubicHermiteSpline24::Constant(CubicHermiteSpline24::VectorD::Zero()))
      , m_reference_configurations(0)
      {}

      /// \brief copy constructor
      ContactPhaseHumanoidTpl(const ContactPhaseHumanoidTpl & other)
      : Base(other)
      , RF_patch(m_contact_patches[0])
      , LF_patch(m_contact_patches[1])
      , RH_patch(m_contact_patches[2])
      , LH_patch(m_contact_patches[3])
      , m_init_state(other.m_init_state)
      , m_final_state(other.m_final_state)
      , m_state_trajectory(other.m_state_trajectory)
      , m_dot_state_trajectory(other.m_dot_state_trajectory)
      , m_control_trajectory(other.m_control_trajectory)
      , m_time_trajectory(other.m_time_trajectory)
      , m_objective_trajectory(other.m_objective_trajectory)
      , m_contact_forces_trajectories(other.m_contact_forces_trajectories)
      , m_raw_control_trajectory(other.m_raw_control_trajectory)
      , m_angular_momentum_ref(other.m_angular_momentum_ref)
      , m_com_ref(other.m_com_ref)
      , m_vcom_ref(other.m_vcom_ref)
      , m_forces_ref(other.m_forces_ref)
      , m_reference_configurations(other.m_reference_configurations)
      {}

      /// \brief copy operator
//      template<typename S2>
      ContactPhaseHumanoidTpl & operator=(const ContactPhaseHumanoidTpl & other)
      {
        Base::operator=(other);
        m_init_state = other.m_init_state;
        m_final_state = other.m_final_state;
        m_state_trajectory = other.m_state_trajectory;
        m_dot_state_trajectory = other.m_dot_state_trajectory;
        m_control_trajectory = other.m_control_trajectory;
        m_time_trajectory = other.m_time_trajectory;
        m_objective_trajectory = other.m_objective_trajectory;
        m_contact_forces_trajectories = other.m_contact_forces_trajectories;
        m_raw_control_trajectory = other.m_raw_control_trajectory;
        m_angular_momentum_ref = other.m_angular_momentum_ref;
        m_com_ref = other.m_com_ref;
        m_vcom_ref = other.m_vcom_ref;
        m_forces_ref = other.m_forces_ref;
        m_reference_configurations = other.m_reference_configurations;
        return *this;
      }

      ContactPatch & RF_patch;
      ContactPatch & LF_patch;
      ContactPatch & RH_patch;
      ContactPatch & LH_patch;

      StateVector m_init_state;
      StateVector m_final_state;

      VectorStateVector m_state_trajectory;
      VectorStateVector m_dot_state_trajectory;
      VectorControlVector m_control_trajectory;
      TimeVector m_time_trajectory;
      VectorScalar m_objective_trajectory;
      VectorConfigurationVector m_reference_configurations;
      ForceVectorArray m_contact_forces_trajectories;
      VectorConfigurationVector m_raw_control_trajectory;
      CubicHermiteSpline3 m_angular_momentum_ref;
      CubicHermiteSpline3 m_com_ref;
      CubicHermiteSpline3 m_vcom_ref;
      CubicHermiteSpline24 m_forces_ref;

    private:

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void save(Archive & ar, const unsigned int /*version*/) const
      {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
        ar & boost::serialization::make_nvp("init_state",m_init_state);
        ar & boost::serialization::make_nvp("final_state",m_final_state);
        ar & boost::serialization::make_nvp("state_trajectory",m_state_trajectory);
        ar & boost::serialization::make_nvp("dot_state_trajectory",m_dot_state_trajectory);
        ar & boost::serialization::make_nvp("control_trajectory",m_control_trajectory);
        ar & boost::serialization::make_nvp("time_trajectory",m_time_trajectory);
        ar & boost::serialization::make_nvp("objective_trajectory",m_objective_trajectory);

        size_t reference_configurations_size = m_reference_configurations.size();
        ar & boost::serialization::make_nvp("reference_configurations_size",reference_configurations_size);
        for(typename VectorConfigurationVector::const_iterator it = m_reference_configurations.begin();
            it != m_reference_configurations.end(); ++it)
          ar & boost::serialization::make_nvp("reference_configuration",*it);

        for(typename ForceVectorArray::const_iterator it = m_contact_forces_trajectories.begin();
            it != m_contact_forces_trajectories.end(); ++it)
          ar & boost::serialization::make_nvp("contact_force_trajectory",*it);

//        const typename VectorConfigurationVector::vector_base & m_raw_control_trajectory_ =
//        static_cast<const typename VectorConfigurationVector::vector_base&> (m_raw_control_trajectory);
        ar & boost::serialization::make_nvp("raw_control",m_raw_control_trajectory);
        ar & boost::serialization::make_nvp("angular_momentum_ref",m_angular_momentum_ref);
        ar & boost::serialization::make_nvp("com_ref",m_com_ref);
        ar & boost::serialization::make_nvp("vcom_ref",m_vcom_ref);
        ar & boost::serialization::make_nvp("forces_ref",m_forces_ref);
      }

      template<class Archive>
      void load(Archive & ar, const unsigned int /*version*/)
      {
        ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
        ar >> boost::serialization::make_nvp("init_state",m_init_state);
        ar >> boost::serialization::make_nvp("final_state",m_final_state);
        ar >> boost::serialization::make_nvp("state_trajectory",m_state_trajectory);
        ar >> boost::serialization::make_nvp("dot_state_trajectory",m_dot_state_trajectory);
        ar >> boost::serialization::make_nvp("control_trajectory",m_control_trajectory);
        ar >> boost::serialization::make_nvp("time_trajectory",m_time_trajectory);
        ar >> boost::serialization::make_nvp("objective_trajectory",m_objective_trajectory);

        size_t reference_configurations_size = 0;
        ar >> boost::serialization::make_nvp("reference_configurations_size",reference_configurations_size);
        m_reference_configurations.resize(reference_configurations_size);
        for(typename VectorConfigurationVector::iterator it = m_reference_configurations.begin();
            it != m_reference_configurations.end(); ++it)
          ar >> boost::serialization::make_nvp("reference_configuration",*it);

        for(typename ForceVectorArray::iterator it = m_contact_forces_trajectories.begin();
            it != m_contact_forces_trajectories.end(); ++it)
          ar >> boost::serialization::make_nvp("contact_force_trajectory",*it);

//        typename VectorConfigurationVector::vector_base & m_raw_control_trajectory_ =
//        static_cast<typename VectorConfigurationVector::vector_base&> (m_raw_control_trajectory);
        ar >> boost::serialization::make_nvp("raw_control",m_raw_control_trajectory);
        ar >> boost::serialization::make_nvp("angular_momentum_ref",m_angular_momentum_ref);
        ar >> boost::serialization::make_nvp("com_ref",m_com_ref);
        ar >> boost::serialization::make_nvp("vcom_ref",m_vcom_ref);
        ar >> boost::serialization::make_nvp("forces_ref",m_forces_ref);
      }

      BOOST_SERIALIZATION_SPLIT_MEMBER()

    };
  }
}

#endif // ifndef __multicontact_api_scenario_contact_phase_humanoid_hpp__


#include <upo_rrt_planners/ros/ValidityChecker3.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>
#include <ros/console.h>

using namespace std;

upo_RRT_ros::ValidityChecker3::ValidityChecker3(tf::TransformListener* tf, float resol, int width, int height, unsigned int dimensions, int distType) : StateChecker()
{

	capture_ = new Capture();

	cm_resolution_ = resol; //m/cell
	cm_width_ = width * 2.0;	//m
	cm_height_ = height * 2.0;	//m

	cm_width_pixs_ = cm_width_/cm_resolution_;
	cm_height_pixs_ = cm_height_/cm_resolution_;

	//The robot position (center) from the upper-right corner
	cm_origin_.clear();
	cm_origin_.push_back(cm_width_/2.0);  //x
	cm_origin_.push_back(cm_height_/2.0); //y  //negative earlier

	costmap_.clear();
	costmap_.assign((cm_width_pixs_*cm_height_pixs_), 1.0);

	tf_ = tf;
	dimensions_ = dimensions;
	distanceType_ = distType;
	time_ = ros::Time::now();

	setup();
}

/*upo_RRT_ros::ValidityChecker3::ValidityChecker3(tf::TransformListener* tf, unsigned int dimensions, int distType) : StateChecker()
{
	capture_ = new Capture();

	tf_ = tf;
	dimensions_ = dimensions;
	distanceType_ = distType;
	time_ = ros::Time::now();

	//Build a default costmap
	resolution_ = 0.05; 	//m/cell
	width_ = 100;			//cells
 	height_ = 100;			//cells
	origin_.clear();
	origin_.push_back(2.5); //m
	origin_.push_back(2.5); //m
	origin_.push_back(0.0); //rad
    costmap_mutex_.lock();
	costmap_.clear();
	for(unsigned int i=0; i<(width_*height_); i++)
		costmap_.push_back((int)0);
	costmap_mutex_.unlock();

	setup();
}*/

upo_RRT_ros::ValidityChecker3::~ValidityChecker3() {

	delete capture_;
}

void upo_RRT_ros::ValidityChecker3::setup()
{
	ros::NodeHandle nh;

	ros::NodeHandle n("~/Validity_checker");
	string pc_topic;
	n.param<string>("pc_topic", pc_topic, std::string("/scan360/point_cloud3"));
	printf("ValidityChecker3. pc_topic: %s\n", pc_topic.c_str());
	int pc_type;
	n.param<int>("pc_type", pc_type, 2); //1->PointCloud, 2->PointCloud2
	printf("ValidityChecker3. pc_type: %i\n", pc_type);

 	n.param<bool>("project_onto_map", project_onto_map_, false);

	double thres;
	n.param<double>("path_threshold", thres, 0.0);
	path_threshold_ = (float)thres;

	n.param<double>("robot_radius", insc_radius_robot_, 0.30);
	printf("ValidityChecker3. Robot Radius: %.2f\n", insc_radius_robot_);

	goal_sub_ = n.subscribe("/rrt_goal", 1, &ValidityChecker3::goalCallback, this);

	sub_people_ = n.subscribe("/people/navigation", 1, &ValidityChecker3::peopleCallback, this);

 	if(pc_type == 1) 	//pointCloud
		sub_pc_ = n.subscribe(pc_topic, 1, &ValidityChecker3::pcCallback, this);
	else   				//pointCloud2
		sub_pc_ = n.subscribe(pc_topic, 1, &ValidityChecker3::pc2Callback, this);

	//Network prediction service
  	cnn_client_ = nh.serviceClient<path_prediction::PathPrediction>("path_prediction");

	costmap_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("prediction_rewardmap", 1);

	if(project_onto_map_)
		setupStaticMap(nh);
	else
		setupNoMapProjection();

}

void upo_RRT_ros::ValidityChecker3::setupStaticMap(ros::NodeHandle nh)
{

	people_paint_area_ = 25;

	ros::ServiceClient map_client = nh.serviceClient<nav_msgs::GetMap>("/static_map");
	while (! ros::service::waitForService("/static_map",1)){
		ROS_INFO("Waiting for map service");
	}

	nav_msgs::GetMap srv;
	map_client.call(srv);
	ROS_INFO_STREAM(srv.response.map.info);
	map_image_ = cv::Mat(srv.response.map.info.height, srv.response.map.info.width,CV_8UC1, cv::Scalar(0));
	map_metadata_ =srv.response.map.info;
	map_resolution_ = (double) map_metadata_.resolution;
	map_origin_.push_back(map_metadata_.origin.position.x);
	map_origin_.push_back(map_metadata_.origin.position.y);
	map_origin_.push_back(tf::getYaw(map_metadata_.origin.orientation));
	uint8_t *myData = map_image_.data;
	for (int i=0;i<srv.response.map.data.size();i++){
		if (srv.response.map.data.at(i)==100  || srv.response.map.data.at(i)==-1 ){
		}
		else {
			map_image_.data[i] = 255;
		}
	}

	dt_mutex_.lock();
	distance_transform_ = cv::Mat(map_image_.rows,map_image_.cols,CV_32FC1);
	cv::distanceTransform(map_image_,distance_transform_,CV_DIST_L1,3);
	dt_mutex_.unlock();
}

void upo_RRT_ros::ValidityChecker3::setupNoMapProjection()
{
	people_paint_area_ = 25;

	map_image_ = cv::Mat(cm_width_pixs_, cm_height_pixs_, CV_8UC1, cv::Scalar(255));

	map_metadata_.resolution = cm_resolution_;
	map_metadata_.width = cm_width_pixs_;   //Cells
	map_metadata_.height = cm_height_pixs_; //Cells
	geometry_msgs::Pose orig;
	orig.position.x = cm_origin_[0];
	orig.position.y = cm_origin_[1];
	orig.position.z = 0.0;
	orig.orientation = tf::createQuaternionMsgFromYaw(cm_origin_[2]);
	map_metadata_.origin = orig; //m

	dt_mutex_.lock();
	distance_transform_ = cv::Mat(map_image_.rows,map_image_.cols,CV_32FC1);
	//cv::distanceTransform(map_image_,distance_transform_,CV_DIST_L1,3);
	dt_mutex_.unlock();

}

//Update map with point cloud data
void upo_RRT_ros::ValidityChecker3::updateDistTransform(){

	sensor_msgs::PointCloud temp_pt_cloud;
	pc_mutex_.lock();
	sensor_msgs::PointCloud2 lcloud = laser_cloud_;
	pc_mutex_.unlock();

	if(lcloud.data.size() <= 0) {
		ROS_WARN("No cloud updated");
		return;
	}

	bool done = sensor_msgs::convertPointCloud2ToPointCloud(lcloud, temp_pt_cloud);
	if(!done)
		ROS_ERROR("\n\nUpdateDistTransform. convertPointCloud2toPoingCloud!!!!!\n");

	//Add the laser readings (pointcloud) to the map
	dt_mutex_.lock();
	cv::Mat map_copy = map_image_.clone();
	dt_mutex_.unlock();
	for (int i =0;i<temp_pt_cloud.points.size();i++){
		vector<int> pix;
		if(project_onto_map_)
			pix = worldToMap(&(temp_pt_cloud.points[i]),&map_metadata_);
		else
			pix = BaseLinkWorldToImg(&(temp_pt_cloud.points[i]));

		if(pix[0] >= 0.0 && pix[0] < map_metadata_.width && pix[1] >= 0.0 && pix[1] < map_metadata_.height)
			map_copy.at<unsigned char>(pix[1],pix[0]) = 0;
		//else
			//ROS_WARN("\n\nNAV FEATURES. UPDATEDT. pixel out of range!\n");
	}

	//Remove the people detected from the map
	/*people_mutex_.lock();
	std::vector<upo_msgs::PersonPoseUPO> per = people_;
	people_mutex_.unlock();
	for (int i=0; i<per.size(); i++){
		geometry_msgs::Point32 temp_point;
		temp_point.x = per[i].position.x;
		temp_point.y = per[i].position.y;
		vector<int> pix;
		if(project_onto_map_)
			pix = worldToMap(&temp_point,&map_metadata_);
		else
			pix = BaseLinkWorldToImg(&temp_point);

		if (pix[0] >= 0.0 && pix[0] < map_metadata_.width && pix[1] >= 0.0 && pix[1] < map_metadata_.height)
		{
			for (int j = -floor(people_paint_area_/2);j<ceil(people_paint_area_/2);j++){
				for (int k = -floor(people_paint_area_/2);k<ceil(people_paint_area_/2);k++){
					if (pix[1]+k>=0 && pix[0]+j>=0){
						map_copy.at<unsigned char>(pix[1]+k,pix[0]+j) =255;
					}
				}

			}
		}
	}*/

	cv::Mat dt;
	try{
		cv::distanceTransform(map_copy, dt, CV_DIST_L1, 3);
	} catch(...){
		ROS_ERROR("\n\nValidityChecker3. UpdateDistTrans. cv::distanceTransform!!!!!\n");
	}

	dt_mutex_.lock();
	distance_transform_ = dt.clone();
	dt_mutex_.unlock();

	//gettimeofday(&stop, NULL);
	//printf("took %lu\n", stop.tv_usec - start.tv_usec);
	imwrite(ros::package::getPath("upo_rrt_planners").append("/test_write.jpg"), map_copy);
}

//People callback
void upo_RRT_ros::ValidityChecker3::peopleCallback(const upo_msgs::PersonPoseArrayUPO::ConstPtr& msg)
{
	ROS_INFO_ONCE("People received!!");
	people_mutex_.lock();
	people_ = msg->personPoses;
	/*if(people_.size() > 0) {
		people_frame_id_ = msg->header.frame_id;
	}*/
	people_mutex_.unlock();

}

//Point_cloud2 callback
void upo_RRT_ros::ValidityChecker3::pc2Callback(const sensor_msgs::PointCloud2::ConstPtr& pc_in){

	setObstacles(*pc_in);
}

//Point_cloud callback
void upo_RRT_ros::ValidityChecker3::pcCallback(const sensor_msgs::PointCloud::ConstPtr& pc_in){

	sensor_msgs::PointCloud2 pc2;
	bool ok = sensor_msgs::convertPointCloudToPointCloud2(*pc_in, pc2);
	if(!ok) {
		ROS_WARN("NavFeatures. Error transforming pointCloud to pointCloud2");
	}

	setObstacles(pc2);

}

//Goal callback
void upo_RRT_ros::ValidityChecker3::goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	ROS_INFO_ONCE("Goal received!!");
	setGoal(*msg);
}

void upo_RRT_ros::ValidityChecker3::setObstacles(sensor_msgs::PointCloud2 obs)
{
	ROS_INFO_ONCE("Obstacles received!\n");
	sensor_msgs::PointCloud2 lcloud;
	obs.header.stamp = ros::Time();
	try{
		if(project_onto_map_) {
			if(!pcl_ros::transformPointCloud("/map", obs, lcloud, *tf_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		} else {
			if(!pcl_ros::transformPointCloud("/base_link", obs, lcloud, *tf_))
				ROS_WARN("TransformPointCloud failed!!!!!");
		}

	} catch (tf::TransformException ex){
		ROS_WARN("NAV FEATURES. pcCallback. TransformException: %s", ex.what());
	}

	pc_mutex_.lock();
	laser_cloud_ = lcloud;
	pc_mutex_.unlock();
}

//Publish the path planning prediction into a ros costmap for visualization
void upo_RRT_ros::ValidityChecker3::publish_costmap_ros(ros::Time t)
{
	//Get the robot coordinates in odom frame
	tf::StampedTransform transform;
	try{
		tf_->waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(1.0));
		tf_->lookupTransform("/odom", "/base_link",  ros::Time(0), transform); //t
	}
	catch (tf::TransformException ex){
		ROS_ERROR("Publish_feature_costmap. TF exception: %s",ex.what());
	}

	nav_msgs::OccupancyGrid cmap;
	cmap.header.frame_id = "odom"; //"base_link";
	cmap.header.stamp = ros::Time::now(); //t;
	//time map_load_time. The time at which the map was loaded
	cmap.info.map_load_time = cmap.header.stamp; //t;
	//float32 resolution. The map resolution [m/cell]
	cmap.info.resolution = cm_resolution_;  //0.25 m/cell
	//uint32 width. Map width [cells]
	cmap.info.width = cm_height_pixs_; //x
	//uint32 height. Map height [cells]
	cmap.info.height = cm_width_pixs_; //y
	//geometry_msgs/Pose origin. The origin of the map [m, m, rad].  This is the real-world pose of the
	// cell (0,0) in the map.
	geometry_msgs::Pose p;
	p.position.x = transform.getOrigin().x()-(cm_height_/2.0); //x
	p.position.y = transform.getOrigin().y()-(cm_width_/2.0);  //y (substraction initially)
	p.position.z = 0.0;
	p.orientation = tf::createQuaternionMsgFromYaw(0.0); //robot_odom_h
	cmap.info.origin = p;
	//int8[] cmap.data. The map data, in row-major order, starting with (0,0).  Occupancy
	// probabilities are in the range [0,100].  Unknown is -1.
	std::vector<signed char> data; // size =(cmap.info.width*cmap.info.height)
	double cost = 0.0;
	//for(int i=0; i<cmap.info.height; i++)
	for(int i=(cmap.info.height-1); i>0; i--)
	{
		for(unsigned int j=0; j<cmap.info.width; j++)
		{
			float reward = costmap_[i*cm_width_pixs_ + j]>1.0?1.0:costmap_[i*cm_width_pixs_ + j];
			//cost = (1 - reward);
			//Transform cost into the scale[0,100]
			data.push_back((int)round(reward*100.0));
		}
	}
	cmap.data = data;
	costmap_pub_.publish(cmap);
}

bool upo_RRT_ros::ValidityChecker3::predictionService(vector<int> input, int rows, int cols)
{
	path_prediction::PathPrediction srv;
  	srv.request.input = input;
  	srv.request.input_rows = rows;
	srv.request.input_cols = cols;
  	if (cnn_client_.call(srv))
  	{
    	ROS_INFO("Service path prediction called!");
		costmap_mutex_.lock();
		costmap_.clear();
		costmap_ = srv.response.prediction;
		cm_width_pixs_ = srv.response.pred_cols;
		cm_height_pixs_ = srv.response.pred_rows;
		cm_width_ = cm_width_pixs_ * cm_resolution_;
		cm_height_ = cm_height_pixs_ * cm_resolution_;
		costmap_mutex_.unlock();

		/*printf("Prediction received:\n");
		printf("Vector size: %u\n", (unsigned int)costmap_.size());
		for(unsigned int i=0; i<cm_height_pixs_; i++)
		{
			for(unsigned int j=0; j<cm_width_pixs_; j++)
				printf("%.2f ", costmap_[i*cm_width_pixs_ + j]);
			printf("\n");
		}*/

		return true;
  	}
  	else
  	{
    	ROS_ERROR("Failed to call service path_prediction");
    	return false;
  	}

}

//Update the navigation scene and get the path planning prediction
void upo_RRT_ros::ValidityChecker3::preplanning_computations()
{
	struct timeval t_ini1, t_fin1;
	gettimeofday(&t_ini1, NULL);
	//if(project_onto_map_)
		updateDistTransform();

	//Take point cloud
	pc_mutex_.lock();
	sensor_msgs::PointCloud2 pc2 = laser_cloud_; //base_link
	pc_mutex_.unlock();
	sensor_msgs::PointCloud pc;
	bool done = sensor_msgs::convertPointCloud2ToPointCloud(pc2, pc);
	if(!done)
		ROS_ERROR("\n\nPreplanning computations. convertPointCloud2toPointCloud!!!!!\n");

	//Take people
	people_mutex_.lock();
	vector<upo_msgs::PersonPoseUPO> p = people_;	//odom
	people_mutex_.unlock();

	//Take goal
	goal_mutex_.lock();
	geometry_msgs::PoseStamped g = goal_;			//base_link
	goal_mutex_.unlock();

	//Form current navigation scene
	vector<int> input = capture_->generateImg(&pc, &p, &g);
	gettimeofday(&t_fin1, NULL);
	double secs1 = capture_->timeval_diff(&t_fin1, &t_ini1);

	/*printf("Nav scene vector generated: \n");
	for(unsigned int i=0; i<200; i++) {
		for(unsigned int j=0; j<200; j++)
			printf("%i", input[((i*200)+j)]);
		printf("\n");
	}*/

	struct timeval t_ini2, t_fin2;
	gettimeofday(&t_ini2, NULL);
	//Update the path prediction (costmap)
	predictionService(input, cm_height_pixs_, cm_width_pixs_);
	gettimeofday(&t_fin2, NULL);
	double secs2 = capture_->timeval_diff(&t_fin2, &t_ini2);
	printf("Preplanning times:\n \tGenerate input: %.3f msecs\n \tGeneratePrediction: %.3f msecs\n", (secs1*1000.0), (secs2*1000.0) );
}

vector<upo_RRT::State> upo_RRT_ros::ValidityChecker3::getPredictedPointList()
{
	vector<upo_RRT::State> points;
	costmap_mutex_.lock();
	for(unsigned int i=0; i<cm_height_pixs_; i++) {
		for(unsigned int j=0; j<cm_width_pixs_; j++) {
			if(costmap_[i*cm_width_pixs_+j] > path_threshold_) {
				vector<int> cell;
				cell.push_back(j); //x
				cell.push_back(i); //y
				vector<float> pos = costmapCellToPose(cell);
				upo_RRT::State state(pos[0], pos[1], 0.0); //x, y, yaw
				points.push_back(state);
			}
		}
	}
	costmap_mutex_.unlock();
	return points;
}

// Transform a point in the world to the pixels in the static map
vector<int> upo_RRT_ros::ValidityChecker3::worldToMap(geometry_msgs::Point32* world_point,nav_msgs::MapMetaData* map_metadata) const {
	vector<int> pixels;
	float x_map = world_point->x - map_metadata->origin.position.x;
	float y_map = world_point->y - map_metadata->origin.position.y;
	pixels.push_back((int)floor(x_map/map_metadata->resolution ));
	pixels.push_back((int)floor(y_map/map_metadata->resolution));
	return pixels;
}

/**
* transform point in base link frame (m) to pixel
*/
vector<int> upo_RRT_ros::ValidityChecker3::BaseLinkWorldToImg(geometry_msgs::Point32* point) const
{
	vector<int> pix;
	float wx = cm_origin_[0] + point->x;
	float wy = cm_origin_[1] + point->y;
	pix.push_back((int)floor(wx/cm_resolution_));
	pix.push_back((int)floor(wy/cm_resolution_));
	return pix;

}

vector<int> upo_RRT_ros::ValidityChecker3::poseToCostmapCell(vector<float> pose) const
{
	//Be careful, I'm not taking into account the rotation because is zero usually.
	//Pose is in robot frame coordinates, and the robot is centered in the costmap
	int x =  floor(fabs((pose[0] + cm_origin_[0])/cm_resolution_)); //x
	int y =  floor(fabs((pose[1] - cm_origin_[1])/cm_resolution_)); //y
 	vector<int> cell;
	cell.push_back(x);
	cell.push_back(y);
	return cell;
}

vector<float> upo_RRT_ros::ValidityChecker3::costmapCellToPose(vector<int> cell) const
{
	//Be careful, I'm not taking into account the rotation because is zero usually.
	float x = (cell[0]*cm_resolution_) - cm_origin_[0]  + (cm_resolution_/2.0);  //-
	float y = (-1)*((cell[1]*cm_resolution_) - cm_origin_[1]  + (cm_resolution_/2.0)); //+
	vector<float> pose;
	pose.push_back(x);
	pose.push_back(y);
	return pose;
}

//This checking is done over the static map (updated with the point cloud or not)
bool upo_RRT_ros::ValidityChecker3::isValid(upo_RRT::State* s) const
{
	geometry_msgs::PoseStamped p_in;
	p_in.header.frame_id = "base_link";
	//p_in.header.stamp = ros::Time(0); //this is a problem when the planning time is long. the time stamp should be the time when the rrt started to plan.
	if((ros::Time::now()-time_).toSec() > 2.0) {
		//time_ = ros::Time::now();
		p_in.header.stamp = ros::Time(0);
	} else
		p_in.header.stamp = time_;

	p_in.pose.position.x = s->getX();
	p_in.pose.position.y = s->getY();
	p_in.pose.orientation = tf::createQuaternionMsgFromYaw(s->getYaw());

	//Transform the coordinates
	float res;
	vector<int> pix;
	geometry_msgs::PoseStamped sm;
	if(project_onto_map_) {
		res = map_metadata_.resolution;
		sm = transformPoseTo(&p_in, string("/map"), false);
		geometry_msgs::Point32 point;
		point.x = sm.pose.position.x;
		point.y = sm.pose.position.y;
		point.z = 0.0;
		//pix = worldToMap(&point, &map_metadata_);
		float x_map = point.x - map_metadata_.origin.position.x;
		float y_map = point.y - map_metadata_.origin.position.y;
		pix.push_back((int)floor(x_map/map_metadata_.resolution));
		pix.push_back((int)floor(y_map/map_metadata_.resolution));

	}else {
		res = cm_resolution_;
		//sm = transformPoseTo(&p_in, string("/base_link"), false);
		geometry_msgs::Point32 point;
		point.x = s->getX(); //sm.pose.position.x;
		point.y = s->getY(); //sm.pose.position.y;
		point.z = 0.0;
		pix = BaseLinkWorldToImg(&point);
	}

	float px = pix[0];
	float py = pix[1];
	float distance = 0.0;
	//dt_mutex_.lock();
	if (py<0 || px<0  || px > map_image_.cols || py > map_image_.rows) {
		distance = 0.0;
	} else{
		try{

			distance = distance_transform_.at<float>(py,px)*res;

		} catch(...)
		{
			ROS_ERROR("ERROR. IS_VALID. px:%.2f, py:%.2f, distance:%.2f", px, py, distance);
			distance = 0.0;
		}
	}
	//dt_mutex_.unlock();
	// Take into account the robot radius
	if(distance <= insc_radius_robot_) {
		return false;
	}else {

		return true;
	}

}

float upo_RRT_ros::ValidityChecker3::distance(upo_RRT::State* s1, upo_RRT::State* s2) const
{
	float dx = s1->getX() - s2->getX();
	float dy = s1->getY() - s2->getY();
	//float dist = sqrt(dx*dx + dy*dy);
	float dist = dx*dx + dy*dy;

	switch(distanceType_) {

		case 1:
			return dist;

		case 2:
			return sqrt(dist);

		case 3:
			if(dimensions_ == 2)
				return sqrt(dist);
			else {
				//SUM w1*|| Pi+1 - Pi|| + w2*(1-|Qi+1 * Qi|)²
				float euc_dist = sqrt(dist);

				tf::Quaternion q1 = tf::createQuaternionFromYaw(s1->getYaw());
				tf::Quaternion q2 = tf::createQuaternionFromYaw(s2->getYaw());
				float dot_prod = q1.dot(q2);
				float angle_dist =  (1 - fabs(dot_prod))*(1 - fabs(dot_prod));
				return 0.7*euc_dist + 0.3*angle_dist;
			}

		case 4:
			if(dimensions_ == 2)
				return sqrt(dist);
			else {
				// Another option
				/*
				First, transform the robot location into person location frame:
											|cos(th)  sin(th)  0|
					Rotation matrix R(th)= 	|-sin(th) cos(th)  0|
											|  0        0      1|

					x' = (xr-xp)*cos(th_p)+(yr-yp)*sin(th_p)
					y' = (xr-xp)*(-sin(th_p))+(yr-yp)*cos(th_p)
				*/
				float x = (s2->getX()-s1->getX())*cos(s1->getYaw()) + (s2->getY()-s1->getY())*sin(s1->getYaw());
				float y =-(s2->getX()-s1->getX())*sin(s1->getYaw()) + (s2->getY()-s1->getY())*cos(s1->getYaw());
				float alpha = atan2(y, x);
				return (0.8*sqrt(dist)+0.2*fabs(alpha));
			}

		case 5:
			if(dimensions_ == 2)
				return sqrt(dist);
			else {
				//UPO. Dist + sum of the angles of both points regarding the intersection line
				float x = (s2->getX()-s1->getX())*cos(s1->getYaw()) + (s2->getY()-s1->getY())*sin(s1->getYaw());
				float y =-(s2->getX()-s1->getX())*sin(s1->getYaw()) + (s2->getY()-s1->getY())*cos(s1->getYaw());
				float alpha = atan2(y, x);
				float beta = s2->getYaw() - alpha;
				beta = normalizeAngle(beta, -M_PI, M_PI);
				return (0.6*sqrt(dist)+0.4*(fabs(alpha)+fabs(beta)));
			}

		case 6:
			if(dimensions_ == 2)
				return sqrt(dist);
			else {
				//Paper IROS2015 "Feedback motion planning via non-holonomic RRT* for mobile robots"
				float x = (s2->getX()-s1->getX())*cos(s1->getYaw()) + (s2->getY()-s1->getY())*sin(s1->getYaw());
				float y =-(s2->getX()-s1->getX())*sin(s1->getYaw()) + (s2->getY()-s1->getY())*cos(s1->getYaw());
				float alpha = atan2(y, x);
				float phi = s2->getYaw() - alpha;
				phi = normalizeAngle(phi, -M_PI, M_PI);
				float ka = 0.5;
				float ko = ka/8.0;
				dist = sqrt(dist);
				// two options
				float alpha_prime = atan(-ko*phi);
				//float alpha_prime = atan(-ko*ko * phi/(dist*dist));
				float r = normalizeAngle((alpha-alpha_prime), -M_PI, M_PI);
				return (sqrt(dist*dist + ko*ko + phi*phi) + ka*fabs(r));
			}

		default:
			return sqrt(dist);
	}

}

float upo_RRT_ros::ValidityChecker3::getCost(upo_RRT::State* s)
{
	//cell.first = x = column, cell.second = y = row
	vector<float> pose;
	pose.push_back((float)s->getX());
	pose.push_back((float)s->getY());
	costmap_mutex_.lock();
	vector<int> cell = poseToCostmapCell(pose);
	//vector<int> cell = BaseLinkWorldToImg(&p);
	int ind = (cell[1]*cm_width_pixs_) + cell[0];
	//if(ind < 0 || ind >= (200*200))
	//	printf("GetCost. Ind out of range: %i\n", ind);
	float reward = costmap_[ind]>1.0?1.0:costmap_[ind];
	//if(reward == 0.0)
	//	return 10.0;
	//printf("r: %.2f\t", reward);
	costmap_mutex_.unlock();
	return (1-reward); //*1000.0 //Value between [0,1]
}

geometry_msgs::PoseStamped upo_RRT_ros::ValidityChecker3::transformPoseTo(geometry_msgs::PoseStamped* pose_in, std::string frame_out, bool usetime) const {

	geometry_msgs::PoseStamped in = *pose_in;
	if(!usetime)
		in.header.stamp = ros::Time();

	geometry_msgs::PoseStamped pose_out;

	try {
		tf_->transformPose(frame_out.c_str(), in, pose_out);
	}catch (tf::TransformException ex){
		ROS_WARN("ValidityChecker3. TransformException in method transformPoseTo. TargetFrame: %s : %s", frame_out.c_str(), ex.what());
	}
	return pose_out;

}

bool upo_RRT_ros::ValidityChecker3::isQuaternionValid(const geometry_msgs::Quaternion q) {

		//first we need to check if the quaternion has nan's or infs
		if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
			ROS_ERROR("Quaternion has infs!!!!");
			return false;
		}
		if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w)) {
			ROS_ERROR("Quaternion has nans !!!");
			return false;
		}

		if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01) {
			ROS_ERROR("Quaternion malformed, magnitude: %.3f should be 1.0", (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w));
			return false;
		}

		tf::Quaternion tf_q(q.x, q.y, q.z, q.w);

		//next, we need to check if the length of the quaternion is close to zero
		if(tf_q.length2() < 1e-6){
		  ROS_ERROR("Quaternion has length close to zero... discarding.");
		  return false;
		}

		//next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
		tf_q.normalize();

		tf::Vector3 up(0, 0, 1);

		double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

		if(fabs(dot - 1) > 1e-3){
		  ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
		  return false;
		}

		return true;

}


/*
 * MIT License
 *
 * Copyright (c) 2018 Tech Solutions Malta LTD
 *
 * 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 <graphene/chain/das33_evaluator.hpp>
#include <graphene/chain/database.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <graphene/chain/market_object.hpp>

namespace graphene { namespace chain {

  // Helper methods:
  share_type users_total_pledges_in_round(account_id_type user_id, das33_project_id_type project_id, share_type round, const database& d)
  {
    share_type sum = 0;
    const auto& idx = d.get_index_type<das33_pledge_holder_index>().indices().get<by_user>().equal_range(user_id);
    for( auto it = idx.first; it != idx.second; ++it )
    {
      if (it->project_id == project_id && it->phase_number == round)
        sum += (it->base_expected.amount);
    }
    return sum;
  }

  void price_check(const price& price_to_check, asset_id_type first_asset, asset_id_type second_asset)
  {
    FC_ASSERT(price_to_check.base.asset_id == first_asset || price_to_check.quote.asset_id == first_asset,
              "Price must be for ${1}", ("1", first_asset));
    FC_ASSERT(price_to_check.base.asset_id == second_asset || price_to_check.quote.asset_id == second_asset,
                  "Price must be for ${1}", ("1", second_asset));
  }

  share_type precision_modifier(asset_object a, asset_object b)
  {
    share_type result = 1;
    if (a.precision > b.precision)
    {
      result = std::pow(10, a.precision - b.precision);
    }
    return result;
  }

  optional<price> calculate_price(asset_id_type asset_id, das33_project_id_type project_id, const database& d)
  {
      const auto& project_obj = project_id(d);
      const auto& price_override_it = project_obj.price_override.find(asset_id);

      // Check if we are in alliance pay project and HF time ...
      if (asset_id == d.get_dascoin_asset_id()
              && project_id == das33_project_id_type{3}
              && d.head_block_time() > HARDFORK_FIX_PLEDGE_PRICE_START
              && d.head_block_time() < HARDFORK_FIX_PLEDGE_PRICE_END)
      {
          // .. if yes set fixed price to 0.1 We
          return price{asset{100000, d.get_dascoin_asset_id()}, asset{10, d.get_web_asset_id()}};
      }
      else if (asset_id == d.get_dascoin_asset_id()
               && project_id == das33_project_id_type{2}
               && d.head_block_time() > HARDFORK_FIX_PLEDGE_PRICE_START
               && d.head_block_time() < HARDFORK_FIX_PLEDGE_PRICE_END)
      {
          // .. or if we are in Greenstorc and HF time set fixed price to 0.0188 We
          return price{asset{10000000, d.get_dascoin_asset_id()}, asset{188, d.get_web_asset_id()}};
      }
      else if (price_override_it != project_obj.price_override.end())
      {
          // ... or if we have price override, use that
          return (*price_override_it).second;
      }
      else
      {
          // .. otherwise get price from db
          return d.get_price_in_web_eur(asset_id);
      }
  }

  typedef boost::multiprecision::uint128_t uint128_t;

  asset asset_price_multiply ( const asset& a, int64_t precision, const price& b, const price& c )
  {
    uint128_t result;
     if( a.asset_id == b.base.asset_id )
     {
        FC_ASSERT( b.base.amount.value > 0 );
        result = (uint128_t(a.amount.value) * precision * b.quote.amount.value)/b.base.amount.value;
        if (b.quote.asset_id == c.base.asset_id)
        {
          FC_ASSERT( c.base.amount.value > 0 );
          result = (result * c.quote.amount.value)/c.base.amount.value;
          result = result / precision;
          return asset( result.convert_to<int64_t>(), c.quote.asset_id );
        }
        else
        {
          FC_ASSERT( c.quote.amount.value > 0 );
          result = (result * c.base.amount.value)/c.quote.amount.value;
          result = result / precision;
          return asset( result.convert_to<int64_t>(), c.base.asset_id );
        }

     }
     else if( a.asset_id == b.quote.asset_id )
     {
        FC_ASSERT( b.quote.amount.value > 0 );
        result = (uint128_t(a.amount.value) * precision * b.base.amount.value)/b.quote.amount.value;
        if (b.base.asset_id == c.base.asset_id)
        {
          FC_ASSERT( c.base.amount.value > 0 );
          result = (result * c.quote.amount.value)/c.base.amount.value;
          result = result / precision;
          return asset( result.convert_to<int64_t>(), c.quote.asset_id );
        }
        else
        {
          FC_ASSERT( c.quote.amount.value > 0 );
          result = (result * c.base.amount.value)/c.quote.amount.value;
          result = result / precision;
          return asset( result.convert_to<int64_t>(), c.base.asset_id );
        }
     }
     FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
  }

  // method implementations:

  void_result das33_project_create_evaluator::do_evaluate( const operation_type& op )
  {
    try {
      const auto& d = db();
      const auto& gpo = d.get_global_properties();

      // Check authority
      const auto& authority_obj = op.authority(d);
      d.perform_chain_authority_check("das33 authority", gpo.authorities.das33_administrator, authority_obj);

      // Check that name is unique
      const auto& idx = d.get_index_type<das33_project_index>().indices().get<by_project_name>();
      FC_ASSERT(idx.find(op.name) == idx.end(), "Das33 project called ${1} already exists.", ("1", op.name));

      // Check that owner is a wallet
      const auto& owner_obj = op.owner(d);
      FC_ASSERT( owner_obj.is_wallet(), "Owner account '${name}' is not a wallet account", ("name", owner_obj.name));

      // Check that token exists
      const auto& token_index = d.get_index_type<asset_index>().indices().get<by_id>();
      FC_ASSERT(token_index.find(op.token) != token_index.end(), "Token with id ${1} does not exist", ("1", op.token));

      // Check that token has max_supply
      FC_ASSERT(op.token(d).options.max_supply > 0, "Token must have max_supply > 0");

      // Check that token isn't one of the system assets
      FC_ASSERT(op.token != d.get_core_asset().id
             && op.token != d.get_web_asset_id()
             && op.token != d.get_dascoin_asset_id()
             && op.token != d.get_btc_asset_id()
             && op.token != d.get_cycle_asset_id(), "Can not create project with system assets");

      // Check that token is not used by another project
      const auto& it = std::find_if(idx.begin(), idx.end(),
            [op](const das33_project_object& m) -> bool { return m.token_id == op.token; });
      FC_ASSERT(it == idx.end(), "Token with id ${1} is already used by another project", ("1", op.token));

      // Check that discounts exist
      FC_ASSERT(op.discounts.size() > 0, "Discounts must be provided. Only assets in discounts can be pledged.");

      // Check that discounts are in (0,1] range
      for (auto itr = op.discounts.begin(); itr != op.discounts.end(); itr++)
      {
        FC_ASSERT(itr->second > 0,
                  "Discount can not be zero or negative (${name}:${value})",
                  ("name", itr->first)
                  ("value", itr->second));
        FC_ASSERT(itr->second <= 1 * BONUS_PRECISION,
                  "Discount can not be larger than 1.00 (${name}:${value})",
                  ("name", itr->first)
                  ("value", itr->second));
      }
      return {};

    } FC_CAPTURE_AND_RETHROW((op))
  }

  object_id_type das33_project_create_evaluator::do_apply( const operation_type& op )
  {
    try {
      auto& d = db();

      const asset_object token = op.token(d);
      const asset max_supply {token.options.max_supply, token.id};
      const asset to_collect {op.goal_amount_eur, d.get_web_asset_id()};
      const price token_price = to_collect / max_supply;

      return d.create<das33_project_object>([&](das33_project_object& dpo){
             dpo.name = op.name;
             dpo.owner = op.owner;
             dpo.token_id = op.token;
             dpo.goal_amount_eur = op.goal_amount_eur;
             dpo.discounts = op.discounts;
             dpo.min_pledge = op.min_pledge;
             dpo.max_pledge = op.max_pledge;
             dpo.token_price = token_price;
             dpo.collected_amount_eur = 0;
             dpo.tokens_sold = 0;
             dpo.status = das33_project_status::inactive;
             dpo.phase_number = 0;
             dpo.phase_limit = max_supply.amount;
             dpo.phase_end = time_point_sec::min();
           }).id;
    } FC_CAPTURE_AND_RETHROW((op))
  }

  void_result das33_project_update_evaluator::do_evaluate( const operation_type& op )
  {
    try {
      const auto& d = db();
      const auto& gpo = d.get_global_properties();

      // Check authority
      const auto& authority_obj = op.authority(d);
      d.perform_chain_authority_check("das33 authority", gpo.authorities.das33_administrator, authority_obj);

      // Get project
      const auto& idx = d.get_index_type<das33_project_index>().indices().get<by_id>();
      auto project_iterator = idx.find(op.project_id);
      FC_ASSERT(project_iterator != idx.end(), "Das33 project with id ${1} does not exist.", ("1", op.project_id));
      project_to_update = &(*project_iterator);

      // Check name
      if (op.name.valid())
      {
        const auto& name_index = d.get_index_type<das33_project_index>().indices().get<by_project_name>();
        FC_ASSERT(name_index.find(*op.name) == name_index.end(), "Das33 project called ${1} already exists.", ("1", *op.name));
      }

      // Check owner
      if (op.owner.valid())
      {
        const auto& owner_obj = (*op.owner)(d);
        FC_ASSERT( owner_obj.is_wallet(), "Owner account '${name}' is not a wallet account", ("name", owner_obj.name));
      }

      // Check price
      if (op.token_price.valid())
      {
        price_check(*op.token_price, d.get_web_asset_id(), project_to_update->token_id);
      }

      // Check bonuses
      if (op.discounts.valid())
      {
        // Check that discounts are in (0,1] range
        for (auto itr = op.discounts->begin(); itr != op.discounts->end(); itr++)
        {
          FC_ASSERT(itr->second > 0,
                    "Discount can not be zero or negative (${name}:${value})",
                    ("name", itr->first)
                    ("value", itr->second));
          FC_ASSERT(itr->second <= 1 * BONUS_PRECISION,
                    "Discount can not be larger than 1.00  (${name}:${value})",
                    ("name", itr->first)
                    ("value", itr->second));
        }
      }

      // Check phase number
      if (op.phase_number.valid())
      {
        FC_ASSERT(*op.phase_number > project_to_update->phase_number, "Phase number can not be decreased");
      }

      // Check phase limit
      if (op.phase_limit.valid())
      {
        share_type new_limit = *op.phase_limit;
        asset_object token = project_to_update->token_id(d);
        // If previous limit is not max supply
        if (project_to_update->phase_limit != token.options.max_supply )
        {
          // New limit must be more than what is already collected
          FC_ASSERT(new_limit >= project_to_update->tokens_sold, "New phase limit must be more than already collected");
        }
        FC_ASSERT(new_limit <= token.options.max_supply, "New limit can not be more then max supply of token");
      }

      // Check status
      if (op.status.valid())
      {
        FC_ASSERT(*op.status < das33_project_status::DAS33_PROJECT_STATUS_COUNT, "Unknown status value");
      }

      return {};
    } FC_CAPTURE_AND_RETHROW((op))
  }

  void_result das33_project_update_evaluator::do_apply( const operation_type& op )
  {
    try {
      auto& d = db();

      d.modify<das33_project_object>(*project_to_update, [&](das33_project_object& dpo){
        if (op.name) dpo.name = *op.name;
        if (op.owner) dpo.owner = *op.owner;
        // token_id: we can't alter
        if (op.goal_amount) dpo.goal_amount_eur = *op.goal_amount;
        if (op.discounts) dpo.discounts = *op.discounts;
        if (op.min_pledge) dpo.min_pledge = *op.min_pledge;
        if (op.max_pledge) dpo.max_pledge = *op.max_pledge;
        if (op.token_price) dpo.token_price = *op.token_price;
        // collected_amount_eur: we can't alter
        // tokens_sold: we can't alter
        if (op.status) dpo.status = static_cast<das33_project_status>(*op.status);
        if (op.phase_number) dpo.phase_number = *op.phase_number;
        if (op.phase_limit) dpo.phase_limit = *op.phase_limit;
        if (op.phase_end) dpo.phase_end = *op.phase_end;

        for (const auto& ext : op.extensions)
        {
           ext.visit(das33_project_visitor{dpo.report});
        }

        auto new_price_override_it = std::find_if(op.extensions.begin(), op.extensions.end(),
                                         [](const das33_project_update_operation::das33_project_extension& ext){
                                               return ext.which() == das33_project_update_operation::das33_project_extension::tag< map<asset_id_type, price> >::value;
                                        });
        if (new_price_override_it != op.extensions.end())
        {
            dpo.price_override = (*new_price_override_it).get<map<asset_id_type, price>>();
        }
      });

      return {};
    } FC_CAPTURE_AND_RETHROW((op))
  }

  void_result das33_project_delete_evaluator::do_evaluate( const operation_type& op )
  {
    try {
      const auto& d = db();
      const auto& gpo = d.get_global_properties();

      const auto& authority_obj = op.authority(d);
      d.perform_chain_authority_check("das33 authority", gpo.authorities.das33_administrator, authority_obj);

      const auto& idx = d.get_index_type<das33_project_index>().indices().get<by_id>();
      auto project_iterator = idx.find(op.project_id);
      FC_ASSERT(project_iterator != idx.end(), "Das33 project with id ${1} does not exist.", ("1", op.project_id));
      project_to_delete = &(*project_iterator);

      const auto& pledges_idx = d.get_index_type<das33_pledge_holder_index>().indices().get<by_project>().equal_range(op.project_id);
      //auto pledges_iterator = pledges_idx.begin();
      FC_ASSERT(pledges_idx.first == pledges_idx.second, "Project can not be deleted as it has pledges");

      return {};
    } FC_CAPTURE_AND_RETHROW((op))
  }

  void_result das33_project_delete_evaluator::do_apply( const operation_type& op )
  {
    try {
      auto& d = db();

      d.remove(*project_to_delete);

      return {};
    } FC_CAPTURE_AND_RETHROW((op))
  }

  void_result das33_pledge_asset_evaluator::do_evaluate(const das33_pledge_asset_operation& op)
  { try {

    const auto& d = db();
    const auto& project_obj = op.project_id(d);
    const auto& token_obj = project_obj.token_id(d);

    // Check if pledged asset and project token are different assets
    FC_ASSERT( op.pledged.asset_id != project_obj.token_id, "Cannot pledge project tokens" );

    // Assure target project exists:
    const auto& idx = d.get_index_type<das33_project_index>().indices().get<by_id>();
    FC_ASSERT( idx.find(project_obj.id) != idx.end(), "Bad project id" );

    // Check if project is active
    FC_ASSERT(project_obj.status == das33_project_status::active, "Pladge can only be made to active project");

    // Assure we have enough balance to pledge:
    const auto& balance_obj = d.get_balance_object(op.account_id, op.pledged.asset_id);
    FC_ASSERT( balance_obj.get_balance() >= op.pledged,
               "Not enough balance on user account ${a}, left ${l}, needed ${n}",
               ("a", op.account_id)
               ("l", d.to_pretty_string(balance_obj.get_balance()))
               ("n", d.to_pretty_string(op.pledged))
    );

    // Assure current phase hasn't ended
    if (project_obj.phase_end != time_point_sec::min())
    {
      FC_ASSERT(d.head_block_time() < project_obj.phase_end, "Can not pledge: new ICO phase hasn;t started yet");
    }

    // Assure that all tokens aren't sold
    FC_ASSERT(project_obj.tokens_sold < token_obj.options.max_supply, "All tokens for project are sold");
    FC_ASSERT(project_obj.tokens_sold < project_obj.phase_limit, "All tokens in this phase are sold");

    // Calculate expected amount
    share_type precision = precision_modifier(op.pledged.asset_id(d), d.get_web_asset_id()(d));
    total.asset_id = project_obj.token_id;

    optional<price> conversion_price = calculate_price(op.pledged.asset_id, op.project_id, d);

    FC_ASSERT(conversion_price.valid(), "There is no proper price for ${asset}", ("asset", op.pledged.asset_id));

    price_at_evaluation = *conversion_price;
    base = asset_price_multiply(op.pledged, precision.value, price_at_evaluation, project_obj.token_price);

    // Assure that pledge amount is above minimum
    if (!(op.pledged.asset_id == d.get_dascoin_asset_id()
                   && op.project_id == das33_project_id_type{2}
                   && d.head_block_time() > HARDFORK_FIX_PLEDGE_PRICE_START
                   && d.head_block_time() < HARDFORK_FIX_PLEDGE_PRICE_END))
    {
        FC_ASSERT(base.amount >= project_obj.min_pledge, "Can not pledge: must buy at least ${min} tokens", ("min", project_obj.min_pledge));
    }

    // Assure that pledge amount is below maximum for current user
    auto previous_pledges = users_total_pledges_in_round(op.account_id, op.project_id, project_obj.phase_number, d);
    if (op.pledged.asset_id == d.get_dascoin_asset_id()
            && op.project_id == das33_project_id_type{3}
            && d.head_block_time() > HARDFORK_FIX_PLEDGE_PRICE_START
            && d.head_block_time() < HARDFORK_FIX_PLEDGE_PRICE_END)
    {
        FC_ASSERT( previous_pledges + base.amount <= 100000000000000,
                  "Can not buy more then ${max} tokens per phase and you already pledged for ${previous} in this phase.",
                  ("max", project_obj.max_pledge)
                  ("previous", previous_pledges));
    }
    else
    {
        FC_ASSERT( previous_pledges + base.amount <= project_obj.max_pledge,
                  "Can not buy more then ${max} tokens per phase and you already pledged for ${previous} in this phase.",
                  ("max", project_obj.max_pledge)
                  ("previous", previous_pledges));
    }

    // Calculate expected amount with discounts
    auto discount_iterator = project_obj.discounts.find(op.pledged.asset_id);
    FC_ASSERT( discount_iterator != project_obj.discounts.end(), "This asset can not be used in this project phase" );
    discount = discount_iterator->second;
    total.amount = base.amount * BONUS_PRECISION / discount;

    // Assure that pledge amount is above zero
    FC_ASSERT(total.amount > 0,
              "Cannot pledge because expected amount of ${tok} is ${ex}",
              ("tok", token_obj.symbol)
              ("ex", d.to_pretty_string(total))
    );

    bool total_reduced = false;
    // Decrease amount if it passes tokens max supply
    if (project_obj.tokens_sold + total.amount > token_obj.options.max_supply)
    {
        total.amount = token_obj.options.max_supply - project_obj.tokens_sold;
        total_reduced = true;
    }

    // Decrease amount if it passes current phase limit
    if (project_obj.tokens_sold + total.amount > project_obj.phase_limit)
    {
        total.amount = project_obj.phase_limit - project_obj.tokens_sold;
        total_reduced = true;
    }

    if (total_reduced)
    {
      base = {total.amount * discount / BONUS_PRECISION, total.asset_id};
      bonus = total - base;
      precision = precision_modifier(base.asset_id(d), d.get_web_asset_id()(d));
      to_take = asset_price_multiply(base, precision.value, project_obj.token_price, price_at_evaluation);
    }
    else
    {
      bonus = total - base;
      to_take = op.pledged;
    }

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  object_id_type das33_pledge_asset_evaluator::do_apply(const das33_pledge_asset_operation& op)
  { try {

    auto& d = db();
    const auto& project_obj = op.project_id(d);

    // Adjust the balance and spent amount:
    const auto& balance_obj = d.get_balance_object(op.account_id, op.pledged.asset_id);
    d.modify(balance_obj, [&](account_balance_object& from){
      from.balance -= to_take.amount;
      from.spent += to_take.amount;
    });

    // Update project
    d.modify(project_obj, [&](das33_project_object& p){
        p.tokens_sold += total.amount;
        p.collected_amount_eur += (to_take * price_at_evaluation).amount;
    });

    // Create the holder object and return its ID:
    return d.create<das33_pledge_holder_object>([&](das33_pledge_holder_object& cpho){
      cpho.account_id = op.account_id;
      cpho.pledged = to_take;
      cpho.pledge_remaining = to_take;
      cpho.base_remaining = base;
      cpho.base_expected = base;
      cpho.bonus_remaining = bonus;
      cpho.bonus_expected = bonus;
      cpho.phase_number = project_obj.phase_number;
      cpho.project_id = op.project_id;
      cpho.timestamp = d.head_block_time();
    }).id;

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_distribute_project_pledges_evaluator::do_evaluate(const das33_distribute_project_pledges_operation& op)
  { try {

     auto& d = db();

     auto& pro_index = d.get_index_type<das33_project_index>().indices().get<by_id>();
     auto pro_itr = pro_index.find(op.project);
     account_id_type pro_owner;
     FC_ASSERT(pro_itr != pro_index.end(), "Missing project object with this project_id!");

     _pro_owner = pro_itr->owner;

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_distribute_project_pledges_evaluator::do_apply(const das33_distribute_project_pledges_operation& op)
  { try {

    auto& d = db();

    std::vector<object_id_type> pledges_to_remove;
    const auto& index = d.get_index_type<das33_pledge_holder_index>().indices().get<by_project>().equal_range(op.project);
    auto itr = index.first;

    while(itr != index.second)
    {
       if(op.phase_number.valid() && itr->phase_number != *op.phase_number) {
           ++itr;
           continue;
       }

       const das33_pledge_holder_object& pho = *itr;

       // calc amount of token and asset that will be exchanged
       share_type base = std::round(static_cast<double>(pho.base_expected.amount.value) * op.base_to_pledger.value / BONUS_PRECISION / 100);
                  base = (base < pho.base_remaining.amount) ? base : pho.base_remaining.amount;
       share_type bonus = std::round(static_cast<double>(pho.bonus_expected.amount.value) * op.bonus_to_pledger.value / BONUS_PRECISION / 100);
                  bonus = (bonus < pho.bonus_remaining.amount) ? bonus : pho.bonus_remaining.amount;
       share_type pledge = std::round(static_cast<double>(pho.pledged.amount.value) * op.to_escrow.value / BONUS_PRECISION / 100);
                  pledge = (pledge < pho.pledge_remaining.amount) ? pledge : pho.pledge_remaining.amount;

       // make virtual op for history traking
       das33_pledge_result_operation pledge_result;
          pledge_result.funders_account = pho.account_id;
          pledge_result.account_to_fund = _pro_owner;
          pledge_result.completed = true;
          pledge_result.pledged = pledge;
          pledge_result.received = base + bonus;
          pledge_result.project_id = op.project;
          pledge_result.timestamp = d.head_block_time();
       d.push_applied_operation(pledge_result);

       d.adjust_balance(_pro_owner, asset{pledge, pho.pledged.asset_id}, 0 /*reserved_delta*/);

       // issue balance object if it does not exists
       if(!d.check_if_balance_object_exists(pho.account_id,pho.base_expected.asset_id))
       {
          d.create<account_balance_object>([&pho](account_balance_object& abo){
             abo.owner = pho.account_id;
             abo.asset_type = pho.base_expected.asset_id;
             abo.balance = 0;
             abo.reserved = 0;
          });
       }

       // issue token asset
       auto& balance1_obj = d.get_balance_object(pho.account_id, pho.base_expected.asset_id);
       d.issue_asset(balance1_obj, base + bonus, 0);

       // update pledge holder object
       d.modify(pho, [&](das33_pledge_holder_object& p){
          p.pledge_remaining.amount -= pledge;
          p.base_remaining.amount -= base;
          p.bonus_remaining.amount -= bonus;
       });

       // if everything is distributed remove object
       if(pho.pledge_remaining.amount + pho.base_remaining.amount + pho.bonus_remaining.amount <= 0)
       {
          pledges_to_remove.push_back(pho.id);
       }
       itr++;
    }

    auto& index1 = d.get_index_type<das33_pledge_holder_index>().indices().get<by_id>();
    for(object_id_type id : pledges_to_remove)
    {
       auto itr = index1.find(id);
       if(itr != index1.end())
          d.remove(*itr);
    }

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_project_reject_evaluator::do_evaluate(const das33_project_reject_operation& op)
  { try {

     auto& d = db();

     auto& pro_index = d.get_index_type<das33_project_index>().indices().get<by_id>();
     auto pro_itr = pro_index.find(op.project);
     FC_ASSERT(pro_itr != pro_index.end(), "Missing project object with this project_id!");

     auto& index = d.get_index_type<das33_pledge_holder_index>().indices().get<by_project>();
     auto itr = index.lower_bound(op.project);
     auto end = index.upper_bound(op.project);
     while(itr != end)
     {
        const das33_pledge_holder_object& pho = *itr;
        FC_ASSERT(pho.base_expected.amount == pho.base_remaining.amount
           && pho.bonus_expected.amount == pho.bonus_remaining.amount
           && pho.pledged.amount == pho.pledge_remaining.amount,
           "Project already accepted, can't be rejected!");
        itr++;
     }

     _pro_owner = pro_itr->owner;

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_project_reject_evaluator::do_apply(const das33_project_reject_operation& op)
  { try {

     auto& d = db();

     while(true)
     {
        auto& index = d.get_index_type<das33_pledge_holder_index>().indices().get<by_project>();
        auto itr = index.lower_bound(op.project);
        auto end = index.upper_bound(op.project);
        if(itr == end)
           break;

        const das33_pledge_holder_object& pho = *itr;

        das33_pledge_result_operation pledge_result;
           pledge_result.funders_account = pho.account_id;
           pledge_result.account_to_fund = _pro_owner;
           pledge_result.completed = false;
           pledge_result.pledged = pho.pledged;
           pledge_result.received = pho.pledged;
           pledge_result.project_id = op.project;
           pledge_result.timestamp = d.head_block_time();
        d.push_applied_operation(pledge_result);

        auto& balance_obj = d.get_balance_object(pho.account_id, pho.pledged.asset_id);
        d.modify(balance_obj, [&](account_balance_object& balance_obj){
           balance_obj.balance += pho.pledged.amount;
        });

        d.remove(pho);
     }

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_distribute_pledge_evaluator::do_evaluate(const das33_distribute_pledge_operation& op)
  { try {
     auto& d = db();

     // find pledge object
     auto& index = d.get_index_type<das33_pledge_holder_index>().indices().get<by_id>();
     auto itr = index.find(op.pledge);
     FC_ASSERT(itr != index.end(),"Missing pledge object with this pledge_id.");

     auto& pro_index = d.get_index_type<das33_project_index>().indices().get<by_id>();
     auto pro_itr = pro_index.find(itr->project_id);
     account_id_type pro_owner;
     FC_ASSERT(pro_itr != pro_index.end(), "Missing project object with this project_id!");

     _pro_owner = pro_itr->owner;
     _pledge_holder_ptr = &(*itr);

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_distribute_pledge_evaluator::do_apply(const das33_distribute_pledge_operation& op)
  { try {

     auto& d = db();
     const das33_pledge_holder_object& pho = *_pledge_holder_ptr;

     // calc amount of token and asset that will be exchanged
     share_type base = std::round(static_cast<double>(pho.base_expected.amount.value) * op.base_to_pledger.value / BONUS_PRECISION / 100);
                base = (base < pho.base_remaining.amount) ? base : pho.base_remaining.amount;
     share_type bonus = std::round(static_cast<double>(pho.bonus_expected.amount.value) * op.bonus_to_pledger.value / BONUS_PRECISION / 100);
                bonus = (bonus < pho.bonus_remaining.amount) ? bonus : pho.bonus_remaining.amount;
     share_type pledge = std::round(static_cast<double>(pho.pledged.amount.value) * op.to_escrow.value / BONUS_PRECISION / 100);
                pledge = (pledge < pho.pledge_remaining.amount) ? pledge : pho.pledge_remaining.amount;

     // make virtual op for history traking
     das33_pledge_result_operation pledge_result;
        pledge_result.funders_account = pho.account_id;
        pledge_result.account_to_fund = _pro_owner;
        pledge_result.completed = true;
        pledge_result.pledged = pledge;
        pledge_result.received = base + bonus;
        pledge_result.project_id = pho.project_id;
        pledge_result.timestamp = d.head_block_time();
     d.push_applied_operation(pledge_result);

     d.adjust_balance(_pro_owner, asset{pledge, pho.pledged.asset_id}, 0 /*reserved_delta*/);

     // issue balance object if it does not exists
     if(!d.check_if_balance_object_exists(pho.account_id,pho.base_expected.asset_id))
     {
        d.create<account_balance_object>([&pho](account_balance_object& abo){
           abo.owner = pho.account_id;
           abo.asset_type = pho.base_expected.asset_id;
           abo.balance = 0;
           abo.reserved = 0;
        });
     }

     // issue token asset
     auto& balance1_obj = d.get_balance_object(pho.account_id, pho.base_expected.asset_id);
     d.issue_asset(balance1_obj, base + bonus, 0);

     // update pledge holder object
     d.modify(pho, [&](das33_pledge_holder_object& p){
        p.pledge_remaining.amount -= pledge;
        p.base_remaining.amount -= base;
        p.bonus_remaining.amount -= bonus;
     });

     // if everything is distributed remove object
     if(pho.pledge_remaining.amount + pho.base_remaining.amount + pho.bonus_remaining.amount <= 0)
     {
        d.remove(pho);
     }

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_pledge_reject_evaluator::do_evaluate(const das33_pledge_reject_operation& op)
  { try {

     auto& d = db();

     // find pledge object
     auto& index = d.get_index_type<das33_pledge_holder_index>().indices().get<by_id>();
     auto itr = index.find(op.pledge);
     FC_ASSERT(itr != index.end(),"Missing pledge object with this pledge_id.");

     auto& pro_index = d.get_index_type<das33_project_index>().indices().get<by_id>();
     auto pro_itr = pro_index.find(itr->project_id);
     account_id_type pro_owner;
     FC_ASSERT(pro_itr != pro_index.end(), "Missing project object with this project_id!");

     _pro_owner = pro_itr->owner;
     _pledge_holder_ptr = &(*itr);
     const das33_pledge_holder_object& pho = *_pledge_holder_ptr;
     FC_ASSERT(pho.base_expected.amount == pho.base_remaining.amount
           && pho.bonus_expected.amount == pho.bonus_remaining.amount
           && pho.pledged.amount == pho.pledge_remaining.amount,
           "Project already accepted, can't be rejected!");

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_pledge_reject_evaluator::do_apply(const das33_pledge_reject_operation& op)
  { try {

     auto& d = db();

     // make virtual op for history traking
     const das33_pledge_holder_object& pho = *_pledge_holder_ptr;

     // make virtual op for history traking
     das33_pledge_result_operation pledge_result;
        pledge_result.funders_account = pho.account_id;
        pledge_result.account_to_fund = _pro_owner;
        pledge_result.completed = false;
        pledge_result.pledged = pho.pledged;
        pledge_result.received = pho.pledged;
        pledge_result.project_id = pho.project_id;
        pledge_result.timestamp = d.head_block_time();
     d.push_applied_operation(pledge_result);

     // give to project owner pledged amount
     auto& balance_obj = d.get_balance_object(pho.account_id, pho.pledged.asset_id);
     d.modify(balance_obj, [&](account_balance_object& balance_obj){
        balance_obj.balance += pho.pledged.amount;
     });

     d.remove(pho);

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_set_use_external_btc_price_evaluator::do_evaluate(const operation_type& op)
  { try {

      return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_set_use_external_btc_price_evaluator::do_apply(const operation_type& op)
  { try {

      return {};
  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_set_use_market_price_for_token_evaluator::do_evaluate(const operation_type& op)
  { try {
    const auto& d = db();
    const auto& gpo = d.get_global_properties();
    const auto& authority_obj = op.authority(d);

    d.perform_chain_authority_check("das33 authority", gpo.authorities.das33_administrator, authority_obj);

    return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

  void_result das33_set_use_market_price_for_token_evaluator::do_apply(const operation_type& op)
  { try {
    auto& d = db();

    d.modify(d.get_global_properties(), [&](global_property_object& gpo){
      gpo.use_market_price_for_token = op.use_market_price_for_token;
  });

      return {};

  } FC_CAPTURE_AND_RETHROW((op)) }

} }  // namespace graphene::chain


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.

#include <iostream>
#include <random>

#include <boost/optional.hpp>
#include <boost/random.hpp>

#include <poplar/Graph.hpp>

#include <poplibs_support/Algorithm.hpp>
#include <poplibs_support/VectorUtils.hpp>

#include "SparseDensePartitionElementWise.hpp"
#include <popsolver/Model.hpp>
#include <poputil/Util.hpp>

// Test functions to generate sparse tensor data and metadata for codelet
// testing

using namespace poplar;
using namespace poputil;
using namespace poplibs_support;

template <typename RandomEngine>
std::vector<std::array<unsigned, 2>>
generateSparseIndices(RandomEngine &randomEngine,
                      const std::vector<std::size_t> &shape, std::size_t n) {
  // Generate n random indices that are within the flattened given shape.
  std::vector<unsigned> randomIndices(product(shape));
  std::iota(randomIndices.begin(), randomIndices.end(), 0);
  auto randomGen = [&](unsigned max) {
    boost::random::uniform_int_distribution<unsigned> dist(0, max - 1);
    return dist(randomEngine);
  };
  boost::range::random_shuffle(randomIndices, randomGen);
  randomIndices.resize(n);

  std::vector<std::array<unsigned, 2>> rowColumnIndices(n);
  for (std::size_t i = 0; i < n; ++i) {
    const auto unflattenedIndex =
        vectorConvert<unsigned>(unflattenIndex(shape, randomIndices[i]));
    rowColumnIndices[i] = {unflattenedIndex[0], unflattenedIndex[1]};
  }
  return rowColumnIndices;
}

std::tuple<unsigned, unsigned, unsigned>
getForwardWorkerPartition(const Target &target, const Type &inputType,
                          const unsigned bColumns, const unsigned aRows,
                          const std::vector<unsigned> &aRowColumnCounts) {
  // Split rows of a and columns of b between workers.
  //
  // For this functional test we'll just first split columns of b, then
  // rows of a to try and utilise all workers.
  //
  // NOTE: A problem with this is that by needing a contiguous range of
  // columns to process, in the given order, we are restricting how work
  // can be split. We cannot change the order of columns because that
  // would make things difficult for other passes. We could do some more
  // heavy encoding to allow interleaved columns to be selected for
  // workers but it's more memory to encode.
  const auto bColumnGrainSize = target.getVectorWidth(inputType);
  const auto bColumnGrains = ceildiv(bColumns, bColumnGrainSize);

  popsolver::Model m;
  const auto mWorkerARowsPartition = m.addVariable(1, aRows);
  const auto mWorkerBColumnGrainsPartition = m.addVariable(1, bColumnGrains);

  const auto mNumARows = m.addConstant(aRows);
  const auto mNumBColumnGrains = m.addConstant(bColumnGrains);

  const auto mWorkerARows = m.ceildiv(mNumARows, mWorkerARowsPartition);
  const auto mWorkerBColumnGrains =
      m.ceildiv(mNumBColumnGrains, mWorkerBColumnGrainsPartition);

  const auto numWorkers = target.getNumWorkerContexts();
  const auto mMaxWorkerAElems = m.call<unsigned>(
      {mWorkerARows},
      [&](const std::vector<unsigned> &values) -> popsolver::DataType {
        const auto workerARows = values[0];
        unsigned maxWorkerAElems = 0;
        for (unsigned worker = 0; worker < numWorkers; ++worker) {
          const auto elems = std::accumulate(
              aRowColumnCounts.begin() +
                  std::min<unsigned>(worker * workerARows, aRows),
              aRowColumnCounts.begin() +
                  std::min<unsigned>((worker + 1) * workerARows, aRows),
              0u);
          maxWorkerAElems = std::max<unsigned>(maxWorkerAElems, elems);
        }
        return popsolver::DataType{maxWorkerAElems};
      });

  m.lessOrEqual(
      m.product({mWorkerARowsPartition, mWorkerBColumnGrainsPartition}),
      popsolver::DataType{numWorkers});

  const auto mMaxWorkerGrains =
      m.product({mMaxWorkerAElems, mWorkerBColumnGrains});

  const auto s = m.minimize(mMaxWorkerGrains);
  if (!s.validSolution()) {
    throw poplibs_error("Failed to find a plan to split work between workers!");
  }

  const auto numAPartitions = s[mWorkerARowsPartition].getAs<unsigned>();
  const auto numBPartitions =
      s[mWorkerBColumnGrainsPartition].getAs<unsigned>();
  return std::make_tuple(bColumnGrainSize, numAPartitions, numBPartitions);
}

std::tuple<unsigned, unsigned>
getGradWWorkerPartition(const Target &target, const Type &inputType,
                        const unsigned bColumns, const unsigned aRows,
                        const std::vector<unsigned> &aRowColumnCounts) {
  // Much easier than forward partitions. Just partition the total number
  // of columns of A between workers.
  const auto totalAElems = std::accumulate(
      aRowColumnCounts.begin(), aRowColumnCounts.end(), std::size_t(0));

  // Grain size of columns of b is the same as for forward for GradW codelet.
  const auto bColumnGrainSize = target.getVectorWidth(inputType);
  const auto numWorkers = target.getNumWorkerContexts();
  const auto aElemsPerWorker = ceildiv(totalAElems, numWorkers);
  const auto numAPartitions = ceildiv(totalAElems, aElemsPerWorker);

  return std::make_tuple(bColumnGrainSize, numAPartitions);
}

template <typename RandomEngine>
std::vector<std::vector<unsigned>> generateMetaInfoAndPartition(
    RandomEngine &randomEngine, std::vector<std::array<unsigned, 2>> &indices,
    const std::vector<std::size_t> &aShape,
    const std::vector<std::size_t> &bShape, unsigned numBuckets,
    unsigned processedSubGroupId, const std::vector<unsigned> &otherSubGroupIds,
    const std::vector<std::vector<unsigned>> processedSubGroupIndices,
    const std::vector<std::vector<unsigned>> &subGroupNumElems,
    const Target &target, const Type &inputType, const Type &partialType,
    VertexType vertexType, unsigned xPartition, unsigned yPartition) {
  const auto generateGradWMetaInfo = vertexType == VertexType::GradW;
  const auto generateGradAMetaInfo = vertexType == VertexType::GradA;

  // Factor by which row and column offsets are scaled
  const auto yOffsetFactor = inputType == FLOAT ? 4 : 2;

  // Order indices of a by column then row
  std::sort(indices.begin(), indices.end());

  std::vector<std::vector<unsigned>> metaInfo(numBuckets);
  auto garbageDist =
      boost::random::uniform_int_distribution<unsigned>(0, 0xffff);
  std::size_t nzOffset = 0;
  for (unsigned bucket = 0; bucket < numBuckets; ++bucket) {
    std::size_t splitIdx = 0;
    std::size_t numSplits = processedSubGroupIndices.at(bucket).size();
    for (std::size_t i = 0; i < otherSubGroupIds.size() + numSplits; ++i) {
      if (splitIdx != numSplits &&
          i == processedSubGroupIndices[bucket][splitIdx]) {

        std::vector<unsigned> rows;
        std::vector<unsigned> rowColumnCounts;
        boost::optional<unsigned> lastRowIndex;
        for (std::size_t nzIdx = nzOffset;
             nzIdx < nzOffset + subGroupNumElems[bucket].at(i); ++nzIdx) {
          if (!lastRowIndex || *lastRowIndex != indices.at(nzIdx)[0]) {
            rows.emplace_back();
            rowColumnCounts.emplace_back();
          }
          rows.back() = indices.at(nzIdx)[0];
          ++rowColumnCounts.back();
          lastRowIndex = rows.back();
        }

        const auto bColumns = bShape[1];

        unsigned fwdBColumnGrainSize, fwdNumAPartitions, fwdNumBPartitions;
        std::tie(fwdBColumnGrainSize, fwdNumAPartitions, fwdNumBPartitions) =
            getForwardWorkerPartition(target, inputType, bColumns, rows.size(),
                                      rowColumnCounts);

        const auto fwdBColumnGrains = ceildiv(bColumns, fwdBColumnGrainSize);
        const auto fwdNumUsedWorkers = fwdNumAPartitions * fwdNumBPartitions;
        const auto fwdMaxPartitionARows =
            ceildiv(rows.size(), fwdNumAPartitions);
        const auto fwdMaxPartitionBColumnGrains =
            ceildiv(fwdBColumnGrains, fwdNumBPartitions);

        std::vector<unsigned> fwdPartitionAElemOffsets(fwdNumAPartitions, 0);
        for (unsigned partition = 1; partition < fwdNumAPartitions;
             ++partition) {
          const auto prevPartitionARowStart =
              (partition - 1) * fwdMaxPartitionARows;
          const auto prevPartitionARowEnd = partition * fwdMaxPartitionARows;
          fwdPartitionAElemOffsets[partition] =
              fwdPartitionAElemOffsets[partition - 1] +
              std::accumulate(rowColumnCounts.begin() + prevPartitionARowStart,
                              rowColumnCounts.begin() + prevPartitionARowEnd,
                              unsigned(0));
        }

        unsigned gradWNumUsedWorkers, gradWBColumnGrainSize,
            gradWNumAPartitions = 0;
        if (generateGradWMetaInfo) {
          std::tie(gradWBColumnGrainSize, gradWNumAPartitions) =
              getGradWWorkerPartition(target, inputType, bColumns, rows.size(),
                                      rowColumnCounts);
          gradWNumUsedWorkers = gradWNumAPartitions;
        }

        metaInfo[bucket].emplace_back(processedSubGroupId);
        const auto processedSubGroupNumElems = subGroupNumElems[bucket].at(i);
        const auto elemsPerNzElem = generateGradAMetaInfo ? 2 : 1;

        metaInfo[bucket].emplace_back(xPartition);
        metaInfo[bucket].emplace_back(yPartition);
        metaInfo[bucket].emplace_back(processedSubGroupNumElems);
        const auto totalMetaInfoElems =
            9 + fwdNumUsedWorkers * 5 + rows.size() * 2 +
            processedSubGroupNumElems * elemsPerNzElem +
            (generateGradWMetaInfo ? 1 + 4 * gradWNumUsedWorkers : 0);
        const auto offsetToFirstOutputEntry =
            totalMetaInfoElems -
            (rows.size() * 2 + processedSubGroupNumElems * elemsPerNzElem);

        metaInfo[bucket].emplace_back(totalMetaInfoElems);
        metaInfo[bucket].emplace_back(bColumns);
        metaInfo[bucket].emplace_back(rows.size() - 1);
        metaInfo[bucket].emplace_back(offsetToFirstOutputEntry);
        metaInfo[bucket].emplace_back(fwdNumUsedWorkers);

        // Reserve space for worker entries
        std::vector<std::size_t> metaInfoFwdWorkerEntryIndices(
            fwdNumUsedWorkers);
        for (unsigned worker = 0; worker < fwdNumUsedWorkers; ++worker) {
          metaInfoFwdWorkerEntryIndices[worker] = metaInfo[bucket].size();
          for (std::size_t i = 0; i < 5; ++i) {
            metaInfo[bucket].emplace_back(~0u);
          }
        }

        // If needed reserve space for GradW worker entries
        std::vector<unsigned> metaInfoGradWWorkerEntryIndices;
        if (generateGradWMetaInfo) {
          metaInfo[bucket].emplace_back(gradWNumUsedWorkers);

          metaInfoGradWWorkerEntryIndices.resize(gradWNumUsedWorkers);
          for (unsigned worker = 0; worker < gradWNumUsedWorkers; ++worker) {
            metaInfoGradWWorkerEntryIndices[worker] = metaInfo[bucket].size();
            for (std::size_t i = 0; i < 4; ++i) {
              metaInfo[bucket].emplace_back(~0u);
            }
          }
        }

        // Output row -> column list meta-info
        std::vector<unsigned> outputEntryMetaInfoIndices(rows.size());
        std::size_t offsetGradA = 0;
        for (std::size_t r = 0; r < rows.size(); ++r) {
          const auto aRow = indices.at(nzOffset)[0];
          // First entry is offset into output memory to process.
          // bColumns are inner-most dimension.
          const auto aRowOffsetInC = aRow;
          outputEntryMetaInfoIndices[r] = metaInfo[bucket].size();
          metaInfo[bucket].push_back(aRowOffsetInC);
          // Use 1 less for float input type
          metaInfo[bucket].push_back(rowColumnCounts[r]);
          for (unsigned c = 0; c < rowColumnCounts[r]; ++c) {
            if (generateGradAMetaInfo) {
              metaInfo[bucket].push_back(yOffsetFactor * offsetGradA++);
            }
            metaInfo[bucket].push_back(indices.at(nzOffset)[1] * bColumns *
                                       yOffsetFactor);
            ++nzOffset;
          }
        }

        // Fill out worklist info for each worker
        for (unsigned worker = 0; worker < fwdNumUsedWorkers; ++worker) {
          const auto aPartitionIdx = worker % fwdNumAPartitions;
          const auto bPartitionIdx = worker / fwdNumAPartitions;
          const auto aRowIndex = aPartitionIdx * fwdMaxPartitionARows;
          const auto aRowEndIndex =
              std::min((aPartitionIdx + 1) * fwdMaxPartitionARows, rows.size());
          const auto bColumnIndex = bPartitionIdx *
                                    fwdMaxPartitionBColumnGrains *
                                    fwdBColumnGrainSize;
          const auto bColumnEndIndex =
              std::min((bPartitionIdx + 1) * fwdMaxPartitionBColumnGrains *
                           fwdBColumnGrainSize,
                       bColumns);
          const auto workerEntryIndex = metaInfoFwdWorkerEntryIndices[worker];
          metaInfo[bucket][workerEntryIndex + 0] =
              generateGradAMetaInfo ? 0
                                    : fwdPartitionAElemOffsets[aPartitionIdx];
          metaInfo[bucket][workerEntryIndex + 1] =
              bColumnEndIndex - bColumnIndex;
          metaInfo[bucket][workerEntryIndex + 2] = bColumnIndex;
          // Number is 1 less
          metaInfo[bucket][workerEntryIndex + 3] = aRowEndIndex - aRowIndex - 1;
          metaInfo[bucket][workerEntryIndex + 4] =
              outputEntryMetaInfoIndices[aRowIndex] - workerEntryIndex;
        }

        if (generateGradWMetaInfo) {
          const auto totalAElems = std::accumulate(
              rowColumnCounts.begin(), rowColumnCounts.end(), std::size_t(0));
          const auto numAElemsPerPartition =
              ceildiv(totalAElems, gradWNumAPartitions);
          unsigned currRowIndex = 0;
          unsigned currRowColumnIndex = 0;
          for (unsigned worker = 0; worker < gradWNumUsedWorkers; ++worker) {
            const auto sparseStartIndex = worker * numAElemsPerPartition;
            const auto sparseEndIndex =
                std::min((worker + 1) * numAElemsPerPartition, totalAElems);
            const auto numSparseElems = sparseEndIndex - sparseStartIndex;

            unsigned startRowIndex = currRowIndex;
            unsigned startRowStartColumnIndex = currRowColumnIndex;
            const auto workerEntryIndex =
                metaInfoGradWWorkerEntryIndices[worker];
            metaInfo[bucket][workerEntryIndex + 0] = sparseStartIndex;
            metaInfo[bucket][workerEntryIndex + 1] =
                outputEntryMetaInfoIndices[startRowIndex] - workerEntryIndex;
            metaInfo[bucket][workerEntryIndex + 2] = startRowStartColumnIndex;
            metaInfo[bucket][workerEntryIndex + 3] = numSparseElems;

            // Advance to next worker's work
            unsigned numRemainingElems = numSparseElems;
            while (numRemainingElems > 0) {
              const auto elemsThisRow =
                  std::min(numRemainingElems,
                           rowColumnCounts[currRowIndex] - currRowColumnIndex);
              numRemainingElems -= elemsThisRow;
              currRowColumnIndex += elemsThisRow;
              if (currRowColumnIndex >= rowColumnCounts[currRowIndex]) {
                currRowColumnIndex = 0;
                currRowIndex++;
              }
            }
          }
        }
        ++splitIdx;
      } else {
        const auto otherSubGroupIdx = i - splitIdx;
        const auto subGroupId = otherSubGroupIds[otherSubGroupIdx];
        const auto numElems = subGroupNumElems[bucket][i];
        metaInfo[bucket].emplace_back(subGroupId);
        // Enforce a large numbered partition to go with sub group IDs that
        // aren't to be used.
        metaInfo[bucket].emplace_back(0xffff);
        metaInfo[bucket].emplace_back(0xffff);
        metaInfo[bucket].emplace_back(numElems);
        // We also just use this no. of sub-elements as garbage in the meta-info
        // for the other (unprocessed) sub-groups.
        metaInfo[bucket].emplace_back(numElems + 5);
        for (std::size_t i = 0; i < numElems; ++i) {
          metaInfo[bucket].emplace_back(garbageDist(randomEngine));
        }
      }
    }
    constexpr unsigned endSubGroupId = 0;
    metaInfo[bucket].push_back(endSubGroupId);
  }

  return metaInfo;
}

template std::vector<std::array<unsigned, 2>>
generateSparseIndices<std::mt19937>(std::mt19937 &randomEngine,
                                    const std::vector<std::size_t> &shape,
                                    std::size_t n);

template std::vector<std::vector<unsigned>>
generateMetaInfoAndPartition<std::mt19937>(
    std::mt19937 &randomEngine, std::vector<std::array<unsigned, 2>> &indices,
    const std::vector<std::size_t> &aShape,
    const std::vector<std::size_t> &bShape, unsigned numBuckets,
    unsigned processedSubGroupId, const std::vector<unsigned> &otherSubGroupIds,
    const std::vector<std::vector<unsigned>> processedSubGroupIndices,
    const std::vector<std::vector<unsigned>> &subGroupNumElems,
    const Target &target, const Type &inputType, const Type &partialType,
    VertexType vertexType, unsigned xPartition, unsigned yPartition);


// SPDX-License-Identifier: MIT
// The MIT License (MIT)
//
// Copyright (c) 2014-2018, Institute for Software & Systems Engineering
// Copyright (c) 2018-2019, Johannes Leupolz
//
// 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 "pemc/lmc/lmc_model_checker.h"

#include <boost/timer/timer.hpp>
#include <utility>
#include <vector>

#include "pemc/basic/exceptions.h"
#include "pemc/formula/formula_utils.h"

namespace {
using namespace pemc;
using boost::timer::cpu_timer;

enum PrecalculatedTransition : uint8_t {
  Nothing = 0,
  SatisfiedDirect = 1,
  ExcludedDirect = 2,
  Satisfied = 4,  // Satisfied for the current run
  Excluded = 8,   // Excluded for the current run
  Mark = 16,
};

void calculateIteration(Lmc& lmc,
                        gsl::span<PrecalculatedTransition> precalculations,
                        gsl::span<Probability> xold,
                        gsl::span<Probability> xnew) {
  auto stateCount = lmc.getStates().size();
  for (StateIndex s = 0; s < stateCount; ++s) {
    auto transitions = lmc.getTransitions();
    auto sum = Probability::Zero();
    TransitionIndex begin, end = 0;
    std::tie(begin, end) = lmc.getTransitionIndexesOfState(s);

    for (TransitionIndex t = begin; t < end; t++) {
      auto& transition = transitions[t];
      auto& precalulated = precalculations[t];
      if (precalulated & PrecalculatedTransition::Satisfied) {
        sum += transition.probability;

      } else if (precalulated & PrecalculatedTransition::Excluded) {
      } else {
        sum += transition.probability * xold[transition.state];
      }
    }
    xnew[s] = sum;
  }
}

Probability calculateInitialProbability(
    Lmc& lmc,
    gsl::span<PrecalculatedTransition> precalculations,
    gsl::span<Probability> x) {
  auto transitions = lmc.getTransitions();
  auto sum = Probability::Zero();
  TransitionIndex begin, end = 0;
  std::tie(begin, end) = lmc.getInitialTransitionIndexes();

  for (TransitionIndex t = begin; t < end; t++) {
    auto& transition = transitions[t];
    auto& precalulated = precalculations[t];
    if (precalulated & PrecalculatedTransition::Satisfied) {
      sum += transition.probability;
    } else if (precalulated & PrecalculatedTransition::Excluded) {
    } else {
      sum += transition.probability * x[transition.state];
    }
  }
  return sum;
}

void precalculateDirectSatisfactionAndExclusion(
    Lmc& lmc,
    gsl::span<PrecalculatedTransition> precalculatedTransitions,
    Formula* phi,
    Formula* psi,
    std::ostream& cout) {
  cout << "Precalculate transitions that are directly satisfied or excluded. "
       << std::endl;
  cpu_timer timer;

  auto psiEvaluator = lmc.createLabelBasedFormulaEvaluator(psi);
  std::function<bool(TransitionIndex)> returnFalse = [](TransitionIndex t) {
    return false;
  };
  auto phiEvaluator =
      phi != nullptr ? lmc.createLabelBasedFormulaEvaluator(psi) : returnFalse;

  // bitwise or casts uint8_t implicitly to int
  auto satisfied =
      (PrecalculatedTransition)(PrecalculatedTransition::SatisfiedDirect |
                                PrecalculatedTransition::Satisfied);
  auto excluded =
      (PrecalculatedTransition)(PrecalculatedTransition::ExcludedDirect |
                                PrecalculatedTransition::Excluded);

  for (TransitionIndex t = 0; t < precalculatedTransitions.size(); t++) {
    if (psiEvaluator(t)) {
      precalculatedTransitions[t] = satisfied;
    } else if (phiEvaluator(t)) {
      precalculatedTransitions[t] = excluded;
    } else {
      precalculatedTransitions[t] = PrecalculatedTransition::Nothing;
    }
  }

  timer.stop();
  auto elapsedTime = timer.elapsed();
  auto elapsedTimeStr = format(elapsedTime);
  cout << "\t\tFinished in " << elapsedTimeStr << "." << std::endl;
}

Probability calculateBoundedUntil(Lmc& lmc,
                                  Formula* phi,
                                  Formula* psi,
                                  int bound,
                                  std::ostream& cout) {
  cpu_timer timer;

  auto transitions = lmc.getTransitions();
  std::vector<PrecalculatedTransition> precalculations(transitions.size());
  precalculateDirectSatisfactionAndExclusion(lmc, precalculations, phi, psi,
                                             cout);

  auto stateCount = lmc.getStates().size();
  auto probablityVector1 = std::vector<Probability>(stateCount);
  auto probablityVector2 = std::vector<Probability>(stateCount);
  auto xold = gsl::span<Probability>(probablityVector1);
  auto xnew = gsl::span<Probability>(probablityVector2);

  for (auto i = 0; i < bound; i++) {
    calculateIteration(lmc, precalculations, xold, xnew);
    std::swap(xold, xnew);

    if (i % 10 == 0) {
      cout << "Calculated " << i << " iterations" << std::endl;
    }
  }

  auto result = calculateInitialProbability(lmc, precalculations, xold);

  timer.stop();
  auto elapsedTime = timer.elapsed();
  auto elapsedTimeStr = format(elapsedTime);

  return result;
}
}  // namespace

namespace pemc {

using boost::timer::cpu_timer;

LmcModelChecker::LmcModelChecker(Lmc& _lmc, const Configuration& _conf)
    : lmc(_lmc), conf(_conf){};

Probability LmcModelChecker::calculateProbability(Formula& formulaToCheck) {
  auto matchFormula = tryExtractPhiUntilPsiWithBound(formulaToCheck);
  if (matchFormula == std::nullopt)
    return Probability::Error();
  Formula* phi;
  Formula* psi;
  std::optional<int> bound;
  std::tie(phi, psi, bound) = *matchFormula;

  *conf.cout << "Checking formula: " << formulaToString(formulaToCheck)
             << std::endl;

  if (bound != std::nullopt) {
    return calculateBoundedUntil(lmc, phi, psi, *bound, *conf.cout);
  } else {
    // CalculateUnboundUntil
    throw NotImplementedYetException();
  }

  return Probability::Error();
}

}  // namespace pemc