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

#include <boost/simd/bitwise/functions/rshl.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/mpl/equal_to.hpp>

#ifndef NDEBUG
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/constants/zero.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF          ( rshl_, tag::cpu_, (A0)(A1)(X)
                                       , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                               , boost::simd::meta::cardinal_of<A1>
                                                               >
                                         )
                                       , ((simd_<arithmetic_<A0>,X>))
                                         ((generic_<integer_<A1> >))
                                       )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      #ifndef NDEBUG
      return if_else(is_gtz(a1), shl(a0, max(Zero<A1>(), a1)), shr(a0, max(Zero<A1>(), -a1)));
      #else
      return if_else(is_gtz(a1), shl(a0, a1), shr(a0, -a1));
      #endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF          ( rshl_, tag::cpu_, (A0)(A1)(X)
                                       , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                               , boost::simd::meta::cardinal_of<A1>
                                                               >
                                       )
                                       , ((simd_<arithmetic_<A0>,X>))
                                         ((generic_<unsigned_<A1> >))
                                       )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return shl(a0, a1);
    }
  };

} } }

#endif


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

upo_RRT_ros::ValidityChecker2::ValidityChecker2(nav_msgs::OccupancyGrid* costmap, tf::TransformListener* tf, unsigned int dimensions, int distType) : StateChecker()
{
    costmap_mutex_.lock();
	costmap_.clear();
	for(unsigned int i=0; i<costmap->data.size(); i++)
		costmap_.push_back((int)costmap->data[i]);

	resolution_ = (float)costmap->info.resolution; 	//m/cell
	width_ = costmap->info.width;					//cells
 	height_ = costmap->info.height;					//cells
	origin_.clear();
	origin_.push_back(costmap->info.origin.position.x); //m
	origin_.push_back(costmap->info.origin.position.y); //m
	origin_.push_back(tf::getYaw(costmap->info.origin.orientation)); //rad
	costmap_mutex_.unlock();

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

upo_RRT_ros::ValidityChecker2::ValidityChecker2(tf::TransformListener* tf, unsigned int dimensions, int distType) : StateChecker()
{
	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();
}

upo_RRT_ros::ValidityChecker2::~ValidityChecker2() {

	//delete navfeatures_;
}

void upo_RRT_ros::ValidityChecker2::updateCostmap(nav_msgs::OccupancyGrid* costmap)
{
	costmap_mutex_.lock();
	costmap_.clear();
	for(unsigned int i=0; i<costmap->data.size(); i++)
		costmap_.push_back((int)costmap->data[i]);

	resolution_ = (float)costmap->info.resolution;
	width_ = costmap->info.width;
 	height_ = costmap->info.height;
	origin_.clear();
	origin_.push_back(costmap->info.origin.position.x);
	origin_.push_back(costmap->info.origin.position.y);
	origin_.push_back(tf::getYaw(costmap->info.origin.orientation));
	costmap_mutex_.unlock();
}

std::vector<int> upo_RRT_ros::ValidityChecker2::poseToCell(std::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 =  fabs(origin_[0])/resolution_ + pose[0]/resolution_;
	int y =  fabs(origin_[1])/resolution_ + pose[1]/resolution_;
 	std::vector<int> cell;
	cell.push_back(x);
	cell.push_back(y);
	return cell;
}

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

bool upo_RRT_ros::ValidityChecker2::isValid(upo_RRT::State* s) const
{
	std::vector<float> pose;
	pose.push_back((float)s->getX());
	pose.push_back((float)s->getY());
	//costmap_mutex_.lock();
	std::vector<int> cell = poseToCell(pose);
 	//printf("IsValid cell[%i][%i]\n", cell[0],cell[1]);
	int ind = cell[0]*200 + cell[1];
    //printf("Indice: %i\n", ind);
	float cost = costmap_[ind];
	//costmap_mutex_.unlock();

	if(cost == -1 || cost > 100) //cost == 100
		return false;
	else
		return true;

}

void upo_RRT_ros::ValidityChecker2::preplanning_computations()
{
}

float upo_RRT_ros::ValidityChecker2::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));
				//printf("eu_dist: %.2f, angle_dist: %.3f, dist: %.3f\n", euc_dist, angle_dist, 0.8*euc_dist + 0.2*angle_dist);
				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::ValidityChecker2::getCost(upo_RRT::State* s)
{
	//cell.first = x = column, cell.second = y = row
	std::vector<float> pose;
	pose.push_back((float)s->getX());
	pose.push_back((float)s->getY());
	costmap_mutex_.lock();
	std::vector<int> cell = poseToCell(pose);
	int ind = (cell[1]+1) * cell[0]; //row*column
	float cost = costmap_[ind];
	costmap_mutex_.unlock();
	return cost;

}

/*void upo_RRT_ros::ValidityChecker::setPeople(upo_msgs::PersonPoseArrayUPO p)
{
	navfeatures_->setPeople(p);
}

void upo_RRT_ros::ValidityChecker::setWeights(std::vector<float> we) {
	navfeatures_->setWeights(we);
}*/

geometry_msgs::PoseStamped upo_RRT_ros::ValidityChecker2::transformPoseTo(geometry_msgs::PoseStamped pose_in, std::string frame_out, bool usetime) {

	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("ValidityChecker2. TransformException in method transformPoseTo. TargetFrame: %s : %s", frame_out.c_str(), ex.what());
	}
	return pose_out;

}

bool upo_RRT_ros::ValidityChecker2::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;

}


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <cctbx/sgtbx/find_affine.h>

namespace cctbx { namespace sgtbx {

namespace {

  struct find_affine_wrappers
  {
    typedef find_affine w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("find_affine", no_init)
        .def(init<space_group const&, optional<int, bool> >((
          arg("group"),
          arg("range")=2,
          arg("use_p1_algorithm")=false)))
        .def("cb_mx", &w_t::cb_mx)
      ;
    }
  };

} // namespace <anoymous>

namespace boost_python {

