#include <Eigen/Core>
#include <Eigen/StdVector>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <aslam/calibration/focallength-initializers.h>
#include <aslam/calibration/target-aprilgrid.h>
#include <aslam/calibration/target-observation.h>
#include <aslam/cameras/camera.h>
#include <aslam/cameras/camera-factory.h>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/distortion.h>
#include <aslam/common/entrypoint.h>
#include <aslam/common/pose-types.h>

const double kDegToRad = M_PI / 180.0;

///////////////////////////////////////////////
// Types to test
///////////////////////////////////////////////
template<typename Camera, typename Distortion>
struct CameraDistortion {
  typedef Camera CameraType;
  typedef Distortion DistortionType;
};

using testing::Types;
typedef Types<
    //CameraDistortion<aslam::PinholeCamera,aslam::FisheyeDistortion>,
    CameraDistortion<aslam::PinholeCamera, aslam::EquidistantDistortion>,
    CameraDistortion<aslam::PinholeCamera, aslam::RadTanDistortion>,
    CameraDistortion<aslam::PinholeCamera,aslam::NullDistortion>
    //CameraDistortion<aslam::UnifiedProjectionCamera,aslam::FisheyeDistortion>,
    //CameraDistortion<aslam::UnifiedProjectionCamera,aslam::EquidistantDistortion>,
    //CameraDistortion<aslam::UnifiedProjectionCamera,aslam::RadTanDistortion>,
    //CameraDistortion<aslam::UnifiedProjectionCamera,aslam::NullDistortion>
    >
    Implementations;

///////////////////////////////////////////////
// Test fixture
///////////////////////////////////////////////
template <typename CameraDistortion>
class TestCameras : public testing::Test {
 public:
  typedef typename CameraDistortion::CameraType CameraType;
  typedef typename CameraDistortion::DistortionType DistortionType;
 protected:
  TestCameras() : camera_(CameraType::template createTestCamera<DistortionType>()) {
    // Make sure the focal length are the same in both directions, as the
    // initializers assume that.
    CHECK(camera_);
    Eigen::VectorXd intrinsics = camera_->getParameters();
    intrinsics[aslam::PinholeCamera::kFu] =
        intrinsics[aslam::PinholeCamera::kFv];
    camera_->setParameters(intrinsics);
  };
  virtual ~TestCameras() {};

  bool runInitialization(
      const std::vector<aslam::calibration::TargetObservation::Ptr>& observations,
      Eigen::VectorXd* intrinsics_vector) const {
    CHECK_NOTNULL(intrinsics_vector);

    return aslam::calibration::FocalLengthInitializer<CameraType, DistortionType>::initialize(
        observations, intrinsics_vector);
  }

  typename CameraType::Ptr camera_;
};

TYPED_TEST_CASE(TestCameras, Implementations);

aslam::calibration::TargetObservation::Ptr simulateTargetObservation(
    const aslam::calibration::TargetAprilGrid::Ptr& target,
    const aslam::Camera& camera, const aslam::Transformation& T_C_T) {
  CHECK(target);

  // Project the target points into the camera frame and then onto the image plane/
  const Eigen::Matrix3Xd& points_target_frame = target->points();
  const Eigen::Matrix3Xd points_camera_frame = T_C_T.transformVectorized(points_target_frame);

  Eigen::Matrix2Xd image_points;
  std::vector<aslam::ProjectionResult> results;
  camera.project3Vectorized(points_camera_frame, &image_points, &results);

  // Remove corners that were projected outside the image boundary.
  Eigen::Matrix2Xd visible_image_points(2, target->size());
  Eigen::VectorXi visible_corner_ids(target->size());
  size_t num_visible_corners = 0u;

  for (size_t corner_idx = 0u; corner_idx < results.size(); ++corner_idx) {
    if (!results[corner_idx].isKeypointVisible()) {
      continue;
    }

    visible_image_points.col(num_visible_corners) = image_points.col(corner_idx);
    visible_corner_ids(num_visible_corners) = corner_idx;
    ++num_visible_corners;
  }
  visible_image_points.conservativeResize(Eigen::NoChange, num_visible_corners);
  visible_corner_ids.conservativeResize(num_visible_corners);

  return aslam::calibration::TargetObservation::Ptr(
      new aslam::calibration::TargetObservation(target, camera.imageHeight(),
                                                camera.imageWidth(), visible_corner_ids,
                                                visible_image_points));
}

// Tests are temporally disabled due to possible malfunction. Further checks required.
TYPED_TEST(TestCameras, DISABLED_InitializeIntrinsics) {
  CHECK(this->camera_);

  // Create a target April grid.
  aslam::calibration::TargetAprilGrid::TargetConfiguration aprilgrid_config;
  aslam::calibration::TargetAprilGrid::Ptr aprilgrid(
      new aslam::calibration::TargetAprilGrid(aprilgrid_config));

  // Simulate poses and reproject the target.
  constexpr size_t kNumCamPoses = 50;
  constexpr double kMaxTranslationM = 1.0;
  constexpr double kMaxRotationDeg = 60.0;

  aslam::Transformation T_C_T_nominal;
  T_C_T_nominal.getPosition() = 1.5 * Eigen::Vector3d::UnitZ();

  std::vector<aslam::calibration::TargetObservation::Ptr> target_observations;
  while (target_observations.size() < kNumCamPoses) {
    // Add a random disturbance around the nominal point from which the target is
    // completely visible.
    aslam::Transformation disturbance;
    disturbance.setRandom(kMaxTranslationM, kMaxRotationDeg * kDegToRad);
    aslam::Transformation T_C_T = T_C_T_nominal * disturbance;

    aslam::calibration::TargetObservation::Ptr obs =
        simulateTargetObservation(aprilgrid, *this->camera_, T_C_T);

    // Skip incomplete simulated targets.
    if (!obs->allCornersObservered()) {
      continue;
    }
    target_observations.push_back(obs);
  }

  // Run the initialization over all targets.
  Eigen::VectorXd intrinsics_guess(this->camera_->getParameterSize());
  bool intrinsics_success = this->runInitialization(target_observations, &intrinsics_guess);
  EXPECT_TRUE(intrinsics_success) << "Intrinsics initialization failed.";

  const double kTolerance = 1.0;
  EXPECT_TRUE(EIGEN_MATRIX_NEAR(intrinsics_guess, this->camera_->getParameters(), kTolerance));
}

ASLAM_UNITTEST_ENTRYPOINT


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

#include <boost/simd/ieee/functions/frexp.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/bitwise_notand.hpp>
#include <boost/simd/include/functions/simd/logical_notand.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::frexp_
                                      , tag::cpu_, (A0)(A1)(A2)(X)
                                      , ( boost::mpl::and_
                                          < boost::is_same<A0,A1>
                                          , boost::is_same
                                            < typename dispatch::meta
                                                        ::as_integer<A0>::type
                                            , A2
                                            >
                                          >
                                        )
                                      , ((simd_< floating_<A0>, X>))
                                        ((simd_< floating_<A1>, X>))
                                        ((simd_< integer_<A2>, X>))
                                      )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A1 & r0,A2 & r1) const
    {
      typedef typename meta::as_logical<A0>::type bA0;
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      typedef typename meta::as_logical<int_type>::type                 bint_type;
      typedef typename meta::scalar_of<int_type>::type                  sint_type;
      typedef typename meta::scalar_of<A0>::type                           s_type;

      const sint_type me = Maxexponent<s_type>()-1;
      const sint_type nmb= Nbmantissabits<s_type>();
      const sint_type n1 = ((2*me+3)<<nmb);
      const sint_type n2 = me<<nmb;
      const int_type vme = splat<int_type>(me);

      A0 ci   = simd::bitwise_cast<A0>(splat<int_type>(n1));

          r1  = simd::bitwise_cast<int_type>(b_and(ci, a0));  // extract exponent
      A0  x   = b_notand(ci, a0);                             // clear exponent in a0
          r1  = sub(shri(r1,nmb), vme);                       // compute exponent
          r0  = b_or(x,splat<int_type>(n2));                  // insert exponent+1 in x

      bA0       test0 = is_nez(a0);
      bint_type test1 = gt(r1,vme);

      r1 = if_else_zero(logical_notand(test1, test0), r1);
      r0 = if_else_zero(test0, seladd(test1,r0,a0));
    }
  };
} } }