  void wrap_find_affine()
  {
    find_affine_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


#pragma once

//#include <bmchain/protocol/authority.hpp>
//#include <bmchain/protocol/bmchain_operations.hpp>
//
//#include <bmchain/chain/bmchain_object_types.hpp>
//#include <bmchain/chain/comment_object.hpp>
//
//#include <boost/multi_index/composite_key.hpp>
//#include <boost/multiprecision/cpp_int.hpp>

#include <bmchain/chain/bmchain_objects.hpp>

namespace bmchain { namespace chain {

   enum class smt_phase : uint8_t
   {
      account_elevated,
      setup_completed,
      contribution_begin_time_completed,
      contribution_end_time_completed,
      launch_time_completed,              /// launch window opened
      launch_failed,                      /// launch window closed with either not enough contributions or some cap not revealed
      launch_success                      /// enough contributions were declared and caps revealed before launch windows closed
   };

   class custom_token_object : public object< custom_token_object_type, custom_token_object > {
   public:
      custom_token_object() = delete;

      template<typename Constructor, typename Allocator>
      custom_token_object(Constructor &&c, allocator<Allocator> a) {
         c(*this);
      }

      id_type id;
      account_name_type control_account;
      asset_symbol_type symbol;
      asset             current_supply;
      asset             reward_fund;
      uint16_t          inflation_rate = 0;
      time_point_sec    generation_time;

      time_point_sec       schedule_time =  BMCHAIN_GENESIS_TIME;;
      bmchain::protocol::custom_token_emissions_unit emissions_unit;
      uint32_t             interval_seconds = 0;
      uint32_t             interval_count = 0;

      /// set_setup_parameters
      bool              allow_voting = false;
      bool              allow_vesting = false;

      smt_phase         phase = smt_phase::account_elevated;

      ///parameters for 'setup_operation'
      int64_t                       max_supply = 0;
      bmchain::protocol::
      custom_token_capped_generation_policy  capped_generation_policy;
      time_point_sec                generation_begin_time;
      time_point_sec                generation_end_time;
      time_point_sec                announced_launch_time;
      time_point_sec                launch_expiration_time;
   };

   class account_balance_object : public object< account_balance_object_type, account_balance_object > {
      account_balance_object() = delete;

   public:
      template<typename Constructor, typename Allocator>
      account_balance_object(Constructor &&c, allocator <Allocator> a) {
         c(*this);
      }

      id_type id;
      /// Name of the account, the balance is held for.
      asset_symbol_type symbol;
      account_name_type owner;
      asset             balance;
   };

   class custom_token_event_object : public object< custom_token_event_object_type, custom_token_event_object >   {

      custom_token_event_object() = delete;

   public:
      template<typename Constructor, typename Allocator>
      custom_token_event_object(Constructor &&c, allocator <Allocator> a) {
         c(*this);
      }

      // id_type is actually oid<smt_event_token_object>
      id_type id;

      custom_token_event_id_type parent;

      smt_phase phase = smt_phase::setup_completed;

      time_point_sec generation_begin_time;
      time_point_sec generation_end_time;
      time_point_sec announced_launch_time;
      time_point_sec launch_expiration_time;
   };

   struct by_symbol;
   struct by_control_account;
   typedef multi_index_container<
              custom_token_object,
              indexed_by<
                      ordered_unique< tag< by_id >, member< custom_token_object, custom_token_id_type, &custom_token_object::id > >,
                      ordered_unique< tag< by_control_account >,
                              composite_key< custom_token_object,
                                      member< custom_token_object, account_name_type,  &custom_token_object::control_account >,
                                      member< custom_token_object, custom_token_id_type,  &custom_token_object::id >
                              >,
                              composite_key_compare< std::less< account_name_type >, std::greater< custom_token_id_type > >
                      >,
                      ordered_unique< tag< by_symbol >,
                              composite_key< custom_token_object,
                                      member< custom_token_object, asset_symbol_type, &custom_token_object::symbol >
                              >
                      >,
                      ordered_unique< tag< by_schedule_time >,
                              composite_key< custom_token_object,
                                      member< custom_token_object, time_point_sec,  &custom_token_object::schedule_time >,
                                      member< custom_token_object, custom_token_id_type,  &custom_token_object::id >
                              >,
                              composite_key_compare< std::greater< time_point_sec >, std::greater< custom_token_id_type > >
                      >
              >,
              allocator< custom_token_object >
   > custom_token_index;

   typedef multi_index_container<
              account_balance_object,
              indexed_by<
                      ordered_unique< tag< by_id >, member< account_balance_object, account_balance_id_type, &account_balance_object::id > >,
                      ordered_unique< tag< by_symbol >,
                              composite_key< account_balance_object,
                                      member< account_balance_object, asset_symbol_type,  &account_balance_object::symbol >,
                                      member< account_balance_object, account_balance_id_type, &account_balance_object::id >
                              >,
                              composite_key_compare< std::less< asset_symbol_type >, std::greater< account_balance_id_type > >
                      >,
                      ordered_unique< tag< by_owner >,
                              composite_key< account_balance_object,
                                      member< account_balance_object, account_name_type,  &account_balance_object::owner >,
                                      member< account_balance_object, asset_symbol_type,  &account_balance_object::symbol >,
                                      member< account_balance_object, account_balance_id_type,  &account_balance_object::id >
                              >,
                              composite_key_compare< std::less< account_name_type >, std::less< asset_symbol_type >, std::greater< account_balance_id_type > >
                      >
              >,
              allocator< account_balance_object >
   > account_balance_index;

struct by_interval_gen_begin;
struct by_interval_gen_end;
struct by_interval_launch;
struct by_interval_launch_exp;
typedef multi_index_container <
   custom_token_event_object,
   indexed_by <
      ordered_unique< tag< by_id >,
         member< custom_token_event_object, custom_token_event_id_type, &custom_token_event_object::id > >,

      ordered_non_unique< tag< by_interval_gen_begin >,
         composite_key< custom_token_event_object,
            member< custom_token_event_object, smt_phase, &custom_token_event_object::phase >,
            member< custom_token_event_object, time_point_sec, &custom_token_event_object::generation_begin_time >
         >
      >,
      ordered_non_unique< tag< by_interval_gen_end >,
         composite_key< custom_token_event_object,
            member< custom_token_event_object, smt_phase, &custom_token_event_object::phase >,
            member< custom_token_event_object, time_point_sec, &custom_token_event_object::generation_end_time >
         >
      >,
      ordered_non_unique< tag< by_interval_launch >,
         composite_key< custom_token_event_object,
            member< custom_token_event_object, smt_phase, &custom_token_event_object::phase >,
            member< custom_token_event_object, time_point_sec, &custom_token_event_object::announced_launch_time >
         >
      >,
      ordered_non_unique< tag< by_interval_launch_exp >,
         composite_key< custom_token_event_object,
            member< custom_token_event_object, smt_phase, &custom_token_event_object::phase >,
            member< custom_token_event_object, time_point_sec, &custom_token_event_object::launch_expiration_time >
         >
      >
   >,
   allocator< custom_token_event_object >
> custom_token_event_index;

}}

FC_REFLECT( bmchain::chain::custom_token_object,
                    (id)(control_account)(symbol)(current_supply)(inflation_rate)(reward_fund)(generation_time)(schedule_time)
                    (emissions_unit)(interval_seconds)(interval_count)(allow_voting)(allow_vesting) )
CHAINBASE_SET_INDEX_TYPE( bmchain::chain::custom_token_object, bmchain::chain::custom_token_index )

FC_REFLECT( bmchain::chain::account_balance_object,(id)(owner)(balance) )
CHAINBASE_SET_INDEX_TYPE( bmchain::chain::account_balance_object, bmchain::chain::account_balance_index )

FC_REFLECT( bmchain::chain::custom_token_event_object, (id)(parent)(phase)(generation_begin_time)(generation_end_time)(announced_launch_time)(launch_expiration_time) )
CHAINBASE_SET_INDEX_TYPE( bmchain::chain::custom_token_event_object, bmchain::chain::custom_token_event_index )


#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "ces.h"
#include "filelib.h"
#include "stringlib.h"
#include "sparse_vector.h"
#include "scorer.h"
#include "viterbi_envelope.h"
#include "inside_outside.h"
#include "error_surface.h"
#include "b64tools.h"
#include "hg_io.h"

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

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("reference,r",po::value<vector<string> >(), "[REQD] Reference translation (tokenized text)")
        ("source,s",po::value<string>(), "Source file (ignored, except for AER)")
        ("loss_function,l",po::value<string>()->default_value("ibm_bleu"), "Loss function being optimized")
        ("input,i",po::value<string>()->default_value("-"), "Input file to map (- is STDIN)")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  bool flag = false;
  if (!conf->count("reference")) {
    cerr << "Please specify one or more references using -r <REF.TXT>\n";
    flag = true;
  }
  if (flag || conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

bool ReadSparseVectorString(const string& s, SparseVector<double>* v) {
#if 0
  // this should work, but untested.
  std::istringstream i(s);
  i>>*v;
#else
  vector<string> fields;
  Tokenize(s, ';', &fields);
  if (fields.empty()) return false;
  for (int i = 0; i < fields.size(); ++i) {
    vector<string> pair(2);
    Tokenize(fields[i], '=', &pair);
    if (pair.size() != 2) {
      cerr << "Error parsing vector string: " << fields[i] << endl;
      return false;
    }
    v->set_value(FD::Convert(pair[0]), atof(pair[1].c_str()));
  }
  return true;
#endif
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  const string loss_function = conf["loss_function"].as<string>();
  ScoreType type = ScoreTypeFromString(loss_function);
  DocScorer ds(type, conf["reference"].as<vector<string> >(), conf["source"].as<string>());
  cerr << "Loaded " << ds.size() << " references for scoring with " << loss_function << endl;
  Hypergraph hg;
  string last_file;
  ReadFile in_read(conf["input"].as<string>());
  istream &in=*in_read.stream();
  while(in) {
    string line;
    getline(in, line);
    if (line.empty()) continue;
    istringstream is(line);
    int sent_id;
    string file, s_origin, s_axis;
    // path-to-file (JSON) sent_ed starting-point search-direction
    is >> file >> sent_id >> s_origin >> s_axis;
    SparseVector<double> origin;
    assert(ReadSparseVectorString(s_origin, &origin));
    SparseVector<double> axis;
    assert(ReadSparseVectorString(s_axis, &axis));
    // cerr << "File: " << file << "\nAxis: " << axis << "\n   X: " << origin << endl;
    if (last_file != file) {
      last_file = file;
      ReadFile rf(file);
      HypergraphIO::ReadFromJSON(rf.stream(), &hg);
    }
    ViterbiEnvelopeWeightFunction wf(origin, axis);
    ViterbiEnvelope ve = Inside<ViterbiEnvelope, ViterbiEnvelopeWeightFunction>(hg, NULL, wf);
    ErrorSurface es;
    ComputeErrorSurface(*ds[sent_id], ve, &es, type, hg);
    //cerr << "Viterbi envelope has " << ve.size() << " segments\n";
    // cerr << "Error surface has " << es.size() << " segments\n";
    string val;
    es.Serialize(&val);
    cout << 'M' << ' ' << s_origin << ' ' << s_axis << '\t';
    B64::b64encode(val.c_str(), val.size(), &cout);
    cout << endl << flush;
  }
  return 0;
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "ambiguous_read_fraction.hpp"

#include <algorithm>
#include <iterator>

#include <boost/variant.hpp>

#include "core/tools/read_assigner.hpp"
#include "core/types/allele.hpp"
#include "io/variant/vcf_record.hpp"
#include "io/variant/vcf_spec.hpp"
#include "utils/genotype_reader.hpp"
#include "../facets/samples.hpp"
#include "../facets/overlapping_reads.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string AmbiguousReadFraction::name_ = "ARF";

std::unique_ptr<Measure> AmbiguousReadFraction::do_clone() const
{
    return std::make_unique<AmbiguousReadFraction>(*this);
}

Measure::ResultType AmbiguousReadFraction::get_default_result() const
{
    return std::vector<boost::optional<double>> {};
}

Measure::ResultType AmbiguousReadFraction::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& reads = get_value<OverlappingReads>(facets.at("OverlappingReads"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).haplotypes;
    std::vector<boost::optional<double>> result {};
    result.reserve(samples.size());
    for (const auto& sample : samples) {
        boost::optional<double> sample_result {};
        const auto num_overlapping_reads = count_overlapped(reads.at(sample), call);
        if (num_overlapping_reads > 0) {
            if (assignments.count(sample) == 1) {
                const auto num_ambiguous_reads = count_overlapped(assignments.at(sample).ambiguous_wrt_reference, call);
                sample_result = static_cast<double>(num_ambiguous_reads) / num_overlapping_reads;
            } else {
                sample_result = 0;
            }
        }
        result.push_back(sample_result);
    }
    return result;
}

Measure::ResultCardinality AmbiguousReadFraction::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& AmbiguousReadFraction::do_name() const
{
    return name_;
}

std::string AmbiguousReadFraction::do_describe() const
{
    return "Fraction of reads overlapping the call that cannot be assigned to a unique haplotype";
}

std::vector<std::string> AmbiguousReadFraction::do_requirements() const
{
    return {"Samples", "OverlappingReads", "ReadAssignments"};
}

} // namespace csr
} // namespace octopus


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

// Library headers.
#ifndef INCLUDED_FUNCTIONAL
#include <functional>
#define INCLUDED_FUNCTIONAL
#endif

#ifndef INCLUDED_BOOST_BIND
#include <boost/bind.hpp>
#define INCLUDED_BOOST_BIND
#endif

// PCRaster library headers.
#ifndef INCLUDED_DAL_MATHUTILS
#include "dal_MathUtils.h"
#define INCLUDED_DAL_MATHUTILS
#endif

#ifndef INCLUDED_DISCR_BLOCK
#include "discr_block.h"
#define INCLUDED_DISCR_BLOCK
#endif

#ifndef INCLUDED_DISCR_BLOCKDATA
#include "discr_blockdata.h"
#define INCLUDED_DISCR_BLOCKDATA
#endif

#ifndef INCLUDED_DISCR_RASTERDATA
#include "discr_rasterdata.h"
#define INCLUDED_DISCR_RASTERDATA
#endif

#ifndef INCLUDED_DISCR_VOXELSTACK
#include "discr_voxelstack.h"
#define INCLUDED_DISCR_VOXELSTACK
#endif

// Module headers.
#ifndef INCLUDED_BLOCK_COMPACTORS
#include "block_compactors.h"
#define INCLUDED_BLOCK_COMPACTORS
#endif

#ifndef INCLUDED_BLOCK_DEHAANCOMPACTOR
#include "block_dehaancompactor.h"
#define INCLUDED_BLOCK_DEHAANCOMPACTOR
#endif

#ifndef INCLUDED_BLOCK_TYPES
#include "block_types.h"
#define INCLUDED_BLOCK_TYPES
#endif

/*!
  \file
  This file contains the implementation of the Add class.
*/

namespace block {

void noCompactionAdd(
         discr::Block& block,
         size_t nrVoxels,
         REAL4 thickness)
{
  block.addVoxels(nrVoxels, thickness);
}

void noCompactionAdd(
         discr::Block& block,
         discr::RasterData<REAL4> const& thickness)
{
  DEVELOP_PRECOND(
         thickness.raster() == &static_cast<discr::Raster const&>(block) ||
         *thickness.raster() == static_cast<discr::Raster const&>(block));

  for(size_t i = 0; i < block.nrCells(); ++i) {
    if(thickness.isMV(i)) {
      block.cell(i).setMV();
    }
    else if(!block.cell(i).isMV()) {
      DEVELOP_PRECOND(dal::greaterOrComparable(thickness.cell(i), REAL4(0.0)));
      if(thickness.cell(i) > REAL4(0.0)) {
        block.addVoxels(i, 1, thickness.cell(i));
      }
    }
  }
}

//!
/*!
  \param     .
  \return    .
  \exception .
  \warning   .
  \sa        .
  \todo      Document each argument. Handle compaction. Compaction function
             should be an argument for efficiency.
  \todo      Check implementation with Derek.

  This function is called by a function which handles the argument checking.
  We can assume here that all arguments have valid values.
*/
static void mackeyBridgeAdd(
         discr::Block& block,
         size_t index,
         std::vector<REAL4>& originalThickness,
         REAL4 thickness,
         REAL4 maxVoxelThickness,
         std::vector<INT4>& sediment,
         INT4 sedimentToAdd,
         Compactors<MackeyBridgeCompactor> const& compactors)
{
  double depth;
  double voxelThickness;

  discr::VoxelStack& stack(block.cell(index));

  if(!stack.empty()) {

    // --------------------------------
    // PROCESS EXISTING STACK OF VOXELS
    // --------------------------------
    // Compact existing sediment, from top to bottom. Don't change the
    // voxel at the top.
    depth = stack[stack.size() - 1];

    for(int i = stack.size() - 2; i >= 0; --i) {

      // The depth of burial of the voxel is the thickness of
      // the voxels on top of the current voxel (which are already
      // compacted in this loop).
      // Compact the voxel.
      DEVELOP_PRECOND(!pcr::isMV(originalThickness[i]));
      DEVELOP_PRECOND(originalThickness[i] > 0.0);
      // stack[i] = compactors.compact(sedimentToAdd, originalThickness[i],
      //        depth + thickness);
      stack[i] = compactors.compactor(sedimentToAdd)(originalThickness[i],
             depth + thickness);

      // Calculate depth of next voxel.
      depth += stack[i];
    }

    DEVELOP_PRECOND(!stack.empty());
    size_t i = stack.size() - 1;   // Index of top voxel.
    DEVELOP_PRECOND(!pcr::isMV(originalThickness[i]));
    DEVELOP_PRECOND(originalThickness[i] > 0.0);

    if(sediment[i] == sedimentToAdd &&
             dal::comparable(originalThickness[i], stack[i]) &&
             maxVoxelThickness > originalThickness[i]) {

      // The top voxel:
      //   - has the same sediment type, and
      //   - is not compacted, and
      //   - has room for new material

      // Uncompacted thickness to add, including stuff already in top voxel.
      thickness += originalThickness[i];

      if(maxVoxelThickness >= thickness) {

        // The current top voxel stays the top voxel. We've just enough
        // new material to add some to the top voxel and that's it.
        originalThickness[i] = thickness;
        stack[i] = originalThickness[i];
        thickness -= thickness; // Should be 0.0 by now.
      }
      else {

        // The current top voxel will be buried with other material.
        // We fill it untill its maximum original thickness is reached
        // and compact it.
        DEVELOP_PRECOND(thickness > maxVoxelThickness);
        originalThickness[i] = maxVoxelThickness;
        thickness -= maxVoxelThickness;   // Should be > 0.0 here.
        // stack[i] = compactors.compact(sediment[i], originalThickness[i],
        //       thickness);
        stack[i] = compactors.compactor(sediment[i])(originalThickness[i],
              thickness);
        DEVELOP_PRECOND(thickness > REAL4(0.0));
      }
    }
    else {

      // The top voxel:
      //   - has a different sediment type, or
      //   - is already compacted, or
      //   - has no room for new material
      // We can treat the top voxel as all lower voxels: compact.
      // stack[i] = compactors.compact(sediment[i], stack[i], thickness);
      stack[i] = compactors.compactor(sediment[i])(stack[i], thickness);
    }
  }

  // Check if there's accomodation space left to fill.
  if(thickness > REAL4(0.0)) {

    // ------------------
    // PROCESS NEW VOXELS
    // ------------------
    // Add and compact new voxels from bottom to top. Continue untill
    // the available space left is less than the height of one
    // uncompacted voxel.
    voxelThickness = maxVoxelThickness;
    size_t i = stack.size();

    // Add whole voxels, with thickness maxVoxelThickness.
    for(; dal::greaterOrComparable(thickness, maxVoxelThickness);
         thickness -= voxelThickness, ++i) {

      voxelThickness = compactors.compactor(sedimentToAdd)(maxVoxelThickness,
         thickness - maxVoxelThickness);

      // Add a new voxel with thickness voxelThickness.
      block.addVoxel(index, voxelThickness);
      DEVELOP_PRECOND(i == stack.size() - 1);
      sediment[i] = sedimentToAdd;
      originalThickness[i] = maxVoxelThickness;
    }

    // Fails
    // DEVELOP_PRECOND(dal::greaterOrComparable(thickness, REAL4(0.0)));

    // Now we have filled up thickness except for a small amount of
    // space left which is less than maxVoxelThickness. Here we add a
    // uncompacted voxel to fill up the last part of thickness.
    if(thickness > REAL4(0.0)) {

      // Add a new voxel with thickness thickness.
      DEVELOP_PRECOND(sediment.size() == stack.size());
      DEVELOP_PRECOND(originalThickness.size() == stack.size());
      block.addVoxel(index, thickness);
      DEVELOP_PRECOND(i == stack.size() - 1);
      DEVELOP_POSTCOND(sediment.size() == stack.size());
      DEVELOP_POSTCOND(originalThickness.size() == stack.size());
      sediment[i] = sedimentToAdd;
      originalThickness[i] = thickness;
    }
  }
}

//! Adds sediment to an existing block with sediments, thereby changing the spatial extent of the block and all its data subjects.
/*!
  \param     block Block discretisation to add to. This will be changed.
  \param     originalThickness Original thicknesses (before compaction)
             of the voxels in the block. This will be changed.
  \param     sediment Block data with the current values. This will be changed.
  \param     thickness Amount of sediment to add to the block data.
  \param     maxVoxelThickness Maximum thickness of newly created voxels.
  \return    .
  \exception .
  \warning   .
  \sa        .

  Not an easy function to use, requires some bookkeeping by the user.
  This is also not a general function, it is limited to the domain of
  sedimentation and refers directly to sediment and compaction.

  New voxels created in the nominal block data will have the default
  value set for this argument. This is a common rule in this library:
  when the dicretisation is extended, all data objects are extended
  too. Newly created voxels will be set to the default value set for
  each data object.

  add(block, originalThickness, sediment, thickness, maxVoxelThickness);
*/
void mackeyBridgeAdd(discr::Block& block,
         discr::BlockData<REAL4>& originalThickness,
         discr::BlockData<INT4>& sediment,
         discr::RasterData<REAL4>const & thickness,
         REAL4 maxVoxelThickness,
         Compactors<MackeyBridgeCompactor> const& compactors)
{
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(originalThickness);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(sediment);

  DEVELOP_PRECOND(
         originalThickness.block() == &block ||
         *originalThickness.block() == block);
  DEVELOP_PRECOND(
         sediment.block() == &block ||
         *sediment.block() == block);
  DEVELOP_PRECOND(
         thickness.raster() == &static_cast<discr::Raster const&>(block) ||
         *thickness.raster() == static_cast<discr::Raster const&>(block));
  PRECOND(!pcr::isMV(maxVoxelThickness));
  PRECOND(maxVoxelThickness > 0.0);

  for(size_t i = 0; i < block.nrCells(); ++i) {
    if(!block.cell(i).isMV()) {
      if(!pcr::isMV(sediment.defaultValue().cell(i)) && (!thickness.isMV(i))) {
        PRECOND(dal::greaterOrComparable(thickness.cell(i), REAL4(0.0)));
        mackeyBridgeAdd(block, i,
           originalThickness.cell(i), thickness.cell(i), maxVoxelThickness,
           sediment.cell(i), sediment.defaultValue().cell(i), compactors);
      }
    }
  }

  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(originalThickness);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(sediment);
}

static void deHaanAdd(
         discr::Block& block,
         size_t index,
         std::vector<INT4>& sediment,
         INT4 sedimentToAdd,
         std::vector<REAL4>& initialThickness,
         std::vector<REAL4>& cummulativeLoad,
         std::vector<REAL4>& cummulativeDuration,
         REAL4 duration,
         REAL4 thickness,
         Compactors<DeHaanCompactor> const& compactors)
{
  DEVELOP_PRECOND(!(thickness < 0.0));

  if(dal::comparable(thickness, REAL4(0.0))) {
    return;
  }

  discr::VoxelStack& stack(block.cell(index));

  DeHaanCompactor const& compactor(compactors.compactor(sedimentToAdd));

  // -----------------
  // PROCESS NEW VOXEL
  // -----------------

  // Add new load to current stack of loads.
  REAL4 load = 0.5 * thickness * compactor.buoyancy();
  std::transform(cummulativeLoad.begin(), cummulativeLoad.end(),
         cummulativeLoad.begin(),
         boost::bind(std::plus<REAL4>(), _1, load));
  std::transform(cummulativeDuration.begin(), cummulativeDuration.end(),
         cummulativeDuration.begin(),
         boost::bind(std::plus<REAL4>(), _1, duration));

  // Add voxel to the discretisation which will signal the attributes.
  block.addVoxel(index, compactor(thickness, load, duration));

  // Set attribute values for newly create voxels.
  initialThickness.back() = thickness;
  cummulativeLoad.back() = load;
  DEVELOP_PRECOND(dal::comparable(cummulativeDuration.back(), duration));
  DEVELOP_PRECOND(sediment.back() == sedimentToAdd);

  if(stack.size() > 1) {
    // --------------------------------
    // PROCESS EXISTING STACK OF VOXELS
    // --------------------------------
    // Compact existing sediment.
    for(size_t i = 0; i < stack.size() - 1; ++i) {
      stack[i] = compactor(initialThickness[i], cummulativeLoad[i],
         cummulativeDuration[i]);
    }
  }
}

void deHaanAdd(
         discr::Block& block,
         discr::BlockData<INT4>& sediment,
         discr::BlockData<REAL4>& initialThickness,
         discr::BlockData<REAL4>& cummulativeLoad,
         discr::BlockData<REAL4>& cummulativeDuration,
         discr::RasterData<REAL4> const& thickness,
         Compactors<DeHaanCompactor> const& compactors)
{
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(sediment);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(initialThickness);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(cummulativeLoad);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(cummulativeDuration);

  for(size_t i = 0; i < block.nrCells(); ++i) {
    if(!block.cell(i).isMV()) {
      if(!pcr::isMV(sediment.defaultValue().cell(i)) && !thickness.isMV(i)) {
        PRECOND(dal::greaterOrComparable(thickness.cell(i), REAL4(0.0)));
        deHaanAdd(block, i, sediment.cell(i), sediment.defaultValue().cell(i),
              initialThickness.cell(i), cummulativeLoad.cell(i),
              cummulativeDuration.cell(i),
              cummulativeDuration.defaultValue().cell(i),
              thickness.cell(i), compactors);
      }
    }
  }

  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(sediment);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(initialThickness);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(cummulativeLoad);
  DEVELOP_CHECK_BLOCK_DATA_CONSISTENCY(cummulativeDuration);
}

} // namespace block


//
//  bpp_c.hpp for mammal
//  PhyloAcc_init2
//
//  Created by hzr on 4/19/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#ifndef bpp_c_hpp
#define bpp_c_hpp

#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <string>
#include <armadillo>
#include <cassert>

#include <cmath>
#include <sys/time.h>

#include <map>
#include <fstream>
#include <vector>
#include <set>
#include <sstream>
#include <algorithm>

#include "newick.h"
#include "profile.h"
#include "bpp.hpp"
#include "utils.h"

using namespace std;
using namespace arma;

class BPP_C
{
    private:

    int CC;  //current number of elements
    int GG; //base pairs current elements

    int S;
    int N;

    vector< vector<vec > > ambiguousS_null;  //base * species# * 4;

    int    (*children2)[2];  //children from pruned tree
    double  *distances2;
    int *parent2;
    int root;
    vec pi;
    vec log_pi;

    vector<int> nodes;
    vector<int> internal_nodes;
    vector<bool> missing;
    vector<int> upper_c;
    vector<int> upper_conserve_c;

   // double prior_glr[3];
    double prior_l_a, prior_l_b;
     double prior_l2_a, prior_l2_b;
     double prior_g_a, prior_g_b;
    double ratio0;
    double ratio1;
    int num_burn;   // num of burn-in updates
    int num_mcmc;   // num of MCMC updates
    int num_thin;   // num of updates between two samples
    int adaptive_freq = 100;

    vector<int>Z ; //N *0, accelerate(time,0-1), loss(-1)
    vector<int>fixZ ;
    vector < vector<int> > Tg; //current history for each element, GG*N

    //buffer P(zt|zt-1,Hg)
    vector<vec> log_prob_back;

    //get P(X|theta)
    vector< vector<double> >  log_emission; //( N-1)*2

    // MCMC updating states
    int m;                                      // current MCMC step

    vector<mat> log_TM_Int;

    vec prior_z;

    vector<mat > log_cache_TM_neut;
    vector<mat > log_cache_TM_cons;
    vector<mat > log_cache_TM_null;

    vector< vector<vec> > lambda;  //GG *N *acgt

    // samples to output
    double MaxLoglik;
    int Max_m;
    vector <int > Max_Z;

    vector< double >  trace_loglik;  //P(X|Z, TM, r)
    vector< double >  trace_full_loglik;  //P(X,Z,r|TM)
    vector< vector<int> > trace_Z;  //iteration * Species * {0,1,2}, only for init
    vector <double> trace_n_rate;  //iter*element
    vector <double> trace_c_rate;

    vector <double> trace_l_rate;  //iter*element
    vector <double> trace_l2_rate;
    vector <double> trace_g_rate;

    int accept_n_rate = 0;
    int accept_c_rate = 0;  //how many accepted in current cycle

    double prop_n;  //for adaptive MCMC, changed by acceptance rate
    double prop_c;

    // GSL random number generator
    gsl_rng * RNG;

    time_t last_time;

    unsigned long int seed;

public:
    bool failure = false;
    bool verbose;

    BPP_C(int c, PhyloProf _prof, BPP& bpp, char gapchar, double missing_thres, bool & filter, bool _verbose, double consToMis, bool prune=0, double revgap=0, int min_length =50, double nconsToMis = 1)//, double _indel)
    {

        RNG = gsl_rng_alloc(gsl_rng_default);
        gsl_rng_set(RNG, bpp.seed);

        num_burn = bpp.num_burn;   // num of burn-in updates
        num_mcmc = bpp.num_mcmc;   // num of MCMC updates
        adaptive_freq = bpp.num_thin;   // num of updates between two samples, 100

        N = bpp.N;
        CC = c;
        GG =bpp.element_size[c];

        if(GG < min_length)
        {
            filter = true;
            return;
        }

        Tg = vector<vector<int> > (GG, vector<int>(N, -1));
        root = N-1;
        S = bpp.S;

        lambda = vector< vector<vec> >(GG, vector<vec>(N, zeros<vec>(bpp.num_base)));

        ratio0 = bpp.ratio0;  //no use
        ratio1 = bpp.ratio1; //no use

        verbose = _verbose;

        log_cache_TM_neut = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_cons = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_null = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));

        log_TM_Int = vector<mat >(N, zeros<mat>(3,2));

//        for(size_t i =0 ;i <3; i++)  // hyperparameter for loss and gain rates
//        {
//            prior_glr[i] = bpp.prior_glr[i];
//        }

        prior_l_a = bpp.prior_l_a;
        prior_l_b = bpp.prior_l_b;
        prior_g_a = bpp.prior_g_a;
        prior_g_b = bpp.prior_g_b;
        prior_l2_a = bpp.prior_l2_a;
        prior_l2_b = bpp.prior_l2_b;

        prior_z = zeros<vec>(3);
        prior_z[0] = 0.5;  // prior for root
        prior_z[1] = 0.5;
        prior_z = log(prior_z);

        if(verbose) cout << "Init lambda" <<endl;
        vector<int> num_missing = vector<int> (S,0);

        int st = bpp.element_start[CC];
        //get leaves lambda
        for(int s=0; s<S; s++)
        {
            //const char* y = bpp.X[c][s].c_str();
            string y = _prof.X[s].substr(st, st+GG);

            for(int g=0; g<GG; g++){
                lambda[g][s].fill(LOG_ZERO);
//                if( y[g]== gapchar || y[g]=='n')
//                {
//                    num_missing[s] ++;
//                    //Tg[g][s] = 4;  // for missing base pair
//                }
                switch (y[g])
                {
                    case 'a':
                        lambda[g][s][0] = 0; Tg[g][s] = 0; break;
                    case 'c':
                        lambda[g][s][1] = 0; Tg[g][s] = 1; break;
                    case 'g':
                        lambda[g][s][2] = 0; Tg[g][s] = 2; break;
                    case 't':
                        lambda[g][s][3] = 0; Tg[g][s] = 3; break;
                    case 'r':
                        lambda[g][s][0] = 0;
                        lambda[g][s][2] = 0;
                        break;
                    case 'y':
                        lambda[g][s][1] = 0;
                        lambda[g][s][3] = 0;   break;
                    case 'k':
                        lambda[g][s][2] = 0;
                        lambda[g][s][3] = 0;  break;
                    case 'm':
                        lambda[g][s][0] = 0;
                        lambda[g][s][1] = 0;  break;
                    case 's':
                        lambda[g][s][1] = 0;
                        lambda[g][s][2] = 0;   break;
                    case 'w':
                        lambda[g][s][0] = 0;
                        lambda[g][s][3] = 0;   break;
                    // case gapchar:
                    //     if(bpp.num_base <= 4){
                    //         for(int b =0;b<bpp.num_base;b++) lambda[g][s][b] = 0;
                    //     }
                    //     else{
                    //         lambda[g][s][4] = 0;
                    //     }
                    //     Tg[g][s] = 4; break;
                    default:
                        for(int b =0;b<bpp.num_base;b++) lambda[g][s][b] = 0;
                        if(y[g] == gapchar) {Tg[g][s] = 4; break;}
                        Tg[g][s] = 5;
                }
            }

        }

        // remove columns with nearly all gaps
        if(revgap < 1)
        {
            //remove bases with 'n'/'*' or gap in more than 80% species
            vector<int> missingBase;
            for(int g=0; g<GG; g++)
            {
                int mis = 0;
                for(int s=0; s<S; s++){
                    if(Tg[g][s] >= 4)
                    {
                        mis++;
                    }
                }
                if(mis > S*revgap)
                {
                    missingBase.push_back(g);
                }

            }

            if(GG - missingBase.size() < min_length)
            {
                filter = true;
                return;
            }

            for(vector<int>::reverse_iterator it = missingBase.rbegin(); it!= missingBase.rend(); it ++)
            {
                lambda.erase(lambda.begin() + *it);
                Tg.erase(Tg.begin() + *it);
            }

            GG = lambda.size();
        }

        // get gap species after filtering
        for(int s=0; s<S; s++)
        {
            for(int g= 0; g < GG; g++)
            {
                if( Tg[g][s] == 4)
                {
                    num_missing[s] ++;
                }

            }
        }

        ambiguousS_null = vector<vector<vec> > (GG, vector<vec>(S,zeros<vec>(bpp.num_base)));

        children2    = new int[N][2];
        parent2      = new int[N];
        distances2   = new double[N];
        for(int i=0; i<N; i++)
        {
            children2[i][0] = bpp.children[i][0];
            children2[i][1] = bpp.children[i][1];
            parent2[i] = bpp.parent[i];
            distances2[i] = bpp.distances[i];
        }

        // set missing
        missing = vector<bool>(N, false);

        for(int s=S; s<N; s++)
        {
            int* p = children2[s];
            for(int cc=0;cc<2;cc++)
            {
                int chi = p[cc];
                if(chi<S && num_missing[chi]>missing_thres*GG)
                {

                    missing[chi] = true;

                }
            }

            if(missing[p[0]] && missing[p[1]]) {
                missing[s] = true;

            }
        }

//        cout << "missing: ";
//        for(int s = 0; s<N;s++)
//        {
//            if(missing[s]) cout << s << " ";
//        }
//        cout <<endl;

        int ct =0;
        for(vector<int>::iterator it = bpp.conservedgroup.begin(); it<bpp.conservedgroup.end();it++)
        {
            if(missing[*it]){
                ct +=1;
            }
        }

        if(ct > bpp.conserve_prop * bpp.conservedgroup.size())
        {
            filter = true;
            return;
        }

        //prune tree if outgroup not conserved, find root
        if(prune)
        {
            set<int> alls;
            for(int s=0; s<N; s++) alls.insert(s);
            getSubtree_missing(N-1, alls, -1);
        }else{
            getSubtree_missing(N-1, bpp.upper, -1);
        }

        parent2[root] = N;  //parent2 not correct for all nodes, but children2 is correct

        if(verbose) cout << "root: " << root << endl;

        getSubtree(root, nodes);
        for(vector<int>::iterator it = nodes.begin(); it < nodes.end(); it++)
        {
            if(*it>=S)
            {
                internal_nodes.push_back(*it);  // internal nodes
            }

            if(bpp.upper.find(*it)!=bpp.upper.end())
            {
                    upper_c.push_back(*it);
            }

            if(bpp.upper_conserve.find(*it)!=bpp.upper_conserve.end())
            {
                upper_conserve_c.push_back(*it);
            }

        }

        // if both children are missing, can't infer the parent's base pair, just set to 'missing'
        for(vector<int>::iterator it = internal_nodes.begin(); it < internal_nodes.end(); it++)
        {
            int* p = children2[*it];
            for(int g=0; g<GG; g++){
                if(Tg[g][p[0]] >= bpp.num_base && Tg[g][p[1]]>= bpp.num_base)
                {
                    Tg[g][*it] = bpp.num_base;

                }
            }

        }

        log_pi = bpp.log_pi;

        // from initMCMC
        trace_loglik = vector<double >(num_burn+num_mcmc, 0); //P(X|Z, r)
        trace_full_loglik = vector<double >(num_burn+num_mcmc, 0); //P(X, Z, r)
        trace_Z = vector<vector<int> >(num_burn+num_mcmc, vector<int>(N,1));
        trace_n_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_c_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_l_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_g_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_l2_rate = vector<double >(num_burn+num_mcmc, 0);

        //getEmission_ambig();

        log_emission = vector<vector<double> >(N, vector<double>(3,0));

        for(int s=0;s<N;s++)  // For all nodes !!! ....only terminal nodes, S
        {
            if(missing[s])
            {
                //            log_prob_back[s][2] += log(nconsTomis);
                //            log_prob_back[s][1] = log(consTomis) ;//-INFINITY;
                //            log_prob_back[s][0] += log(nconsTomis);

                log_emission[s][2] = log(nconsToMis);
                log_emission[s][1] = log(consToMis) ;//-INFINITY;
                log_emission[s][0] = log(nconsToMis);

                //fixZ[s] = 0; // ?

            }
            //        }else{
            //
            //            log_prob_back[s][2] += log(1 - nconsTomis);
            //            log_prob_back[s][1] = log( 1 - consTomis) ;//-INFINITY;
            //            log_prob_back[s][0] += log(1 - nconsTomis);
            //
            //
            //        }
        }

    }

    ~BPP_C()
    {
        gsl_rng_free(RNG);
    }

    void getSubtree(int root, set<int>& child, vector<int> & visited_init);
    void getSubtree(int root, vector<int> & visited_init);
    void getSubtree_missing(int root, set<int>& upper, int child);

    void getEmission_ambig();
    void initMCMC(int iter, BPP&bpp, int resZ);
    void Update_Tg(int g, vector<bool> visited,BPP& bpp, bool tosample);
    void getUpdateNode(vector<int> changedZ, vector<bool> & visited_init);
    void MonitorChain(int m, BPP &bpp, double &loglik, const double add_loglik, const int resZ) ;
    void getUpdateNode(bool neut,vector<bool> & visited_init);
    double log_f_Xz(vector<bool> visited, vector<vector<vec>>& lambda_tmp, bool neut, double propos, BPP& bpp);
    double sample_rate(int resZ, double old_rate, bool neut, vector<bool> visited, double & loglik_old, BPP& bpp, int M =1, bool adaptive = true, double adaptive_factor = 0.5);
    void Gibbs(int iter, BPP &bpp, ofstream & outZ, string output_path,string output_path2,int resZ, bool UpR, bool UpHyper, double lrate_prop, double grate_prop);
    vector<int> Update_Z_subtree(int num_base = 5, bool prior = false);
    void Output_init(string output_path, string output_path2, BPP& bpp,ofstream& outZ, int resZ);
    void Output_sampling(int iter, string output_path2, BPP &bpp, int resZ);