#endif


#include <sys/time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

#include "ros/ros.h"
#include "bosch_arm.h"
#include <kdl/frames.hpp>
#include "trajectory_controller3.h"
#include <bosch_arm_control/ArmState.h>
#include <bosch_arm_control/PointCmd.h>
#include <bosch_arm_control/Diagnostic.h>
#include "cc2.h"
#include <Eigen/Core>
#include<Eigen/Array>
#include <Eigen/LU>
#include <Eigen/QR>

using namespace std;
void TrajectoryController::singlePtCmdCallBack(const bosch_arm_control::PointCmd& msg)
{
  MoveAbsAction* a=new MoveAbsAction(msg.coordinates,this,msg.duration);
  act_que.push_back(a);
}

TrajectoryController::TrajectoryController(BoschArm *ptr)
{
  rob=ptr;
  diagnostic_pub =  n.advertise<bosch_arm_control::Diagnostic> ("/diagnostics",100);
  sigle_point_cmd_sub = n.subscribe("/joint_point_cmd",3, &TrajectoryController::singlePtCmdCallBack, this);
  joint_state_pub =  n.advertise<bosch_arm_control::ArmState> ("/joint_states",100);
  js.position.resize(4);
  js.velocity.resize(4);
  js.effort.resize(4);
  Kp[0]=10;
  Kp[1]=10;
  Kp[2]=20;
  Kp[3]=23;
//    for(int i=0;i<4;i++)
//      Kp[i]=Kp[i]*0.02;
  Kv[0]=0.20;
  Kv[1]=0.20;
  Kv[2]=0.24;
  Kv[3]=0.26;
//   for(int i=0;i<4;i++)
//     Kv[i]=0;
  joint_limit_low[0]=-1.7;
  joint_limit_high[0]=2.1;
  joint_limit_low[1]=-0.6;
  joint_limit_high[1]=0.55;
  joint_limit_low[2]=-2.5;
  joint_limit_high[2]=2.3;
  joint_limit_low[3]=-2.3;
  joint_limit_high[3]=2.4;

  tmax[0]=constants::t_max/constants::r1;
  tmax[1]=constants::t_max/constants::r2;
  tmax[2]=constants::t_max/constants::r2;
  tmax[3]=constants::t_max/constants::r2;

  vtime[0]=3000;
  vtime[1]=1500;
  vtime[2]=1500;
  vtime[3]=1500;

  vstep[0]=0.0001;
  vstep[1]=0.0002;
  vstep[2]=0.001;
  vstep[3]=0.001;

  dovstep=false;
  tvstep=0;
}

//calculate the frames given the joint values.
void TrajectoryController::updateKinematics()
{
  frm_cur[0]=frm_home[0];
  frm0[0]=frm_home[0];
  for (int i=1;i<=4;i++)
  {
    frm_cur[i].p=frm_home[i].p;
    frm_cur[i].M=frm_home[i].M*Rotation::RotZ(q[i-1]);
    frm0[i]=frm0[i-1]*frm_cur[i];
  }
  frm_cur[5]=frm_home[5];
  frm0[5]=frm0[4]*frm_cur[5];
  frmn[4]=frm_cur[5];
  Vector z(0,0,1);

  //jacobian[3](joint 4) = z4*p54
  for (int i=3;i>=0;i--)
  {
    frmn[i]=frm_cur[i+1]*frmn[i+1];
    jacobian[i]=z*frmn[i+1].p;
    jacobian[i]=frm0[i+1].M*jacobian[i];
  }
}

void TrajectoryController::start()
{
  rob->initialize();
  //L0,L3 are not available until rob is initialized.
  frm_home[0]=Frame::Identity();
  Rotation r(0,-1,0,
             0,0,-1,
             1,0,0);
  frm_home[1].M=r;
  frm_home[1].p=Vector(0,0,rob->L0);
  frm_home[2].M=r;
  frm_home[3].M=r;
  frm_home[4].M=r;
  frm_home[4].p=Vector(-rob->L5,0,rob->L3);
  frm_home[5].p=Vector(rob->L4,0,0);

  rob->motor2JointPosition(rob->q,q);

  for (int i=0;i<4;i++)
  {
    v[i]=0;
    dq[i]=0;
    f[i]=0;
    ql[i]=q[i];
    qd[i]=q[i];
    vd[i]=v[i];
  }
  for (int i=0;i<4;i++)
    gc[i]=Vector(constants::gravc[i*2],constants::gravc[i*2+1],0);
  //state=CALIBRATION;
  //state=LOADTRACE;
  state=SERVO;
  ctr_mode=GRAVITY;
  //ctr_mode=PDCONTROL;
  //ctr_mode=PDWITHGC;
  if (state==CALIBRATION)
  {
    genCalibTraj2();
    cur_act=act_que.front();
    cur_act->initialize();
    act_que.pop_front();
  }
  else if (state==LOADTRACE)
  {

    vector<double> des(4,0);
//     for(int i=0;i<4;i++)
//       des[i]=q[i];
//     des[1]=joint_limit_low[1];
//     des[3]=joint_limit_low[3]+0.1;
//     act_que.push_back(new MoveAbsAction(des,this));
//     des[1]=joint_limit_high[1];
    act_que.push_back(new MoveAbsAction(des,this));
    cur_act=act_que.front();
    cur_act->initialize();
    act_que.pop_front();

    //state=SERVO;

  }
}

void TrajectoryController::genCalibTraj2()
{
  vector<double> des(4,0);
//  int t=8000;
  //move the the zero position.
  act_que.push_back(new MoveAbsAction(des,this));
  des[0]=-1.5;
  des[1]=-0.5;
  des[2]=-2.2;
  des[3]=-1.7;
  cali1_act=new MoveAbsAction(des,this);
  calib1=new Calibration("calib1");
  cali1_act->calib=calib1;
  act_que.push_back(cali1_act);
  des[0]=0.7;
  des[1]=0;
  des[2]=0;
  des[3]=0;
  act_que.push_back(new MoveAbsAction(des,this));
  des[0]=-0.8;
  des[1]=-0.5;
  des[2]=-2.2;
  des[3]=-1.7;
  cali2_act=new MoveAbsAction(des,this);
  calib2=new Calibration("calib2");
  cali2_act->calib=calib2;
  act_que.push_back(cali2_act);

}

void TrajectoryController::calcGravityCompensation()
{
  for (int j=0;j<calib1->len_b;j++)
    if (calib1->count[j]>0&&calib2->count[j]>0)
    {
      calib1->b[j]-=calib2->b[j];
      calib1->count[j]=1;
    }

  for (int i=0;i<8;i++)
  {
    for (int j=0;j<calib1->len_b;j++)
    {
      if (calib1->count[j]>0)
      {
        calib1->A[i][j]-=calib2->A[i][j];
      }
    }
  }

  Eigen::MatrixXf AA(8,8);
  for (int i=0;i<8;i++)
  {
    for (int j=0;j<8;j++)
    {
      double s=0;
      for (int k=0;k<calib1->len_b;k++)
        if (calib1->count[k]>0)
          s+=calib1->A[i][k]*calib1->A[j][k];
      AA(i,j)=s/calib1->len_b;
    }
  }
  //cout<<AA;
  Eigen::VectorXf Ab(8);
  for (int i=0;i<8;i++)
  {
    double s=0;
    for (int j=0;j<calib1->len_b;j++)
      if (calib1->count[j]>0)
        s+=calib1->A[i][j]*calib1->b[j];
    Ab(i)=-s/calib1->len_b;
  }
  Eigen::MatrixXf M=AA.inverse()*Ab;
  //cout<<M<<endl;
  for (int i=0;i<4;i++)
  {
    gc[i]=Vector(M(i*2,0),M(i*2+1,0),0);
  }
  for (int i=0;i<4;i++)
  {
    for (int j=0;j<3;j++)
      cout<<gc[i][j]<<',';
    cout<<endl;
  }
}