    void getEmission(int num_base);

    vector<int>  Move_Z(int & propConf, int & revConf,  int & changeZ);
    double log_f_Xz(vec log_pi, int num_base, vector<int>& Z, vector<mat> & log_cache_TM_neut,  vector<mat> & log_cache_TM_cons);
    double Update_f_Xz(vec log_pi, int num_base, vector<int>& Z, vector<mat> & log_cache_TM_neut, vector<mat> & log_cache_TM_cons, vector<bool> & visited);
    void log_f_Z(vector<int>& Z, vector<mat> & log_Int, double & MH_ratio_g, double & MH_ratio_l);

    //void Eval(BPP&bpp,int resZ); //, int numH,int numHZ);
    void Eval2(BPP&bpp, int resZ);
    //double prior_Z_subtree(vector<int> & tmpZ) ;
    vector<double> prior_Z_subtree(vector< vector<int> > & configZ, vector< int > numConfigZ);
    void sample_transition( double  & gr, double  & lr, double  & lr2);

};

#endif /* bpp_c_hpp */


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

/// \file UnderwaterCurrentPlugin.cc

#include <math.h>

#include <ros/ros.h>
#include <ros/package.h>
#include <boost/algorithm/string.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/PhysicsEngine.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <sdf/sdf.hh>
#include <dave_world_plugins/UnderwaterCurrentPlugin.hh>

using namespace gazebo;

GZ_REGISTER_WORLD_PLUGIN(UnderwaterCurrentPlugin)

/////////////////////////////////////////////////
UnderwaterCurrentPlugin::UnderwaterCurrentPlugin()
{
  // Doing nothing for now
}

/////////////////////////////////////////////////
UnderwaterCurrentPlugin::~UnderwaterCurrentPlugin()
{
#if GAZEBO_MAJOR_VERSION >= 8
  this->updateConnection.reset();
#else
  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
#endif
}

/////////////////////////////////////////////////
void UnderwaterCurrentPlugin::
  Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
  GZ_ASSERT(_world != NULL, "World pointer is invalid");
  GZ_ASSERT(_sdf != NULL, "SDF pointer is invalid");