// void TrajectoryController::vstep(int joint, double step)
// {
//   vector<double> des(4,0);
//   for (int i=0;i<4;i++)
//     des[i]=qd[i];
//   des[joint]+=step;
//   cur_act=new MoveAbsAction(des,this);
//   cur_act->initialize();
//
// }

void TrajectoryController::step(int joint, double step)
{

  for (int i=0;i<4;i++)
    qd[i]=q[i];
  qd[joint]+=step;

}

void TrajectoryController::update()
{
  rob->update();
  clock_gettime(CLOCK_REALTIME, &ts);
  //save the last joint position
  for (int i=0;i<4;i++)
    ql[i]=q[i];
  rob->motor2JointPosition(rob->q,q);
  rob->motor2JointVelocity(rob->v,v);

  if (state==CALIBRATION)
  {
    //update desired position
    if (cur_act->finished)
    {
      if (cur_act->calib==NULL)
        delete cur_act;

      if (act_que.size()==0)
      {
        calcGravityCompensation();
        state=SERVO;
        return;
      }
      cur_act=act_que.front();
      //cout<<cur_act->rel[3]<<endl;
      cur_act->initialize();
      act_que.pop_front();
    }
    cur_act->update();
    for (int i=0;i<4;i++)
    {
      qd[i]=cur_act->cmd.coordinates[i];
      vd[i]=cur_act->cmd.velocity[i];

    }
    //update torque command
    PDControl();

    logging();
  }
  else if (state==LOADTRACE)
  {
    if (cur_act->finished)
    {
      if (cur_act->calib==NULL)
        delete cur_act;

      if (act_que.size()==0)
      {
        //calcGravityCompensation();
        state=SERVO;
        return;
      }

      cur_act=act_que.front();
      //cout<<cur_act->rel[3]<<endl;
      cur_act->initialize();
      act_que.pop_front();

    }
    cur_act->update();
    for (int i=0;i<4;i++)
    {
      qd[i]=cur_act->cmd.coordinates[i];
      vd[i]=cur_act->cmd.velocity[i];

    }
    //update torque command
    //PDWithGC();
    PDControl();
    logging();
  }
  else if (state==SERVO)
  {
    if (cur_act!=NULL)
    {
      if (cur_act->finished)
      {
        if (cur_act->calib==NULL)
        {
          delete cur_act;
          cur_act=NULL;
        }

//         if (act_que.size()!=0)
//         {
//           cur_act=act_que.front();
//           cur_act->initialize();
//           act_que.pop_front();
//         }

      }
      else
      {
        cur_act->update();
        for (int i=0;i<4;i++)
        {
          qd[i]=cur_act->cmd.coordinates[i];
          vd[i]=cur_act->cmd.velocity[i];

        }
      }
    }
    if(dovstep)
    {
      if(tvstep==2*vtime[jid])
        tvstep=0;
      if(tvstep<vtime[jid]/2)
      {
        qd[jid]+=vstep[jid];
        vd[jid]=vstep[jid];
      }
      else if(tvstep<vtime[jid]/2+vtime[jid])
      {
        qd[jid]-=vstep[jid];
        vd[jid]=-vstep[jid];
      }
      else
      {
        qd[jid]+=vstep[jid];
        vd[jid]=vstep[jid];
      }
      tvstep++;
    }
    //PDControl();
    if (ctr_mode==PDWITHGC)
      PDWithGC();
    else if (ctr_mode==PDCONTROL)
      PDControl();
    else if (ctr_mode==GRAVITY)
      gravity_compensation();
    else if (ctr_mode==NOCONTROL)
    {
      for (int i=0;i<4;i++)
        rob->torque[i]=0;
    }
    //floating();
    //gravity_compensation();
    logging();
  }
}

void TrajectoryController::gravity_compensation()
{

  updateKinematics();
  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    f[i]=0;
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      f[i]-=dot(gc[j],tmp);
    }
  }

  for (int i=0;i<4;i++)
  {
    torque[i]=0;
    for (int j=0;j<4;j++)
    {
      torque[i]+=rob->m2j[4*j+i]*f[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=torque[i];
  //logging();

}

void TrajectoryController::PDWithGC()
{
  updateKinematics();
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    f[i]=Kp[i]*dq[i]-Kv[i]*(v[i]-vd[i]);
  }

  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      f[i]-=dot(gc[j],tmp);
    }
  }
  for (int i=0;i<4;i++)
  {
    torque[i]=0;
    for (int j=0;j<4;j++)
    {
      torque[i]+=rob->m2j[4*j+i]*f[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=torque[i];
  //logging();
}

void TrajectoryController::PDControl()
{
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    f[i]=Kp[i]*dq[i]-Kv[i]*(v[i]-vd[i]);
  }
  for (int i=0;i<4;i++)
  {
    torque[i]=0;
    for (int j=0;j<4;j++)
    {
      torque[i]+=rob->m2j[4*j+i]*f[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=torque[i];
  //logging();
}

void TrajectoryController::logging()
{
  std::string log;
  std::stringstream out;
  for (int i=0;i<4;i++)
    out<<q[i]<<",";
  for (int i=0;i<4;i++)
    out<<qd[i]<<",";
  for (int i=0;i<4;i++)
    out<<v[i]<<",";
  for (int i=0;i<4;i++)
    out<<f[i]<<",";
  for (int i=0;i<4;i++)
    out<<(q[i]-qd[i])<<",";
  for (int i=0;i<4;i++)
    out<<rob->q[i]<<',';
  for (int i=0;i<4;i++)
    out<<rob->v[i]<<',';
  for (int i=0;i<4;i++)
    out<<rob->torque[i]<<',';
  out << ts.tv_sec << ','<< ts.tv_nsec;
  log = out.str();
  diag.data=log;
  diag.header.stamp.sec=ts.tv_sec;
  diag.header.stamp.nsec=ts.tv_nsec;
  diagnostic_pub.publish(diag);
  //vector<double> cur_pos_act(q,q+4);
  js.header.stamp.sec=ts.tv_sec;
  js.header.stamp.nsec=ts.tv_nsec;
  //js.position=cur_pos_act;

  for (int i=0;i<4;i++)
  {
    js.position[i]=q[i];
    js.velocity[i]=v[i];
    js.effort[i]=f[i];
  }
  joint_state_pub.publish(js);
}
void TrajectoryController::stop()
{
  rob->close();
}


#ifdef TORSTEN_MPI

#include <stan/math.hpp>
#include <stan/math/rev/core.hpp>
#include <test/unit/math/rev/mat/fun/util.hpp>
#include <stan/math/torsten/dsolve/pmx_cvodes_fwd_system.hpp>
#include <stan/math/torsten/dsolve/pmx_cvodes_integrator.hpp>
#include <stan/math/torsten/dsolve/pmx_integrate_ode_adams.hpp>
#include <test/unit/math/torsten/pmx_ode_test_fixture.hpp>
#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <stan/math/rev/mat/functor/integrate_ode_adams.hpp>
#include <nvector/nvector_serial.h>
#include <boost/mpi.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <memory>
#include <iostream>
#include <fstream>
#include <limits>
#include <vector>
#include <chrono>
#include <ctime>
#include <random>

TEST_F(TorstenOdeTest_neutropenia, fwd_sensitivity_theta_adams_mpi) {
  using torsten::dsolve::PMXCvodesFwdSystem;
  using torsten::pmx_integrate_ode_group_adams;
  using torsten::pmx_integrate_ode_adams;
  using stan::math::var;
  using std::vector;

  torsten::mpi::Envionment::init();

  // size of population
  const int np = 100;
  std::vector<double> ts0 {ts};

  vector<var> theta_var = stan::math::to_var(theta);

  vector<int> len(np, ts0.size());
  vector<double> ts_m;
  ts_m.reserve(np * ts0.size());
  for (int i = 0; i < np; ++i) ts_m.insert(ts_m.end(), ts0.begin(), ts0.end());

  vector<vector<double> > y0_m (np, y0);
  vector<vector<var> > theta_var_m (np, theta_var);
  vector<vector<double> > x_r_m (np, x_r);
  vector<vector<int> > x_i_m (np, x_i);

  std::default_random_engine gen;
  std::normal_distribution<double> dis1(3.0,1.0);
  std::normal_distribution<double> dis2(10.0,3.0);
  for (int i = 0; i < np; ++i) {
    theta_var_m[i][0] += dis1(gen);
    theta_var_m[i][1] += dis1(gen);
    theta_var_m[i][2] += dis1(gen);
    theta_var_m[i][3] += dis2(gen);
    theta_var_m[i][5] += dis2(gen);
  }

  Eigen::Matrix<var, -1, -1> y_m = pmx_integrate_ode_group_adams(f, y0_m, t0, len, ts_m, theta_var_m , x_r_m, x_i_m);
}

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_BINARY_FIND_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_BINARY_FIND_HPP

#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/find_if.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/detail/parameter_cache.hpp>

namespace boost {
namespace compute {
namespace detail{

///
/// \brief Binary find kernel class
///
/// Subclass of meta_kernel to perform single step in binary find.
///
class binary_find_kernel : public meta_kernel
{
public:
    binary_find_kernel(size_t threads) : meta_kernel("binary_find")
    {
        m_threads = threads;
    }

    template<class InputIterator, class UnaryPredicate>
    void set_range(InputIterator first,
                   InputIterator last,
                   UnaryPredicate predicate)
    {
        typedef typename std::iterator_traits<InputIterator>::value_type value_type;
        int block = (iterator_range_size(first, last)-1)/(m_threads-1);

        m_index_arg = add_arg<uint_ *>(memory_object::global_memory, "index");

        atomic_min<uint_> atomic_min_uint;

        *this <<
            "uint i = get_global_id(0) * " << block << ";\n" <<
            decl<value_type>("value") << "=" << first[var<uint_>("i")] << ";\n" <<
            "if(" << predicate(var<value_type>("value")) << ") {\n" <<
                atomic_min_uint(var<uint_ *>("index"), var<uint_>("i")) << ";\n" <<
            "}\n";

    }

    event exec(command_queue &queue, scalar<uint_> index)
    {
        set_arg(m_index_arg, index.get_buffer());

        return exec_1d(queue, 0, m_threads);
    }

private:
    size_t m_threads;
    size_t m_index_arg;
};

///
/// \brief Binary find algorithm
///
/// Finds the end of true values in the partitioned range [first, last).
/// \return Iterator pointing to end of true values
///
/// \param first Iterator pointing to start of range
/// \param last Iterator pointing to end of range
/// \param predicate Predicate according to which the range is partitioned
/// \param queue Queue on which to execute
///
template<class InputIterator, class UnaryPredicate>
inline InputIterator binary_find(InputIterator first,
                                 InputIterator last,
                                 UnaryPredicate predicate,
                                 command_queue &queue = system::default_queue())
{
    const device &device = queue.get_device();

    boost::shared_ptr<parameter_cache> parameters =
        detail::parameter_cache::get_global_cache(device);

    const std::string cache_key = "__boost_binary_find";

    size_t find_if_limit = 128;
    size_t threads = parameters->get(cache_key, "tpb", 128);
    size_t count = iterator_range_size(first, last);

    while(count > find_if_limit) {

        scalar<uint_> index(queue.get_context());
        index.write(static_cast<uint_>(count), queue);

        binary_find_kernel kernel(threads);
        kernel.set_range(first, last, predicate);
        kernel.exec(queue, index);

        size_t i = index.read(queue);

        if(i == count) {
            first = last - count%threads;
            break;
        } else {
            last = first + i;
            first = last - count/threads;
        }

        count = iterator_range_size(first, last);
    }

    return find_if(first, last, predicate, queue);
}

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_BINARY_FIND_HPP


// Copyright (C) 2014-2015 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/product_type.hpp>
#if __cplusplus >= 201402L

#include <experimental/product_type.hpp>
#include <experimental/tuple.hpp>
#include <sstream>
#include <experimental/fundamental/v3/product_type/stream_insert.hpp>

#include <boost/detail/lightweight_test.hpp>

struct Point3D {
  int x;
  int y;
  int z;
};

struct Point2D {
  int x;
  int y;
};

namespace std {
namespace experimental {
inline namespace fundamental_v3 {
namespace product_type {

  template <>
  struct traits<Point2D>
  {
    using size = integral_constant<size_t, 2>;

    template <size_t I>
    using element = meta::id<int>;

    static constexpr int& get_detail(integral_constant<size_t, 0>, Point2D & pt) noexcept
    {      return pt.x;    }
    static constexpr int const& get_detail(integral_constant<size_t, 0>, Point2D const& pt) noexcept
    {      return pt.x;    }
    static constexpr int&& get_detail(integral_constant<size_t, 0>, Point2D && pt) noexcept
    {      return move(pt).x;    }
//    static constexpr int && get_detail(integral_constant<size_t, 0>, Point2D const&& pt) noexcept
//    {      return pt.x;    }

    static constexpr int& get_detail(integral_constant<size_t, 1>, Point2D & pt) noexcept
    {      return pt.y;    }
    static constexpr int const& get_detail(integral_constant<size_t, 1>, Point2D const& pt) noexcept
    {      return pt.y;    }
    static constexpr int&& get_detail(integral_constant<size_t, 1>, Point2D && pt) noexcept
    {      return move(pt).y;    }
//    static constexpr int && get_detail(integral_constant<size_t, 1>, Point2D const&& pt) noexcept
//    {      return pt.y;    }

    template <size_t I>
      static constexpr int& get(Point2D & pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, pt);
      }
    template <size_t I>
      static constexpr int const& get(Point2D const& pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, pt);
      }
    template <size_t I>
      static constexpr auto get(Point2D && pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, move(pt));
      }
  };

  template <>
  struct traits<Point3D>
  {
    using size = integral_constant<size_t, 3>;

    template <size_t I>
    using element = meta::id<int>;

    static constexpr int& get_detail(integral_constant<size_t, 0>, Point3D & pt) noexcept
    {      return pt.x;    }
    static constexpr int const& get_detail(integral_constant<size_t, 0>, Point3D const& pt) noexcept
    {      return pt.x;    }
    static constexpr int&& get_detail(integral_constant<size_t, 0>, Point3D && pt) noexcept
    {      return move(pt).x;    }
//    static constexpr int && get_detail(integral_constant<size_t, 0>, Point3D const&& pt) noexcept
//    {      return pt.x;    }

    static constexpr int& get_detail(integral_constant<size_t, 1>, Point3D & pt) noexcept
    {      return pt.y;    }
    static constexpr int const& get_detail(integral_constant<size_t, 1>, Point3D const& pt) noexcept
    {      return pt.y;    }
    static constexpr int&& get_detail(integral_constant<size_t, 1>, Point3D && pt) noexcept
    {      return move(pt).y;    }
//    static constexpr int && get_detail(integral_constant<size_t, 1>, Point3D const&& pt) noexcept
//    {      return pt.y;    }

    static constexpr int& get_detail(integral_constant<size_t, 2>, Point3D & pt) noexcept
    {      return pt.z;    }
    static constexpr int const& get_detail(integral_constant<size_t, 2>, Point3D const& pt) noexcept
    {      return pt.z;    }
    static constexpr int&& get_detail(integral_constant<size_t, 2>, Point3D && pt) noexcept
    {      return move(pt).z;    }
//    static constexpr int && get_detail(integral_constant<size_t, 2>, Point3D const&& pt) noexcept
//    {      return pt.z;    }

    template <size_t I>
      static constexpr int& get(Point3D & pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, pt);
      }
    template <size_t I>
      static constexpr int const& get(Point3D const& pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, pt);
      }
    template <size_t I>
      static constexpr auto get(Point3D && pt) noexcept
      {
        return get_detail(integral_constant<size_t, I>{}, move(pt));
      }
  };
}
}}}

int f(int,int,int) { return 0; }

struct X {
  int i;
  int j;
  int k;
  X(int i, int j, int k) : i(i), j(j), k(k){}
};

#if 0
// fixme: We can not have rvalue to c-arrays
int[2] make_carray(int i, int j) {
  return {i,j};
}
#endif

int main()
{
  namespace stde = std::experimental;
  {
      Point3D arr = {0,1,2};
      static_assert(! stde::has_tuple_like_access<int>::value, "Hrr");
      static_assert(! stde::has_tuple_like_access<decltype(arr)>::value, "Hrr");

      static_assert(3 == stde::product_type::size<decltype(arr)>::value, "Hrr");

      static_assert(std::is_same<int, stde::product_type::element_t<0,decltype(arr)>>::value, "Hrr");
      static_assert(stde::is_product_type_v<decltype(arr)>, "Hrr");

      BOOST_TEST(3 == stde::product_type::size<decltype(arr)>::value);
      BOOST_TEST(0 == stde::product_type::get<0>(arr));
      BOOST_TEST(1 == stde::product_type::get<1>(arr));
      BOOST_TEST(2 == stde::product_type::get<2>(arr));
      //auto x = stde::product_type::get<3>(arr); // COMPILE FAIL AS REQUIRED
      static_assert(std::is_same<int&, decltype(stde::product_type::get<0>(arr))>::value, "Hrr");
      //check<decltype(stde::product_type::get<0>(arr))> xx
  }
  {
      constexpr Point3D arr = {0,1,2};
      static_assert(0 == stde::product_type::get<0>(arr), "Hrr");
      static_assert(std::is_same<const int, stde::product_type::element_t<0,decltype(arr)>>::value, "Hrr");
  }
#if 0
// We can not have rvalue to c-arrays
  {
    static_assert(std::is_same<int&&, decltype(stde::product_type::get<0>(make_carray(0,1)))>::value, "Hrr");
    BOOST_TEST(0 == stde::product_type::get<0>(make_carray(0,1)));
  }
#endif
  {
      const Point3D arr = {0,1,2};
      static_assert(3 == stde::product_type::size<decltype(arr)>::value, "Hrr");

      BOOST_TEST(3 == stde::product_type::size<decltype(arr)>::value);
      BOOST_TEST(0 == stde::product_type::get<0>(arr));
      BOOST_TEST(1 == stde::product_type::get<1>(arr));
      BOOST_TEST(2 == stde::product_type::get<2>(arr));
  }
  {
      Point3D arr = {0,1,2};
      BOOST_TEST(0 == stde::product_type::apply(f, arr));
  }
  {
      Point3D arr = {0,1,2};
      BOOST_TEST(std::make_tuple(0,1,2) == stde::product_type::to_tuple(arr));
  }
  {
    Point2D p = {0,1};
    Point2D q = {2,3};
    BOOST_TEST(std::make_tuple(0,1,2,3) == stde::product_type::cat(p, q));
  }
#if __cplusplus >= 201402L and defined JASEL_SUPPORT_SWAPPABLE
  {
    Point2D p = {0,1};
    Point2D q = {2,3};
    stde::product_type::swap(p,q);
    BOOST_TEST(0 == q.x);
    BOOST_TEST(1 == q.y);
    BOOST_TEST(2 == p.x);
    BOOST_TEST(3 == p.y);
  }
  {
    Point2D p = {0,1};
    Point2D q = {2,3};
    stde::swappable::swap(p,q);
    BOOST_TEST(0 == q.x);
    BOOST_TEST(1 == q.y);
    BOOST_TEST(2 == p.x);
    BOOST_TEST(3 == p.y);
  }
#endif
  {
    Point3D arr = {0,1,2};
    auto x = stde::product_type::make_from<X>(arr);
    BOOST_TEST(0 == x.i);
    BOOST_TEST(1 == x.j);
    BOOST_TEST(2 == x.k);
  }
  {
    Point2D p = {0,1};
    Point2D q = {2,3};
    static_assert(std::is_same<int&, decltype(stde::product_type::get<0>(p))>::value, "Hrr");
    static_assert(std::is_same<int&, decltype(stde::product_type::get<1>(p))>::value, "Hrr");

    stde::product_type::copy(q,p);
    BOOST_TEST(2 == q.x);
    BOOST_TEST(3 == q.y);
    BOOST_TEST(2 == p.x);
    BOOST_TEST(3 == p.y);
  }
  // COMPILE FAILS AS EXPECTED
#if 0
  {
    Point2D p = {0,1};
    std::string q[] = {"2","3"};
    stde::product_type::copy(q,p);
  }
#endif
  {
    Point2D p = {0,1};
    const Point2D q = {2,3};
    stde::product_type::copy(q,p);
  }
  {
    Point2D p = {0,1};
    stde::product_type::for_each(p,[](auto& v) {std::cout << v << "\n";});
  }
  {
    auto to_string = [](auto x) {
        std::ostringstream ss;
        ss << x;
        return ss.str();
    };

    Point2D p = {0,1};
    auto res = stde::product_type::p_transform<stde::tuple_tc>(p, to_string);
    static_assert(
        std::is_same<std::tuple<std::string, std::string>, decltype(res)>::value,
        "");

    BOOST_TEST(stde::product_type::p_transform<stde::tuple_tc>(p, to_string)
      ==
        std::make_tuple("0", "1")
    );
  }
  {
    auto to_string = [](auto x) {
        std::ostringstream ss;
        ss << x;
        return ss.str();
    };
    auto f = [=](std::string s, auto element) {
        return "f(" + s + ", " + to_string(element) + ")";
    };

    Point3D p = {2, 3, 4};

    // with an initial state
    BOOST_TEST(
        stde::product_type::fold_left(p, "1", f)
            ==
        "f(f(f(1, 2), 3), 4)"
    );
  }
  {
    Point3D p = {2, 3, 4};
    using namespace stde::product_type::operators;
    std::cout<< p << "\n";
  }
  return ::boost::report_errors();
}

#else
int main() { return 0; }
#endif


#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
  Array<double,2> A(0,0);
  Array<double,2> B(0,0);

  firstIndex i;
  secondIndex j;

  B = A(j,i);

  return 0;
}


#include "fieldmap.hh"

#include "../Config/config.hh"
#include "../geometry/LineSegment/linesegment.hh"
#include "../LineJunctionFinder/linejunctionfinder.hh"

#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace bold;
using namespace std;
using namespace Eigen;

vector<LineSegment3d> FieldMap::d_fieldLines;
vector<LineSegment3d> FieldMap::d_fieldLineEdges;
vector<LineJunction, aligned_allocator<LineJunction>> FieldMap::d_fieldLineJunctions;
vector<LineSegment3d> FieldMap::d_circleLines;
vector<Vector3d> FieldMap::d_goalPostPositions;
vector<Vector3d> FieldMap::d_ourGoalPostPositions;
vector<Vector3d> FieldMap::d_theirGoalPostPositions;

double FieldMap::d_fieldLengthX;
double FieldMap::d_fieldLengthY;
double FieldMap::d_outerMarginMinimum;
double FieldMap::d_circleRadius;
double FieldMap::d_maxDiagonalFieldDistance;
double FieldMap::d_goalY;
double FieldMap::d_goalAreaLengthX;
double FieldMap::d_goalAreaLengthY;
double FieldMap::d_goalPostDiameter;
double FieldMap::d_ballDiameter;

void FieldMap::initialise()
{
  d_fieldLengthX             = Config::getStaticValue<double>("world.field-size-x");
  d_fieldLengthY             = Config::getStaticValue<double>("world.field-size-y");
//double goalX               = Config::getStaticValue<double>("world.goal-size-x");
  d_goalY                    = Config::getStaticValue<double>("world.goal-size-y");
//double goalZ               = Config::getStaticValue<double>("world.goal-size-z");
  d_goalPostDiameter         = Config::getStaticValue<double>("world.goal-post-diameter");
  d_goalAreaLengthX          = Config::getStaticValue<double>("world.goal-area-size-x");
  d_goalAreaLengthY          = Config::getStaticValue<double>("world.goal-area-size-y");
  double penaltyMarkDistance = Config::getStaticValue<double>("world.penalty-mark-distance");
  double penaltyMarkLength   = Config::getStaticValue<double>("world.penalty-mark-length");
  double circleDiameter      = Config::getStaticValue<double>("world.circle-diameter");
  double lineWidth           = Config::getStaticValue<double>("world.line-width");
  d_outerMarginMinimum       = Config::getStaticValue<double>("world.outer-margin-minimum");
  d_ballDiameter             = Config::getStaticValue<double>("world.ball-diameter");

  d_maxDiagonalFieldDistance = Vector2d(
    d_fieldLengthX + 2*d_outerMarginMinimum,
    d_fieldLengthY + 2*d_outerMarginMinimum
  ).norm();

  double halfCrossLength = penaltyMarkLength/2;
  double penaltyMarkX = d_fieldLengthX/2 - penaltyMarkDistance;
  double penaltyMarkInnerX = penaltyMarkX - halfCrossLength;
  double penaltyMarkOuterX = penaltyMarkX + halfCrossLength;
  double halfFieldX = d_fieldLengthX/2;
  double halfFieldY = d_fieldLengthY/2;
  double halfGoalAreaY = d_goalAreaLengthY/2;
  double halfGoalY = d_goalY/2;
  d_circleRadius = circleDiameter/2;

  // CROSS MARKS
  // center cross mark
  d_fieldLines.emplace_back(Vector3d(-halfCrossLength, 0, 0), Vector3d(halfCrossLength, 0, 0));
//  d_fieldLines.emplace_back(Vector3d(0, -halfCrossLength, 0), Vector3d(0, halfCrossLength, 0)));
  // left penalty mark
  d_fieldLines.emplace_back(Vector3d(-penaltyMarkInnerX, 0, 0), Vector3d(-penaltyMarkOuterX, 0, 0));
  d_fieldLines.emplace_back(Vector3d(-penaltyMarkX, -halfCrossLength, 0), Vector3d(-penaltyMarkX, halfCrossLength, 0));
  // right penalty mark
  d_fieldLines.emplace_back(Vector3d(penaltyMarkInnerX, 0, 0), Vector3d(penaltyMarkOuterX, 0, 0));
  d_fieldLines.emplace_back(Vector3d(penaltyMarkX, -halfCrossLength, 0), Vector3d(penaltyMarkX, halfCrossLength, 0));

  // OUTER SQUARE
  // top
  d_fieldLines.emplace_back(Vector3d(-halfFieldX, halfFieldY, 0), Vector3d(halfFieldX, halfFieldY, 0));
  // bottom
  d_fieldLines.emplace_back(Vector3d(-halfFieldX, -halfFieldY, 0), Vector3d(halfFieldX, -halfFieldY, 0));
  // left
  d_fieldLines.emplace_back(Vector3d(-halfFieldX, -halfFieldY, 0), Vector3d(-halfFieldX, halfFieldY, 0));
  // right
  d_fieldLines.emplace_back(Vector3d(halfFieldX, -halfFieldY, 0), Vector3d(halfFieldX, halfFieldY, 0));

  // GOAL AREAS
  // left, top
  d_fieldLines.emplace_back(Vector3d(-halfFieldX, halfGoalAreaY, 0), Vector3d(-halfFieldX+d_goalAreaLengthX, halfGoalAreaY, 0));
  // left, bottom
  d_fieldLines.emplace_back(Vector3d(-halfFieldX, -halfGoalAreaY, 0), Vector3d(-halfFieldX+d_goalAreaLengthX, -halfGoalAreaY, 0));
  // left, side
  d_fieldLines.emplace_back(Vector3d(-halfFieldX+d_goalAreaLengthX, halfGoalAreaY, 0), Vector3d(-halfFieldX+d_goalAreaLengthX, -halfGoalAreaY, 0));
  // right, top
  d_fieldLines.emplace_back(Vector3d(halfFieldX, halfGoalAreaY, 0), Vector3d(halfFieldX-d_goalAreaLengthX, halfGoalAreaY, 0));
  // right, bottom
  d_fieldLines.emplace_back(Vector3d(halfFieldX, -halfGoalAreaY, 0), Vector3d(halfFieldX-d_goalAreaLengthX, -halfGoalAreaY, 0));
  // right, side
  d_fieldLines.emplace_back(Vector3d(halfFieldX-d_goalAreaLengthX, halfGoalAreaY, 0), Vector3d(halfFieldX-d_goalAreaLengthX, -halfGoalAreaY, 0));

  // CENTER LINE
  d_fieldLines.emplace_back(Vector3d(0, -halfFieldY, 0), Vector3d(0, halfFieldY, 0));

  LineJunctionFinder lineJunctionFinder;
  d_fieldLineJunctions = lineJunctionFinder.findLineJunctions(d_fieldLines);

  // CIRCLE
  int segments = Config::getStaticValue<int>("world.circle-segment-count");
  Vector3d lastPoint(0, d_circleRadius, 0);
  for (int i = 1; i <= segments; i++)
  {
    double theta = (i/(double)segments) * M_PI * 2;
    Vector3d point(sin(theta) * d_circleRadius, cos(theta) * d_circleRadius, 0);
    d_circleLines.emplace_back(lastPoint, point);
    d_fieldLines.emplace_back(lastPoint, point);
    lastPoint = point;
  }

  // TODO allow including circle in 'expected lines' view

  for (LineSegment3d const& line : d_fieldLines)
  {
    auto vec = (Vector3d)line.delta();
    auto perp = vec.cross(Vector3d::UnitZ());
    perp.normalize();
    perp *= (lineWidth / 2);

    d_fieldLineEdges.push_back(line + perp);
    d_fieldLineEdges.push_back(line - perp);
  }

  // GOAL POST POSITIONS

  // NOTE the rules define the goal width (D) as the space *between* the inner
  // goal post edges, not their centers.

  double goalRadius = d_goalPostDiameter / 2.0;

  d_goalPostPositions = {
    Vector3d(-halfFieldX,  halfGoalY + goalRadius, 0),
    Vector3d(-halfFieldX, -halfGoalY - goalRadius, 0),
    Vector3d(halfFieldX,  halfGoalY + goalRadius, 0),
    Vector3d(halfFieldX, -halfGoalY - goalRadius, 0)
  };

  d_ourGoalPostPositions = {
    Vector3d(-halfFieldX,  halfGoalY + goalRadius, 0),
    Vector3d(-halfFieldX, -halfGoalY - goalRadius, 0)
  };

  d_theirGoalPostPositions = {
    Vector3d(halfFieldX,  halfGoalY + goalRadius, 0),
    Vector3d(halfFieldX, -halfGoalY - goalRadius, 0)
  };
}


/*
 * CircleIterator.hpp
 *
 *  Created on: Nov 13, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"

#include <Eigen/Core>

#include <memory>

namespace grid_map {

/*!
 * Iterator class to iterate through a circular area of the map.
 */
class CircleIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param center the position of the circle center.
   * @param radius the radius of the circle.
   */
  CircleIterator(const GridMap& gridMap, const Position& center, const double radius);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  CircleIterator& operator =(const CircleIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const CircleIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Index& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  CircleIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

private:

  /*!
   * Check if current index is inside the circle.
   * @return true if inside, false otherwise.
   */
  bool isInside() const;

  /*!
   * Finds the submap that fully contains the circle and returns the parameters.
   * @param[in] center the position of the circle center.
   * @param[in] radius the radius of the circle.
   * @param[out] startIndex the start index of the submap.
   * @param[out] bufferSize the buffer size of the submap.
   */
  void findSubmapParameters(const Position& center, const double radius,
                            Index& startIndex, Size& bufferSize) const;

  //! Position of the circle center;
  Position center_;

  //! Radius of the circle.
  double radius_;

  //! Square of the radius (for efficiency).
  double radiusSquare_;

  //! Grid submap iterator. // TODO Think of using unique_ptr instead.
  std::shared_ptr<SubmapIterator> internalIterator_;

  //! Map information needed to get position from iterator.
  Length mapLength_;
  Position mapPosition_;
  double resolution_;
  Size bufferSize_;
  Index bufferStartIndex_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace */


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Matt Maly */

#define BOOST_TEST_MODULE "PDF"
#include <boost/test/unit_test.hpp>
#include "ompl/datastructures/PDF.h"
#include "ompl/util/RandomNumbers.h"
#include "../BoostTestTeamCityReporter.h"
#include <cmath>
#include <vector>

// define a convenience macro
#define BOOST_OMPL_EXPECT_NEAR(a, b, diff) BOOST_CHECK_SMALL((a) - (b), diff)

BOOST_AUTO_TEST_CASE(Simple)
{
    using Element = ompl::PDF<int>::Element;
    ompl::PDF<int> p;
    BOOST_CHECK(p.empty());
    p.add(0, 1.0);
    BOOST_CHECK_EQUAL(0, p.sample(0.5));
    BOOST_CHECK_EQUAL(1u, p.size());
    BOOST_CHECK_EQUAL(p.empty(), false);
    p.add(1, 0.0);
    p.add(2, 0.0);
    p.add(3, 0.0);
    BOOST_CHECK_EQUAL(0, p.sample(0.5));
    BOOST_CHECK_EQUAL(4u, p.size());

    p.clear();
    Element* e25 = p.add(0, 25);
    // 25
    BOOST_CHECK_EQUAL(0, p.sample(1.0));

    Element* e50 = p.add(1, 50);
    // 25 50
    BOOST_CHECK_EQUAL(0, p.sample(0.3));
    BOOST_CHECK_EQUAL(1, p.sample(0.5));

    Element* e15 = p.add(2, 15);
    // 25 50 15
    BOOST_CHECK_EQUAL(0, p.sample(0.25));
    BOOST_CHECK_EQUAL(1, p.sample(0.5));
    BOOST_CHECK_EQUAL(2, p.sample(0.85));

    Element* e10 = p.add(3, 10);
    // 25 50 15 10
    BOOST_CHECK_EQUAL(0, p.sample(0.1));
    BOOST_CHECK_EQUAL(1, p.sample(0.7));
    BOOST_CHECK_EQUAL(2, p.sample(0.8));
    BOOST_CHECK_EQUAL(3, p.sample(0.95));

    Element* e6 = p.add(4, 6);
    Element* e30 = p.add(5, 30);
    Element* e1 = p.add(6, 1);
    // 25 50 15 10 6 30 1

    p.remove(e1);
    // 25 50 15 10 6 30
    BOOST_CHECK_EQUAL(5, p.sample(0.80));
    BOOST_CHECK_EQUAL(0, p.sample(0.05));

    p.update(e30, 4);
    // 25 50 15 10 6 4
    BOOST_CHECK_EQUAL(5, p.sample(0.98));
    BOOST_CHECK_EQUAL(4, p.sample(0.93));

    p.remove(e6);
    // 25 50 15 10 4
    BOOST_CHECK_EQUAL(5u, p.size());
    BOOST_CHECK_EQUAL(3, p.sample(0.96));
    BOOST_CHECK_EQUAL(1, p.sample(0.3));

    p.remove(e25);
    // 4 50 15 10
    BOOST_CHECK_EQUAL(5, p.sample(0.03));
    BOOST_CHECK_EQUAL(1, p.sample(0.4));
    BOOST_CHECK_EQUAL(2, p.sample(0.85));
    BOOST_CHECK_EQUAL(3, p.sample(0.95));

    p.remove(e50);
    // 4 10 15
    BOOST_CHECK_EQUAL(2, p.sample(0.75));

    p.update(e10, 51);
    // 4 51 15
    BOOST_CHECK_EQUAL(3, p.sample(0.5));

    p.remove(e10);
    p.remove(e30);
    // 15
    BOOST_CHECK_EQUAL(2, p.sample(1.0));

    p.remove(e15);
    BOOST_CHECK_EQUAL(0u, p.size());
    BOOST_CHECK(p.empty());

    p.clear();
}

BOOST_AUTO_TEST_CASE(Statistical)
{
    const std::size_t NUM_SAMPLES = 5000000;
    /* The following widening factor is multiplied by the standard error of the mean
     * to obtain a reasonable range to pass to BOOST_OMPL_EXPECT_NEAR(). For five million samples,
     * a widening factor of 2.5 seems sufficient. If the number of samples is increased,
     * then this widening factor can be decreased. */
    const double STDERR_WIDENING_FACTOR = 2.5;

    std::vector<std::pair<int,double> > values;
    values.emplace_back(0, 30.0);
    values.emplace_back(1, 10.0);
    values.emplace_back(2, 25.0);
    values.emplace_back(3, 15.0);
    values.emplace_back(4, 20.0);

    ompl::PDF<int> p;
    double mean = 0.0;
    double sumWeights = 0.0;
    /* Calculate weighted mean of discrete uniform distribution as we add elements to PDF. */
    for (const auto & elem : values)
    {
        p.add(elem.first, elem.second);
        mean += elem.first*elem.second;
        sumWeights += elem.second;
    }
    mean /= sumWeights;

    /* Calculate weighted variance of discrete uniform distribution, which is defined as
     * sum(w[i]*(x[i]-mean)^2)/sumWeights. */
    double variance = 0.0;
    for (const auto elem : values)
    {
        variance += elem.second * (elem.first - mean) * (elem.first - mean);
    }
    variance /= sumWeights;
    double standerr = sqrt(variance/NUM_SAMPLES);

    double sampleMean = 0.0;
    ompl::RNG rand;
    for (std::size_t i = 0; i < NUM_SAMPLES; ++i)
        sampleMean += p.sample(rand.uniform01());
    sampleMean /= NUM_SAMPLES;

    BOOST_OMPL_EXPECT_NEAR(sampleMean, mean, STDERR_WIDENING_FACTOR*standerr);
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//
#include <boost/test/unit_test.hpp>
#include "armnnCaffeParser/ICaffeParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(CaffeParser)

struct ConcatFixture : public ParserPrototxtFixture<armnnCaffeParser::ICaffeParser>
{
    ConcatFixture()
    {
        m_Prototext = "name: \"Concat\"\n"
            "layer {\n"
            "  name: \"data\"\n"
            "  type: \"Input\"\n"
            "  top: \"data\"\n"
            "  input_param { shape: { dim: 1 dim: 1 dim: 4 dim: 4 } }\n"
            "}\n"
            "layer {\n"
            "    bottom: \"data\"\n"
            "    top: \"pool1\"\n"
            "    name: \"pool1\"\n"
            "    type: \"Pooling\"\n"
            "    pooling_param {\n"
            "        kernel_size: 2\n"
            "        stride: 2\n"
            "        pool: MAX\n"
            "    }\n"
            "}\n"
            "layer {\n"
            "    bottom: \"data\"\n"
            "    top: \"pool2\"\n"
            "    name: \"pool2\"\n"
            "    type: \"Pooling\"\n"
            "    pooling_param {\n"
            "        kernel_size: 2\n"
            "        stride: 2\n"
            "        pool: MAX\n"
            "    }\n"
            "}\n"
            "layer {\n"
            "    bottom: \"pool1\"\n"
            "    bottom: \"pool2\"\n"
            "    top: \"concat\"\n"
            "    name: \"concat\"\n"
            "    type: \"Concat\"\n"
            "}\n";
        SetupSingleInputSingleOutput("data", "concat");
    }
};

BOOST_FIXTURE_TEST_CASE(ParseConcat, ConcatFixture)
{
    RunTest<4>(
        {
            0, 1, 0, 0,
            0, 0, 0, 0,
            0, 0, 1, 0,
            1, 0, 1, 1
        },
        {
            1, 0,
            1, 1,

            1, 0,
            1, 1
        });
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef SIGMA_STD140_HPP
#define SIGMA_STD140_HPP

#include <sigma/util/numeric.hpp>

#include <glm/gtc/type_ptr.hpp>
#include <glm/mat2x2.hpp>
#include <glm/mat3x3.hpp>
#include <glm/mat4x4.hpp>
#include <glm/vec2.hpp>
#include <glm/vec3.hpp>
#include <glm/vec4.hpp>

// #include <boost/hana/for_each.hpp>
// #include <boost/hana/fuse.hpp>

#include <algorithm>
#include <array>
#include <cstddef>

#define std140_sizeof(T) sigma::std140::detial::type_traits<T>::size()

namespace sigma {
namespace std140 {
    namespace detial {
        template <typename>
        struct type_traits;
    }

    template <typename T>
    static constexpr std::size_t alignment()
    {
        return detial::type_traits<T>::alignment();
    }

    template <typename T>
    static constexpr std::size_t size()
    {
        return detial::type_traits<T>::size();
    }

    template <typename T>
    static constexpr std::size_t member_offset(std::size_t member_index)
    {
        const T temp {};
        std::size_t i = 0;
        std::size_t offset = 0;
        boost::hana::for_each(temp, boost::hana::fuse([&](auto name, auto member) {
            using member_type = decltype(member);
            const std::size_t member_size = size<member_type>();
            const std::size_t member_alignment = alignment<member_type>();

            if (i < member_index) {
                offset = numeric::round_up(offset, member_alignment);
                offset += member_size;
            } else if (i == member_index) {
                offset = numeric::round_up(offset, member_alignment);
            }
            i++;
        }));

        return offset;
    }

    template <typename T>
    static void to_std140(const T& source, std::uint8_t* dest)
    {
        detial::type_traits<T>::write(source, dest, 0);
    }

    namespace detial {
        template <class T>
        struct type_traits {
            static constexpr std::size_t size()
            {
                const T temp {};
                std::size_t total = 0;
                boost::hana::for_each(temp, boost::hana::fuse([&total](auto name, auto member) {
                    using member_type = decltype(member);
                    const std::size_t member_size = type_traits<member_type>::size();
                    const std::size_t member_alignment = type_traits<member_type>::alignment();
                    total = numeric::round_up(total, member_alignment);
                    total += member_size;
                }));
                return total;
            }

            static std::size_t write(const T& value, std::uint8_t* buffer, std::size_t offset)
            {
                boost::hana::for_each(value, boost::hana::fuse([&](auto name, auto member) {
                    offset = type_traits<decltype(member)>::write(member, buffer, offset);
                }));
                return offset;
            }
        };

        template <>
        struct type_traits<float> {
            static constexpr std::size_t alignment()
            {
                return sizeof(float);
            }

            static constexpr std::size_t size()
            {
                return sizeof(float);
            }

            static std::size_t write(const float& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)&value;
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(float), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <>
        struct type_traits<double> {
            static constexpr std::size_t alignment()
            {
                return sizeof(double);
            }

            static constexpr std::size_t size()
            {
                return sizeof(double);
            }

            static std::size_t write(const double& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)&value;
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(double), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <>
        struct type_traits<bool> {
            static constexpr std::size_t alignment()
            {
                return type_traits<float>::alignment();
            }

            static constexpr std::size_t size()
            {
                return type_traits<float>::size();
            }

            static std::size_t write(const bool& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)&value;
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(bool), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <>
        struct type_traits<int> {
            static constexpr std::size_t alignment()
            {
                return type_traits<float>::alignment();
            }

            static constexpr std::size_t size()
            {
                return type_traits<float>::size();
            }

            static std::size_t write(const int& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)&value;
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(int), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <>
        struct type_traits<unsigned int> {
            static constexpr std::size_t alignment()
            {
                return type_traits<float>::alignment();
            }

            static constexpr std::size_t size()
            {
                return type_traits<float>::size();
            }

            static std::size_t write(const unsigned int& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)&value;
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(unsigned int), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tvec2<T>> {
            static constexpr std::size_t alignment()
            {
                return 2 * type_traits<T>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 2 * type_traits<T>::size();
            }

            static std::size_t write(const glm::tvec2<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value);
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(glm::tvec2<T>), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tvec3<T>> {
            static constexpr std::size_t alignment()
            {
                return 4 * type_traits<T>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 3 * type_traits<T>::size();
            }

            static std::size_t write(const glm::tvec3<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value);
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(glm::tvec3<T>), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tvec4<T>> {
            static constexpr std::size_t alignment()
            {
                return 4 * type_traits<T>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 4 * type_traits<T>::size();
            }

            static std::size_t write(const glm::tvec4<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value);
                offset = numeric::round_up(offset, alignment());
                std::copy(data, data + sizeof(glm::tvec4<T>), buffer + offset);
                offset += size();
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tmat2x2<T>> {
            static constexpr std::size_t alignment()
            {
                return type_traits<glm::tvec4<T>>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 2 * type_traits<glm::tvec4<T>>::size();
            }

            static std::size_t write(const glm::tmat2x2<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                // TODO investigate this
                offset = numeric::round_up(offset, alignment());
                for (unsigned int i = 0; i < 2; ++i) {
                    const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value[i]);
                    std::copy(data, data + 2 * sizeof(T), buffer + offset);
                    offset += alignment();
                }
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tmat3x3<T>> {
            static constexpr std::size_t alignment()
            {
                return type_traits<glm::tvec4<T>>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 3 * type_traits<glm::tvec4<T>>::size();
            }

            static std::size_t write(const glm::tmat3x3<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                // TODO investigate this
                offset = numeric::round_up(offset, alignment());
                for (unsigned int i = 0; i < 3; ++i) {
                    const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value[i]);
                    std::copy(data, data + 3 * sizeof(T), buffer + offset);
                    offset += alignment();
                }
                return offset;
            }
        };

        template <typename T>
        struct type_traits<glm::tmat4x4<T>> {
            static constexpr std::size_t alignment()
            {
                return type_traits<glm::tvec4<T>>::alignment();
            }

            static constexpr std::size_t size()
            {
                return 4 * type_traits<glm::tvec4<T>>::size();
            }

            static std::size_t write(const glm::tmat4x4<T>& value, std::uint8_t* buffer, std::size_t offset)
            {
                // TODO investigate this
                offset = numeric::round_up(offset, alignment());
                for (unsigned int i = 0; i < 4; ++i) {
                    const std::uint8_t* data = (const std::uint8_t*)glm::value_ptr(value[i]);
                    std::copy(data, data + 4 * sizeof(T), buffer + offset);
                    offset += alignment();
                }
                return offset;
            }
        };

        template <typename T, std::size_t N>
        struct type_traits<std::array<T, N>> {
            static constexpr std::size_t alignment()
            {
                return numeric::round_up(type_traits<T>::alignment(), type_traits<glm::vec4>::alignment());
            }

            static constexpr std::size_t size()
            {
                return N * numeric::round_up(type_traits<T>::size(), type_traits<glm::vec4>::alignment());
            }

            static std::size_t write(const std::array<T, N>& value, std::uint8_t* buffer, std::size_t offset)
            {
                // TODO investigate this.
                offset = numeric::round_up(offset, alignment());
                for (unsigned int i = 0; i < N; ++i) {
                    // TODO really investigate this, especially when data is more complicated then basic types.
                    offset = type_traits<T>::write(value[i], buffer, offset);
                    offset = numeric::round_up(offset, alignment());
                }
                return offset;
            }
        };
    }
}
}

#endif // SIGMA_STD140_HPP