  this->world = _world;
  this->sdf = _sdf;

  // Read the namespace for topics and services
  this->ns = _sdf->Get<std::string>("namespace");

  gzmsg << "Loading underwater world..." << std::endl;
  // Initializing the transport node
  this->node = transport::NodePtr(new transport::Node());
#if GAZEBO_MAJOR_VERSION >= 8
  this->node->Init(this->world->Name());
#else
  this->node->Init(this->world->GetName());
#endif
  // Retrieve the velocity configuration, if existent
  GZ_ASSERT(this->sdf->HasElement("constant_current"),
    "Current configuration not available");
  sdf::ElementPtr currentVelocityParams = this->sdf->GetElement(
    "constant_current");

  // Read the topic names from the SDF file
  if (currentVelocityParams->HasElement("topic"))
    this->currentVelocityTopic =
      currentVelocityParams->Get<std::string>("topic");
  else
    this->currentVelocityTopic = "current_velocity";

  GZ_ASSERT(!this->currentVelocityTopic.empty(),
    "Empty ocean current velocity topic");

  if (currentVelocityParams->HasElement("velocity"))
  {
    sdf::ElementPtr elem = currentVelocityParams->GetElement("velocity");
    if (elem->HasElement("mean"))
        this->currentVelModel.mean = elem->Get<double>("mean");
    if (elem->HasElement("min"))
        this->currentVelModel.min = elem->Get<double>("min");
    if (elem->HasElement("max"))
        this->currentVelModel.max = elem->Get<double>("max");
    if (elem->HasElement("mu"))
        this->currentVelModel.mu = elem->Get<double>("mu");
    if (elem->HasElement("noiseAmp"))
        this->currentVelModel.noiseAmp = elem->Get<double>("noiseAmp");

    GZ_ASSERT(this->currentVelModel.min < this->currentVelModel.max,
      "Invalid current velocity limits");
    GZ_ASSERT(this->currentVelModel.mean >= this->currentVelModel.min,
      "Mean velocity must be greater than minimum");
    GZ_ASSERT(this->currentVelModel.mean <= this->currentVelModel.max,
      "Mean velocity must be smaller than maximum");
    GZ_ASSERT(this->currentVelModel.mu >= 0 && this->currentVelModel.mu < 1,
      "Invalid process constant");
    GZ_ASSERT(this->currentVelModel.noiseAmp < 1 &&
      this->currentVelModel.noiseAmp >= 0,
      "Noise amplitude has to be smaller than 1");
  }

  this->currentVelModel.var = this->currentVelModel.mean;
  gzmsg << "Current velocity [m/s] Gauss-Markov process model:" << std::endl;
  this->currentVelModel.Print();

  if (currentVelocityParams->HasElement("horizontal_angle"))
  {
    sdf::ElementPtr elem =
      currentVelocityParams->GetElement("horizontal_angle");

    if (elem->HasElement("mean"))
      this->currentHorzAngleModel.mean = elem->Get<double>("mean");
    if (elem->HasElement("min"))
      this->currentHorzAngleModel.min = elem->Get<double>("min");
    if (elem->HasElement("max"))
      this->currentHorzAngleModel.max = elem->Get<double>("max");
    if (elem->HasElement("mu"))
      this->currentHorzAngleModel.mu = elem->Get<double>("mu");
    if (elem->HasElement("noiseAmp"))
      this->currentHorzAngleModel.noiseAmp = elem->Get<double>("noiseAmp");

    GZ_ASSERT(this->currentHorzAngleModel.min <
      this->currentHorzAngleModel.max,
      "Invalid current horizontal angle limits");
    GZ_ASSERT(this->currentHorzAngleModel.mean >=
      this->currentHorzAngleModel.min,
      "Mean horizontal angle must be greater than minimum");
    GZ_ASSERT(this->currentHorzAngleModel.mean <=
      this->currentHorzAngleModel.max,
      "Mean horizontal angle must be smaller than maximum");
    GZ_ASSERT(this->currentHorzAngleModel.mu >= 0 &&
      this->currentHorzAngleModel.mu < 1,
      "Invalid process constant");
    GZ_ASSERT(this->currentHorzAngleModel.noiseAmp < 1 &&
      this->currentHorzAngleModel.noiseAmp >= 0,
      "Noise amplitude for horizontal angle has to be between 0 and 1");
  }

  this->currentHorzAngleModel.var = this->currentHorzAngleModel.mean;
  gzmsg <<
    "Current velocity horizontal angle [rad] Gauss-Markov process model:"
    << std::endl;
  this->currentHorzAngleModel.Print();

  if (currentVelocityParams->HasElement("vertical_angle"))
  {
    sdf::ElementPtr elem = currentVelocityParams->GetElement("vertical_angle");

    if (elem->HasElement("mean"))
      this->currentVertAngleModel.mean = elem->Get<double>("mean");
    if (elem->HasElement("min"))
      this->currentVertAngleModel.min = elem->Get<double>("min");
    if (elem->HasElement("max"))
      this->currentVertAngleModel.max = elem->Get<double>("max");
    if (elem->HasElement("mu"))
      this->currentVertAngleModel.mu = elem->Get<double>("mu");
    if (elem->HasElement("noiseAmp"))
      this->currentVertAngleModel.noiseAmp = elem->Get<double>("noiseAmp");

    GZ_ASSERT(this->currentVertAngleModel.min <
      this->currentVertAngleModel.max, "Invalid current vertical angle limits");
    GZ_ASSERT(this->currentVertAngleModel.mean >=
      this->currentVertAngleModel.min,
      "Mean vertical angle must be greater than minimum");
    GZ_ASSERT(this->currentVertAngleModel.mean <=
      this->currentVertAngleModel.max,
      "Mean vertical angle must be smaller than maximum");
    GZ_ASSERT(this->currentVertAngleModel.mu >= 0 &&
      this->currentVertAngleModel.mu < 1,
      "Invalid process constant");
    GZ_ASSERT(this->currentVertAngleModel.noiseAmp < 1 &&
      this->currentVertAngleModel.noiseAmp >= 0,
      "Noise amplitude for vertical angle has to be between 0 and 1");
  }

  this->currentVertAngleModel.var = this->currentVertAngleModel.mean;
  gzmsg <<
    "Current velocity vertical angle [rad] Gauss-Markov process model:"
    << std::endl;
  this->currentVertAngleModel.Print();

  // Initialize the time update
#if GAZEBO_MAJOR_VERSION >= 8
  this->lastUpdate = this->world->SimTime();
#else
  this->lastUpdate = this->world->GetSimTime();
#endif
  this->currentVelModel.lastUpdate = this->lastUpdate.Double();
  this->currentHorzAngleModel.lastUpdate = this->lastUpdate.Double();
  this->currentVertAngleModel.lastUpdate = this->lastUpdate.Double();

  // ----------------------------------------------------------------- //
  // -------------------- Transient ocean current -------------------- //
  // ----------------------------------------------------------------- //
  // Retrieve the transient ocean current configuration, if existent
  GZ_ASSERT(this->sdf->HasElement("transient_current"),
    "Transient current configuration not available");
  sdf::ElementPtr transientCurrentParams = this->sdf->GetElement(
    "transient_current");

  if (transientCurrentParams->HasElement("topic_stratified_database"))
    this->stratifiedCurrentVelocityTopic =
      transientCurrentParams->Get<std::string>("topic_stratified_database");
  else
    this->stratifiedCurrentVelocityTopic = "stratified_current_velocity";

  GZ_ASSERT(!this->stratifiedCurrentVelocityTopic.empty(),
    "Empty stratified ocean current velocity topic");

  // Read the depth dependent ocean current file path from the SDF file
  if (transientCurrentParams->HasElement("databasefilePath"))
    this->databaseFilePath =
      transientCurrentParams->Get<std::string>("databasefilePath");
  else
  {
    // Using boost:
    // boost::filesystem::path
    //    concatPath(boost::filesystem::initial_path().parent_path());
    // concatPath +=
    //    "/uuv_ws/src/dave/uuv_dave/worlds/transientOceanCurrentDatabase.csv";
    // this->databaseFilePath = concatPath.generic_string();
    //
    // Use ros package path:
    this->databaseFilePath = this->db_path +
      "/worlds/transientOceanCurrentDatabase.csv";
  }
  GZ_ASSERT(!this->databaseFilePath.empty(),
    "Empty stratified ocean current database file path");

  gzmsg << this->databaseFilePath << std::endl;

  // Read database
  std::ifstream csvFile; std::string line;
  csvFile.open(this->databaseFilePath);
  if (!csvFile)
  {
    this->databaseFilePath = this->db_path +
      "/worlds/" + this->databaseFilePath;
    csvFile.open(this->databaseFilePath);
  }
  GZ_ASSERT(csvFile, "Stratified Ocean database file does not exist");

  gzmsg << "Statified Ocean Current Database loaded : "
        << this->databaseFilePath << std::endl;

  // skip the 3 lines
  getline(csvFile, line); getline(csvFile, line); getline(csvFile, line);
  while (getline(csvFile, line))
  {
      if (line.empty())  // skip empty lines:
      {
          continue;
      }
      std::istringstream iss(line);
      std::string lineStream;
      std::string::size_type sz;
      std::vector <long double> row;
      while (getline(iss, lineStream, ','))
      {
          row.push_back(stold(lineStream, &sz));  // convert to double
      }
      ignition::math::Vector3d read;
      read.X() = row[0]; read.Y() = row[1]; read.Z() = row[2];
      this->database.push_back(read);
  }
  csvFile.close();

  // ----------------------------------------------------------------- //
  // --------------------    Tidal Oscillation    -------------------- //
  // ----------------------------------------------------------------- //
  if (this->sdf->HasElement("tidal_oscillation"))
  {
    this->tideFlag = true;
    this->tidalHarmonicFlag = false;

    sdf::ElementPtr tidalOscillationParams
      = this->sdf->GetElement("tidal_oscillation");
    sdf::ElementPtr tidalHarmonicParams;

    // Read the tidal oscillation parameter from the SDF file
    if (tidalOscillationParams->HasElement("databasefilePath"))
    {
      this->tidalFilePath =
        tidalOscillationParams->Get<std::string>("databasefilePath");
      gzmsg << "Tidal current database configuration found" << std::endl;
    }
    else
    {
      if (tidalOscillationParams->HasElement("harmonic_constituents"))
      {
        tidalHarmonicParams =
          tidalOscillationParams->GetElement("harmonic_constituents");
        gzmsg << "Tidal harmonic constituents "
              << "configuration found" << std::endl;
        tidalHarmonicFlag = true;
      }
      else
        this->tidalFilePath = ros::package::getPath("uuv_dave") +
          "/worlds/ACT1951_predictionMaxSlack_2021-02-24.csv";
    }

    // Read the tidal oscillation direction from the SDF file
    GZ_ASSERT(tidalOscillationParams->HasElement("mean_direction"),
      "Tidal mean direction not defined");
    if (tidalOscillationParams->HasElement("mean_direction"))
    {
      sdf::ElementPtr elem =
        tidalOscillationParams->GetElement("mean_direction");
      GZ_ASSERT(elem->HasElement("ebb"),
        "Tidal mean ebb direction not defined");
      this->ebbDirection = elem->Get<double>("ebb");
      this->floodDirection = elem->Get<double>("flood");
      GZ_ASSERT(elem->HasElement("flood"),
        "Tidal mean flood direction not defined");
    }

    // Read the world start time (GMT) from the SDF file
    GZ_ASSERT(tidalOscillationParams->HasElement("world_start_time_GMT"),
      "World start time (GMT) not defined");
    if (tidalOscillationParams->HasElement("world_start_time_GMT"))
    {
      sdf::ElementPtr elem =
        tidalOscillationParams->GetElement("world_start_time_GMT");
      GZ_ASSERT(elem->HasElement("day"),
        "World start time (day) not defined");
      this->world_start_time_day = elem->Get<double>("day");
      GZ_ASSERT(elem->HasElement("month"),
        "World start time (month) not defined");
      this->world_start_time_month = elem->Get<double>("month");
      GZ_ASSERT(elem->HasElement("year"),
        "World start time (year) not defined");
      this->world_start_time_year = elem->Get<double>("year");
      GZ_ASSERT(elem->HasElement("hour"),
        "World start time (hour) not defined");
      this->world_start_time_hour = elem->Get<double>("hour");
      if (elem->HasElement("minute"))
        this->world_start_time_minute = elem->Get<double>("minute");
      else
        this->world_start_time_minute = 0;
    }

    if (tidalHarmonicFlag)
    {
      // Read harmonic constituents
      GZ_ASSERT(tidalHarmonicParams->HasElement("M2"),
        "Harcomnic constituents M2 not found");
      sdf::ElementPtr M2Params = tidalHarmonicParams->GetElement("M2");
      this->M2_amp = M2Params->Get<double>("amp");
      this->M2_phase = M2Params->Get<double>("phase");
      this->M2_speed = M2Params->Get<double>("speed");
      GZ_ASSERT(tidalHarmonicParams->HasElement("S2"),
        "Harcomnic constituents S2 not found");
      sdf::ElementPtr S2Params = tidalHarmonicParams->GetElement("S2");
      this->S2_amp = S2Params->Get<double>("amp");
      this->S2_phase = S2Params->Get<double>("phase");
      this->S2_speed = S2Params->Get<double>("speed");
      GZ_ASSERT(tidalHarmonicParams->HasElement("N2"),
        "Harcomnic constituents N2 not found");
      sdf::ElementPtr N2Params = tidalHarmonicParams->GetElement("N2");
      this->N2_amp = N2Params->Get<double>("amp");
      this->N2_phase = N2Params->Get<double>("phase");
      this->N2_speed = N2Params->Get<double>("speed");
      gzmsg << "Tidal harmonic constituents loaded : " << std::endl;
      gzmsg << "M2 amp: " << this->M2_amp << " phase: " << this->M2_phase
            << " speed: " << this->M2_speed << std::endl;
      gzmsg << "S2 amp: " << this->S2_amp << " phase: " << this->S2_phase
            << " speed: " << this->S2_speed << std::endl;
      gzmsg << "N2 amp: " << this->N2_amp << " phase: " << this->N2_phase
            << " speed: " << this->N2_speed << std::endl;
    }
    else
    {
      // Read database
      csvFile.open(this->tidalFilePath);
      if (!csvFile)
      {
        this->tidalFilePath = ros::package::getPath("uuv_dave") +
          "/worlds/" + this->tidalFilePath;
        csvFile.open(this->tidalFilePath);
      }
      GZ_ASSERT(csvFile, "Tidal Oscillation database file does not exist");

      gzmsg << "Tidal Oscillation  Database loaded : "
            << this->tidalFilePath << std::endl;

      // skip the first line
      getline(csvFile, line);
      while (getline(csvFile, line))
      {
          if (line.empty())  // skip empty lines:
          {
              continue;
          }
          std::istringstream iss(line);
          std::string lineStream;
          std::string::size_type sz;
          std::vector<std::string> row;
          std::array<int, 5> tmpDateArray;
          while (getline(iss, lineStream, ','))
          {
            row.push_back(lineStream);
          }
          if (strcmp(row[1].c_str(), " slack"))  // skip 'slack' category
          {
            tmpDateArray[0] = std::stoi(row[0].substr(0, 4));
            tmpDateArray[1] = std::stoi(row[0].substr(5, 7));
            tmpDateArray[2] = std::stoi(row[0].substr(8, 10));
            tmpDateArray[3] = std::stoi(row[0].substr(11, 13));
            tmpDateArray[4] = std::stoi(row[0].substr(14, 16));
            this->dateGMT.push_back(tmpDateArray);

            this->speedcmsec.push_back(stold(row[2], &sz));
          }
      }
      csvFile.close();

      // Eliminate data with same consecutive type
      std::vector<int> duplicated;
      for (int i = 0; i  <this->dateGMT.size(); i++)
      {
        // delete latter if same sign
        if (((this->speedcmsec[i] > 0) - (this->speedcmsec[i] < 0))
            == ((this->speedcmsec[i+1] > 0) - (this->speedcmsec[i+1] < 0)))
        {
          duplicated.push_back(i+1);
        }
      }
      int eraseCount = 0;
      for (int i = 0; i < duplicated.size(); i++)
      {
        this->dateGMT.erase(
          this->dateGMT.begin()+duplicated[i]-eraseCount);
        this->speedcmsec.erase(
          this->speedcmsec.begin()+duplicated[i]-eraseCount);
        eraseCount++;
      }
    }
  }  // end of tidal oscillation configuration

  // Advertise the current velocity topic
  this->publishers[this->currentVelocityTopic] =
    this->node->Advertise<msgs::Vector3d>(
    this->ns + "/" + this->currentVelocityTopic);

  gzmsg << "Current velocity topic name: " <<
    this->ns + "/" + this->currentVelocityTopic << std::endl;

  // Connect the update event
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
    boost::bind(&UnderwaterCurrentPlugin::Update,
    this, _1));

  gzmsg << "Underwater current plugin loaded!" << std::endl
    << "\tWARNING: Current velocity calculated in the ENU frame"
    << std::endl;
}

/////////////////////////////////////////////////
void UnderwaterCurrentPlugin::Init()
{
  // Doing nothing for now
}

/////////////////////////////////////////////////
void UnderwaterCurrentPlugin::Update(const common::UpdateInfo & /** _info */)
{
#if GAZEBO_MAJOR_VERSION >= 8
  common::Time time = this->world->SimTime();
#else
  common::Time time = this->world->GetSimTime();
#endif

  // Calculate the flow velocity and the direction using the Gauss-Markov
  // model

  // Update current velocity
  double currentVelMag = this->currentVelModel.Update(time.Double());

  // Update current horizontal direction around z axis of flow frame
  double horzAngle = this->currentHorzAngleModel.Update(time.Double());

  // Update current horizontal direction around z axis of flow frame
  double vertAngle = this->currentVertAngleModel.Update(time.Double());

  // Generating the current velocity vector as in the NED frame
  this->currentVelocity = ignition::math::Vector3d(
      currentVelMag * cos(horzAngle) * cos(vertAngle),
      currentVelMag * sin(horzAngle) * cos(vertAngle),
      currentVelMag * sin(vertAngle));

  // Update time stamp
  this->lastUpdate = time;
  this->PublishCurrentVelocity();
}

/////////////////////////////////////////////////
void UnderwaterCurrentPlugin::PublishCurrentVelocity()
{
  msgs::Vector3d currentVel;
  msgs::Set(&currentVel, ignition::math::Vector3d(this->currentVelocity.X(),
                                                  this->currentVelocity.Y(),
                                                  this->currentVelocity.Z()));
  this->publishers[this->currentVelocityTopic]->Publish(currentVel);
}