/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER 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.
 */

#ifndef MATH_HPP_wegfzebvgfhjusd
#define MATH_HPP_wegfzebvgfhjusd

#include "foundation.hpp"

#include <Eigen/Dense>

namespace vts
{

typedef Eigen::Vector2f vec2f;
typedef Eigen::Vector3f vec3f;
typedef Eigen::Vector4f vec4f;
typedef Eigen::Matrix2f mat2f;
typedef Eigen::Matrix3f mat3f;
typedef Eigen::Matrix4f mat4f;

typedef Eigen::Vector2d vec2;
typedef Eigen::Vector3d vec3;
typedef Eigen::Vector4d vec4;
typedef Eigen::Matrix2d mat2;
typedef Eigen::Matrix3d mat3;
typedef Eigen::Matrix4d mat4;

typedef Eigen::Matrix<sint16, 2, 1> vec2si16;
typedef Eigen::Matrix<uint16, 2, 1> vec2ui16;
typedef Eigen::Matrix<sint16, 3, 1> vec3si16;
typedef Eigen::Matrix<uint16, 3, 1> vec3ui16;
typedef Eigen::Matrix<sint16, 4, 1> vec4si16;
typedef Eigen::Matrix<uint16, 4, 1> vec4ui16;

typedef Eigen::Matrix<sint32, 2, 1> vec2si32;
typedef Eigen::Matrix<uint32, 2, 1> vec2ui32;
typedef Eigen::Matrix<sint32, 3, 1> vec3si32;
typedef Eigen::Matrix<uint32, 3, 1> vec3ui32;
typedef Eigen::Matrix<sint32, 4, 1> vec4si32;
typedef Eigen::Matrix<uint32, 4, 1> vec4ui32;

VTS_API double modulo(double a, double m);
VTS_API double smoothstep(double f);
VTS_API double smootherstep(double f);
VTS_API double degToRad(double angle);
VTS_API double radToDeg(double angle);
VTS_API void normalizeAngle(double &a);
VTS_API double angularDiff(double a, double b);
VTS_API vec3 angularDiff(const vec3 &a, const vec3 &b);

VTS_API vec3 cross(const vec3 &a, const vec3 &b);
VTS_API vec3 anyPerpendicular(const vec3 &v);
VTS_API vec3f cross(const vec3f &a, const vec3f &b);
VTS_API vec3f anyPerpendicular(const vec3f &v);

VTS_API mat3 identityMatrix3();
VTS_API mat4 identityMatrix4();
VTS_API mat4 rotationMatrix(int axis, double degrees);
VTS_API mat4 scaleMatrix(double s);
VTS_API mat4 scaleMatrix(double sx, double sy, double sz);
VTS_API mat4 scaleMatrix(const vec3 &vec);
VTS_API mat4 translationMatrix(double tx, double ty, double tz);
VTS_API mat4 translationMatrix(const vec3 &vec);
VTS_API mat4 lookAt(const vec3 &eye, const vec3 &target, const vec3 &up);
VTS_API mat4 lookAt(const vec3 &eye, const vec3 &target);
VTS_API mat4 frustumMatrix(double left, double right,
                           double bottom, double top,
                           double near_, double far_);
VTS_API mat4 perspectiveMatrix(double fovyDegs, double aspect,
                               double near_, double far_);
VTS_API mat4 orthographicMatrix(double left, double right,
                                double bottom, double top,
                                double near_, double far_);

VTS_API double aabbPointDist(const vec3 &point,
                             const vec3 &min, const vec3 &max);
VTS_API bool aabbTest(const vec3 aabb[2], const vec4 planes[6]);
VTS_API void frustumPlanes(const mat4 &vp, vec4 planes[6]);

VTS_API vec2ui16 vec2to2ui16(const vec2 &v, bool normalized = true);
VTS_API vec2ui16 vec2to2ui16(const vec2f &v, bool normalized = true);
VTS_API mat3 mat4to3(const mat4 &mat);
VTS_API mat4 mat3to4(const mat3 &mat);
VTS_API mat3 rawToMat3(const double v[9]);
VTS_API mat4 rawToMat4(const double v[16]);
VTS_API mat3f rawToMat3(const float v[9]);
VTS_API mat4f rawToMat4(const float v[16]);
VTS_API void matToRaw(const mat3 &a, double v[9]);
VTS_API void matToRaw(const mat4 &a, double v[16]);
VTS_API void matToRaw(const mat3f &a, float v[9]);
VTS_API void matToRaw(const mat4f &a, float v[16]);

VTS_API vec3f convertRgbToHsv(const vec3f &inColor);
VTS_API vec3f convertHsvToRgb(const vec3f &inColor);
VTS_API vec3f convertToRainbowColor(float inValue);

inline double clamp(double a, double min, double max)
{
    assert(min <= max);
    return std::max(std::min(a, max), min);
}

inline double interpolate(double a, double b, double f)
{
    return (b - a) * f + a;
}

inline double dot(const vec3 &a, const vec3 &b)
{
    return a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
}

inline double dot(const vec2 &a, const vec2 &b)
{
    return a(0) * b(0) + a(1) * b(1);
}

inline double length(const vec3 &a)
{
    return std::sqrt(dot(a, a));
}

inline double length(const vec2 &a)
{
    return std::sqrt(dot(a, a));
}

inline vec3 normalize(const vec3 &a)
{
    return a / length(a);
}

inline vec3 min(const vec3 &a, const vec3 &b)
{
    return vec3(std::min(a(0), b(0)),
                std::min(a(1), b(1)),
                std::min(a(2), b(2)));
}

inline vec3 max(const vec3 &a, const vec3 &b)
{
    return vec3(std::max(a(0), b(0)),
                std::max(a(1), b(1)),
                std::max(a(2), b(2)));
}

inline vec3 interpolate(const vec3 &a, const vec3 &b, double f)
{
    return vec3(interpolate(a(0), b(0), f),
                interpolate(a(1), b(1), f),
                interpolate(a(2), b(2), f));
}

inline float dot(const vec3f &a, const vec3f &b)
{
    return a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
}

inline float dot(const vec2f &a, const vec2f &b)
{
    return a(0) * b(0) + a(1) * b(1);
}

inline float length(const vec3f &a)
{
    return std::sqrt(dot(a, a));
}

inline float length(const vec2f &a)
{
    return std::sqrt(dot(a, a));
}

inline vec3f normalize(const vec3f &a)
{
    return a / length(a);
}

inline vec3f min(const vec3f &a, const vec3f &b)
{
    return vec3f(std::min(a(0), b(0)),
        std::min(a(1), b(1)),
        std::min(a(2), b(2)));
}

inline vec3f max(const vec3f &a, const vec3f &b)
{
    return vec3f(std::max(a(0), b(0)),
        std::max(a(1), b(1)),
        std::max(a(2), b(2)));
}

inline vec3f interpolate(const vec3f &a, const vec3f &b, float f)
{
    return vec3f(interpolate(a(0), b(0), f),
        interpolate(a(1), b(1), f),
        interpolate(a(2), b(2), f));
}

inline vec4 vec3to4(const vec3 &v, double w)
{
    vec4 res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = v[2];
    res[3] = w;
    return res;
}

inline vec3 vec4to3(const vec4 &v, bool division = false)
{
    vec3 res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = v[2];
    if (division)
        res /= v[3];
    return res;
}

inline vec3 vec2to3(const vec2 &v, double w)
{
    vec3 res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = w;
    return res;
}

inline vec2 vec3to2(const vec3 &v, bool division = false)
{
    vec2 res;
    res[0] = v[0];
    res[1] = v[1];
    if (division)
        res /= v[2];
    return res;
}

inline vec2 vec4to2(const vec4 &v, bool division = false)
{
    vec2 res;
    res[0] = v[0];
    res[1] = v[1];
    if (division)
        res /= v[3];
    return res;
}

inline vec4f vec3to4(const vec3f &v, float w)
{
    vec4f res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = v[2];
    res[3] = w;
    return res;
}

inline vec3f vec4to3(const vec4f &v, bool division = false)
{
    vec3f res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = v[2];
    if (division)
        res /= v[3];
    return res;
}

inline vec3f vec2to3(const vec2f &v, float w)
{
    vec3f res;
    res[0] = v[0];
    res[1] = v[1];
    res[2] = w;
    return res;
}

inline vec2f vec3to2(const vec3f &v, bool division = false)
{
    vec2f res;
    res[0] = v[0];
    res[1] = v[1];
    if (division)
        res /= v[2];
    return res;
}

inline vec2f vec4to2(const vec4f &v, bool division = false)
{
    vec2f res;
    res[0] = v[0];
    res[1] = v[1];
    if (division)
        res /= v[3];
    return res;
}

inline vec2 rawToVec2(const double v[2])
{
    return vec2(v[0], v[1]);
}

inline vec3 rawToVec3(const double v[3])
{
    return vec3(v[0], v[1], v[2]);
}

inline vec4 rawToVec4(const double v[4])
{
    return vec4(v[0], v[1], v[2], v[3]);
}

inline vec2f rawToVec2(const float v[2])
{
    return vec2f(v[0], v[1]);
}

inline vec3f rawToVec3(const float v[3])
{
    return vec3f(v[0], v[1], v[2]);
}

inline vec4f rawToVec4(const float v[4])
{
    return vec4f(v[0], v[1], v[2], v[3]);
}

inline void vecToRaw(const vec2 &a, double v[2])
{
    for (int i = 0; i < 2; i++)
        v[i] = a[i];
}

inline void vecToRaw(const vec3 &a, double v[3])
{
    for (int i = 0; i < 3; i++)
        v[i] = a[i];
}

inline void vecToRaw(const vec4 &a, double v[4])
{
    for (int i = 0; i < 4; i++)
        v[i] = a[i];
}

inline void vecToRaw(const vec2f &a, float v[2])
{
    for (int i = 0; i < 2; i++)
        v[i] = a[i];
}

inline void vecToRaw(const vec3f &a, float v[3])
{
    for (int i = 0; i < 3; i++)
        v[i] = a[i];
}

inline void vecToRaw(const vec4f &a, float v[4])
{
    for (int i = 0; i < 4; i++)
        v[i] = a[i];
}

template<class T, class U>
inline T vecFromUblas(const U &u)
{
    T res;
    for (unsigned i = 0; i < u.size(); i++)
        res(i) = u[i];
    return res;
}

template<class U, class T>
inline U vecToUblas(const T &t)
{
    U res;
    for (unsigned i = 0; i < res.size(); i++)
        res[i] = t(i);
    return res;
}

inline constexpr double nan1() { return std::numeric_limits<double>::quiet_NaN(); }
inline vec2 nan2() { return vec2(nan1(), nan1()); }
inline vec3 nan3() { return vec3(nan1(), nan1(), nan1()); }
inline vec4 nan4() { return vec4(nan1(), nan1(), nan1(), nan1()); }

inline constexpr double inf1() { return std::numeric_limits<double>::infinity(); }
inline vec2 inf2() { return vec2(inf1(), inf1()); }
inline vec3 inf3() { return vec3(inf1(), inf1(), inf1()); }
inline vec4 inf4() { return vec4(inf1(), inf1(), inf1(), inf1()); }

} // namespace vts

#endif // MATH_H_wegfzebvgfhjusd


#ifndef RECOGNITION_HPP
#define RECOGNITION_HPP

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <boost/foreach.hpp>

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/correspondence.h>
#include <pcl/features/shot.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/hv/hv_go.h>
#include <pcl/registration/icp.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>

#include <nimbus_fh_detector/features.h>
#include <nimbus_fh_detector/filters.h>

namespace nimbus{
    class Recognition
    {
        private:
            std::string _path;
            ros::NodeHandle _nh;
            geometry_msgs::TransformStamped pose;

            tf2_ros::TransformBroadcaster tfb;

            ros::Publisher pubPose;
            ros::Publisher _pub;

        protected:
            std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _model;
            std::vector<pcl::PointCloud<pcl::Normal>::Ptr> _model_normals;
            std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _model_keypoints;
            std::vector<pcl::PointCloud<pcl::SHOT352>::Ptr> _model_description;
            std::vector<pcl::PointCloud<pcl::ReferenceFrame>::Ptr> _model_board;

            nimbus::Features<pcl::PointXYZI, pcl::Normal, pcl::SHOT352> _features;
            std::vector<pcl::CorrespondencesPtr> model_scene_corr;
        public:
            Recognition(ros::NodeHandle nh, const std::string path);
            ~Recognition();
            void constructModelParam();
            void correspondences(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr blob);
            void cloudHough3D(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr blob);
            void registrationICP (const std::vector<pcl::PointCloud<pcl::PointXYZI>::ConstPtr> instances,
                                  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr scene,
                                  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations,
                                  std::vector<pcl::Correspondences> clustered_corrs);
            void hypothesisVerification(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr blob,
                                        std::vector<pcl::PointCloud<pcl::PointXYZI>::ConstPtr> registered_instances,
                                        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations);
            void publishPose(std::vector<bool> mask, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations);
            void visualization (const int num,
                    const pcl::PointCloud<pcl::PointXYZI>::Ptr  scene,
                    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations,
                    std::vector<pcl::Correspondences> clustered_corrs);
    };
}
#endif  //UTILITIES_H


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 3, 1> test_int_3_1(const Eigen::Matrix<int, 3, 1> & M)
{
	return M;
}
void export_int_3_1()
{
	boost::python::def("test_int_3_1",test_int_3_1);
}


#ifndef RGB_ALGORITHMS_HPP
#define RGB_ALGORITHMS_HPP

#include <boost/gil/gil_all.hpp>

namespace boost { namespace gil {
namespace toolbox {

struct calc_negative
{
   template < typename DST_CHANNEL
            , typename SRC_CHANNEL
            > void operator()( DST_CHANNEL& dst
                             , SRC_CHANNEL& src ) const
   {
      SRC_CHANNEL max = channel_traits< SRC_CHANNEL >::max_value();

      dst = max - src;
   }

   template < typename CHANNEL
            > void operator()( CHANNEL& c ) const
   {
      CHANNEL max = channel_traits< CHANNEL >::max_value();

      c = max - c;
   }
};

template< typename RGB_VIEW >
void negative( const RGB_VIEW& view )
{
   typedef typename channel_type< RGB_VIEW >::type channel_t;
   channel_t max = channel_traits<channel_t>::max_value();

   for( int y=0; y < view.height(); ++y )
   {
      typename RGB_VIEW::x_iterator it = view.row_begin( y );

      for( int x = 0; x < view.width(); ++x )
      {
         static_for_each( it[x], calc_negative() );
      }
   }
}

template< typename RGB_VIEW >
void negative( const RGB_VIEW& src
             , const RGB_VIEW& dst )
{
   // make sure src and dst have same dimensions.

   // for rgba do it only for rgb channels

   typedef typename channel_type< RGB_VIEW >::type channel_t;
   channel_t max = channel_traits<channel_t>::max_value();

   for( int y=0; y < src.height(); ++y )
   {
      typename RGB_VIEW::x_iterator src_it = src.row_begin( y );
      typename RGB_VIEW::x_iterator dst_it = dst.row_begin( y );

      for( int x = 0; x < src.width(); ++x )
      {
         static_for_each( dst_it[x], src_it[x], calc_negative() );
      }
   }
}

struct calc_brightness
{
   float _factor;

   template < typename DST_CHANNEL
            , typename SRC_CHANNEL
            > void operator()( DST_CHANNEL&       dst
                             , const SRC_CHANNEL& src ) const
   {
      float d = static_cast<float>( src ) * _factor;

      if( d > channel_traits< DST_CHANNEL >::max_value() )
      {
         dst = channel_traits< DST_CHANNEL >::max_value();
      }
      else
      {
         dst = static_cast< DST_CHANNEL >( d );
      }
   }
};

template< typename RGB_VIEW >
void brightness( const RGB_VIEW& src
               , const RGB_VIEW& dst
               , float           factor )
{
   calc_brightness calc;
   calc._factor = factor;

   for( int y=0; y < src.height(); ++y )
   {
      typename RGB_VIEW::x_iterator src_it = src.row_begin( y );
      typename RGB_VIEW::x_iterator dst_it = dst.row_begin( y );

      for( int x = 0; x < src.width(); ++x )
      {
         static_for_each( dst_it[x], src_it[x], calc );
      }
   }
}

template< typename VALUE >
struct calc_brightness_
{
   VALUE _number;

   template < typename DST_CHANNEL
            , typename SRC_CHANNEL
            > void operator()( DST_CHANNEL&       dst
                             , const SRC_CHANNEL& src ) const
   {
      VALUE d = static_cast< VALUE >( src ) + _number;

      if( d > channel_traits< DST_CHANNEL >::max_value() )
      {
         dst = channel_traits< DST_CHANNEL >::max_value();
      }
      else
      {
         dst = d;
      }
   }
};

template< typename RGB_VIEW
        , typename VALUE >
void brightness_( const RGB_VIEW& src
                , const RGB_VIEW& dst
                , VALUE           number )
{
   calc_brightness_<VALUE> calc;
   calc._number = number;

   for( int y=0; y < src.height(); ++y )
   {
      typename RGB_VIEW::x_iterator src_it = src.row_begin( y );
      typename RGB_VIEW::x_iterator dst_it = dst.row_begin( y );

      for( int x = 0; x < src.width(); ++x )
      {
         static_for_each( dst_it[x], src_it[x], calc );
      }
   }
}

template< typename RGB_VIEW >
void remove_channel( const RGB_VIEW& src
                   , unsigned int    c   )
{
   typedef typename channel_type< RGB_VIEW >::type channel_t;
   channel_t min_value = channel_traits<channel_t>::min_value();

   for( int y = 0; y < src.dimensions().y; ++y )
   {
      typename RGB_VIEW::x_iterator src_it = src.row_begin( y );

      for( int x = 0; x < src.dimensions().x; ++x )
      {
         dynamic_at_c( src_it[x], c ) = min_value;
      }
   }
}

template< typename CHANNEL
        , typename RGB_VIEW >
void remove_channel( const RGB_VIEW& src )
{
   typedef typename channel_type< RGB_VIEW >::type channel_t;
   channel_t min = channel_traits<channel_t>::min_value();

   for( int y = 0; y < src.dimensions().y; ++y )
   {
      typename RGB_VIEW::x_iterator src_it = src.row_begin( y );

      for( int x = 0; x < src.dimensions().x; ++x )
      {
         get_color( src_it[x], CHANNEL() ) = min;
      }
   }
}

template< typename RGB_VIEW >
void difference( const RGB_VIEW& src_1
               , const RGB_VIEW& src_2
               , const RGB_VIEW& dst    )
{
   if( src_1.dimensions() != src_2.dimensions() )
   {
      throw std::runtime_error( "Source image have different dimensions." );
   }

   if( src_1.dimensions() != dst.dimensions() )
   {
      throw std::runtime_error( "Destination image must have equal dimensions as the source images." );
   }

   for( int y = 0; y < src_1.dimensions().y; ++y )
   {
      typename RGB_VIEW::x_iterator src_1_it = src_1.row_begin( y );
      typename RGB_VIEW::x_iterator src_2_it = src_2.row_begin( y );

      typename RGB_VIEW::x_iterator dst_it = dst.row_begin( y );

      typedef typename channel_type<RGB_VIEW>::type channel_t;

      for( int x = 0; x < src_1.dimensions().x; ++x )
      {
			 assert(false);
		//TODO: what is calc_diff?
         //static_for_each( src_1_it[x], src_1_it[x], dst_it[x], calc_diff() );
      }
   }
}

} //namespace toolbox
} //namespace gil
} //namespace boost

#endif // RGB_ALGORITHMS_HPP


#include "Parser.hpp"

#include <stack>
#include <queue>

#include <boost/lexical_cast.hpp>

#include "Lexer.hpp"
#include "Node.hpp"

enum class NodeType
{
    UNDEFINED,
    OPERATOR,
    VALUE
};

struct NodeData
{
    NodePtr node;
    TokenList::iterator iteratorToGetNode;
};

NodeData nextNode(TokenList::iterator it, TokenList::iterator end, VariableMap& map);

ParseResult parse(std::string source)
{
    ParseResult result;
    auto tokens = tokenize(std::move(source)).tokens;
    result.statement = nextNode(tokens.begin(), tokens.end(), result.vars).node;

    return result;
}

NodeData nextNode(TokenList::iterator begin, TokenList::iterator end, VariableMap& map)
{
    NodeData data;
    for(; begin != end; ++begin)
    {
        auto& token = *begin;
        switch(token.type)
        {
            case Token::Type::RIGHT_BRAC:
            case Token::Type::LEFT_BRAC:
                continue;

            case Token::Type::OPERATOR:
                {
                    auto leftNodeData = nextNode(begin + 1, end, map);
                    auto rightNodeData = nextNode(leftNodeData.iteratorToGetNode + 1, end, map);
                    return NodeData{node<OperatorNode>(boost::lexical_cast<Operator>(token.source), std::move(leftNodeData.node), std::move(rightNodeData.node)),
                                    rightNodeData.iteratorToGetNode};
                }
                break;
            case Token::Type::NUMBER:
                {
                    return NodeData{node<ValueNode>(boost::lexical_cast<int>(token.source)), begin};
                }
                break;
            case Token::Type::VARIABLE:
                {
                    if(map.find(token.source) == map.end()) { map.emplace(token.source, int()); }
                    return NodeData{node<VariableNode>(token.source), begin};
                }
                break;
        }
    }

    return data;
}

std::string nextNode(const NodePtr& node)
{
    if(node.get() == nullptr)
    {
        return std::string{""};
    }

    switch(node->type())
    {
        case Node::Type::OPERATION:
            {
                const OperatorNode& castedNode = static_cast<const OperatorNode&>(*node.get());
                if(castedNode.left.get() == nullptr || castedNode.right.get() == nullptr)
                {
                    return std::string{"null"};
                }

                return "(" + boost::lexical_cast<std::string>(castedNode.op) + " " +
                        nextNode(castedNode.left) + " " + nextNode(castedNode.right) + ")";
            }
            break;
        case Node::Type::VALUE:
            {
                const ValueNode& castedNode = static_cast<const ValueNode&>(*node.get());
                return boost::lexical_cast<std::string>(castedNode.value);
            }
           break;
        case Node::Type::VARIABLE:
            {
                const VariableNode& castedNode = static_cast<const VariableNode&>(*node.get());
                return boost::lexical_cast<std::string>(castedNode.id);
            }
            break;
        default:
            break;
    }

    return std::string{"invalid node"};
}

std::string reverse_parse(const NodePtr& node)
{
    ASSERT(node.get() != nullptr, "Node is null");
    return nextNode(node);
}


#define BOOST_TEST_MODULE main test
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(dummy)
{
    BOOST_TEST(1 + 1 == 2);
}


#include "biggles/observation.hpp"
#include "biggles/detail/random.hpp"
#include "biggles/mh_moves/mh_moves.hpp"
#include "biggles/mh_moves/utility.hpp"
#include "biggles/track.hpp"
#include "biggles/tracker.hpp"
#include "biggles/kalman_filter.hpp"
#include "biggles/simulate.hpp"
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <string>
#include "biggles/tools/sundries.hpp"

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

typedef boost::shared_ptr< const kalman_filter > shared_const_kalman_filter_ptr;
//typedef std::pair<float, float> data_pair;

std::ostream& operator<<(std::ostream& os, const kalman_filter::state_vector& state) {
    os << boost::format("[% .4f, % .4f, % .4f, % .4f]") % state[0] % state[1] % state[2] % state[3];
    return os;
}

float max_deviation_from_line( const observation_collection &oc) {
    time_stamp start = oc.first_time_stamp();
    time_stamp end = oc.last_time_stamp();
    float x_off = x(*oc.begin());
    float y_off = y(*oc.begin());
    float delta_x = (x(oc.back()) - x_off)/float(end-start-1);
    float delta_y = (y(oc.back()) - y_off)/float(end-start-1);
    float result = 0.f;
    for (time_stamp ts = start; ts != end; ++ts) {
        observation_collection::const_range obs_at_t = oc.at_time_stamp(ts);
        float line_x = x_off + (ts - start) * delta_x;
        float line_y = y_off + (ts - start) * delta_y;
        for (; obs_at_t.first != obs_at_t.second; ++(obs_at_t.first)) {
            float diff = hypotf(line_x - x(*obs_at_t.first), line_y - y(*obs_at_t.first));
            if (diff > result) result = diff;
        }
    }
    return result;
}

shared_const_track_ptr generate_rotating_track(int duration) {
    const float delta_angle = 2.0f*3.1415f/float(duration);
    observation_collection oc;
    for (time_stamp t = 0; t < duration; ++t) {
        float angle = t*delta_angle;
        oc.insert(new_obs(std::cos(angle), std::sin(angle), t));
    }
    return shared_const_track_ptr(new track(0, duration, oc.begin(), oc.end(), 1.0f));
}

shared_const_track_ptr generate_linear_track(int duration, float dx, float dy) {
    observation_collection oc;
    for (time_stamp t = 0; t < duration; ++t) oc.insert(new_obs( t*dx, t*dy, t));
    return shared_const_track_ptr(new track(0, duration, oc.begin(), oc.end(), 1.0f));
}

shared_const_track_ptr generate_linear_track(
    time_stamp begin_ts, time_stamp end_ts, float x0, float y0, float dx, float dy, float loc_err)
{
    observation_collection oc;
    for (time_stamp ts = begin_ts; ts < end_ts; ++ts) oc.insert(new_obs(
        x0 + (ts - begin_ts)*dx + sampling::sample_normal() * loc_err,
        y0 + (ts - begin_ts)*dy + sampling::sample_normal() * loc_err,
        ts));
    return shared_const_track_ptr(new track(begin_ts, end_ts, oc.begin(), oc.end(), 1.0f));
}

void print_kalman_filter_states(const kalman_filter::states_and_cov_deque& sc) {
    kalman_filter::states_and_cov_deque::const_iterator sc_iter;
    int i = 0;
    for (sc_iter = sc.begin(); sc_iter != sc.end(); ++sc_iter) {
        std::stringstream sstr;
        sstr
            << boost::format("%3d: ") % i++
            << sc_iter->first
            << ", "
            << boost::format("v = % .4f") % hypotf(sc_iter->first[1], sc_iter->first[3])
            << ", "
            << boost::format("log(||C||) = % .4f") % logf(sc_iter->second.determinant());
        diag("%s", sstr.str().c_str());
    }
}

std::string print_parameters(const model::parameters& params) {
    std::stringstream para_ss;
    para_ss << boost::format("b = %.2f, c = %.2f, o = %.2f%%, s = %.2f%%, cr = %.2f")
            % model::mean_new_tracks_per_frame(params)
            % model::mean_false_observations_per_frame(params)
            % (model::generate_observation_probability(params) * 100.f)
            % (model::frame_to_frame_survival_probability(params) * 100.f)
            % model::constraint_radius(params)
            << ", "
            << boost::format("R = ((%.2f, %.2f), (%.2f, %.2f))")
            % model::observation_error_covariance(params)(0, 0)
            % model::observation_error_covariance(params)(0, 1)
            % model::observation_error_covariance(params)(1, 0)
            % model::observation_error_covariance(params)(1, 1)
    ;
    return para_ss.str();
}

/*
data_pair generate_partition(const model::parameters& params, partition& dataset_partition, bool shutup) {

    int image_size = sampling::uniform_int(14, 17);
    int time_extent = sampling::uniform_int(20, 30);
    simulate::generate_partition(dataset_partition, params, 0, time_extent, image_size, image_size);

    if (not shutup) {
        diag("Generated %zu new tracks.", dataset_partition.tracks().size());
        diag("Generated %zu clutter observations.", dataset_partition.clutter().size());
    }

    return std::make_pair(image_size, time_extent);
}
*/

std::string partition_properties(const partition &const_partition) {
    const track_collection &tracks(const_partition.tracks());
    size_t num_tracks = tracks.size();
    track_collection::const_iterator tr_iter;
    float avg_track_duration = 0.0f;
    float avg_track_size = 0.0f;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        avg_track_duration += float((*tr_iter)->duration());
        avg_track_size += float((*tr_iter)->size());
    }
    avg_track_duration /= float(num_tracks);
    avg_track_size /= float(num_tracks);
    float o_obs = avg_track_size/avg_track_duration;
    float b_obs = float(num_tracks)/float(const_partition.duration());
    float c_obs = float(const_partition.clutter().size())/float(const_partition.duration());
    float s_obs = 1.f - 1.f/avg_track_duration;
    std::stringstream pp_ss;
    pp_ss << boost::format("num.tracks = %d, avg.length = %.1f, avg.obs.count = %.1f, "
        "observed: b = %.2f, c = %.2f, o = %.2f, s = %.2f")
        % num_tracks % avg_track_duration % avg_track_size % b_obs % c_obs % o_obs % s_obs
    ;
    return pp_ss.str();
}

int plot_tracks(const track_collection &tracks, const model::parameters &const_parameters) {
    track_collection::const_iterator tr_iter;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        const track & track0(**tr_iter);
        shared_const_kalman_filter_ptr kf = track0.make_kalman_filter(const_parameters);
        float log_posterior = track0.log_posterior(const_parameters);
        const kalman_filter::states_and_cov_deque & sncs(kf->corrections());
        const observation_collection &oc(track0.observations());
        std::deque <float> diffs;
        time_stamp ts=track0.first_time_stamp();
        std::stringstream diff_ss;
        std::stringstream cov_ss;
        std::stringstream track_ss;
        diff_ss <<  "| ";
        track_ss << "| ";
        track_ss << std::string(ts, ' ');
        diff_ss << std::string(ts, ' ');
        cov_ss   << "log(cov) = ";
        for (size_t i=0; i < sncs.size(); ++i, ++ts) {
            kalman_filter::state_vector state = sncs[i].first;
            kalman_filter::covariance_matrix cov = sncs[i].second;
            observation_collection::const_range obs_at_t(oc.at_time_stamp(ts));
            cov_ss << boost::format(" %.1f") % log10f(cov.determinant());
            if (obs_at_t.first != obs_at_t.second) {
                observation obs = *obs_at_t.first;
                float diff = hypotf(x(obs)-state[0], y(obs)-state[2]);
                diff_ss << boost::format("%d") % int(diff);
                diffs.push_back(diff);
                track_ss << "O";
            }
            else {
               diff_ss << "_";
               track_ss << "_";
            }
        }
        diag("log(PDF) = %8.2f %s", log_posterior, track_ss.str().c_str());
        diag("                    %s", diff_ss.str().c_str());
        diag("%s", cov_ss.str().c_str());
        float max_diff = *std::max_element(diffs.begin(), diffs.end());
        float radius = track0.radius();
        float max_dev = max_deviation_from_line(oc);
        if (max_diff >= max_dev or max_dev >= radius) {
            //diag("max diff = %.2f, dev from line = %.2f, radius = %.2f", max_diff, max_dev, radius);
            //diag("%s", diff_ss.str().c_str());
            //diag("length = %ld, observations = %.2f%%", track0.duration(), 100.f*float(track0.size())/float(track0.duration()));
        }
    }
    return 0;
}

int plot_states_of_tracks(const track_collection &tracks, const model::parameters &const_parameters) {
    track_collection::const_iterator tr_iter;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        const track& track0(**tr_iter);
        shared_const_kalman_filter_ptr kf = track0.make_kalman_filter(const_parameters);
        diag("track [%ld, %ld]", track0.first_time_stamp(), track0.last_time_stamp());
        print_kalman_filter_states(kf->corrections());
    }
    return 0;
}

int test2() {
    observation_collection t1_obs;
    //observation_collection cl_obs;
    t1_obs.insert(new_obs(0, 0, 0));
    t1_obs.insert(new_obs(0, 0, 1));
    t1_obs.insert(new_obs(0, 0, 2));
    t1_obs.insert(new_obs(0, 0, 3));
    track track1(0, 5, t1_obs.begin(), t1_obs.end(), 1.0);

    /*
    cl_obs.insert(new_obs(0, 0, 4));
    cl_obs.insert(new_obs(1, 1, 5));
    cl_obs.insert(new_obs(1, 1, 6));
    cl_obs.insert(new_obs(1, 1, 7));
    cl_obs.insert(new_obs(1, 1, 8));
    cl_obs.insert(new_obs(1, 1, 9));
    boost::shared_ptr<track_collection> tracks(new track_collection());
    tracks->insert(track1);
    boost::shared_ptr<const observation_collection> clutter(new observation_collection(cl_obs.begin(), cl_obs.end()));
    const partition original_partition(tracks, clutter);
    */

    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 2.f;
    model::mean_false_observations_per_frame(params)   = 1.f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.9f;
    model::constraint_radius(params) = 100.0f;
    model::observation_error_covariance(params) << 0.001f, 0.0f, 0.0f, 0.001f;

    print_kalman_filter_states( track1.make_kalman_filter(params)->corrections() );
    diag("forward:");
    print_kalman_filter_states( track1.make_kalman_filter(params)->predictions() );
    diag("backward");
    print_kalman_filter_states( generate_rotating_track(20)->make_kalman_filter(params)->corrections() );
    diag("forward");
    print_kalman_filter_states( generate_rotating_track(20)->make_kalman_filter(params)->predictions() );
    diag("backward linear:");
    print_kalman_filter_states( generate_linear_track(40, 1.f, 0.f)->make_kalman_filter(params)->corrections() );
    diag("forward linear:");
    print_kalman_filter_states( generate_linear_track(40, 1.f, 0.f)->make_kalman_filter(params)->predictions() );
    return 0;
}

int test3() {
    const float loc_err(.01f);

    std::deque<float> sing_pdf;
    for (int i=0; i<1; ++i) {
        boost::shared_ptr<track_collection> tracks(new track_collection());
        tracks->insert(*generate_linear_track(0, 50, 0.f, 0.f, 1.f, 0.f, loc_err));
        clutter_ptr clutter(new clutter_t());
        const partition original_partition(tracks, clutter);
        model::parameters params;
        float log_pdf = find_good_parameters(original_partition, params, 1000);
        model::observation_error_covariance(params) << loc_err * loc_err, 0, 0, loc_err * loc_err;
        diag("%s, log(PDF)=%.2f", print_parameters(params).c_str(), log_pdf);
        plot_tracks(*tracks, params);
        plot_states_of_tracks(*tracks, params);
        sing_pdf.push_back(log_pdf);
    }
    std::deque<float> quad_pdf;
    for (int i=0; i<1; ++i) {
        boost::shared_ptr<track_collection> tracks(new track_collection());
        int step = 5;
        for (int j=0; j < 50; j+= step)
            tracks->insert(*generate_linear_track(j, j+step, float(j), 0.f, 1.f, 0.f, loc_err));
        clutter_ptr clutter(new clutter_t());
        const partition original_partition(tracks, clutter);
        model::parameters params;
        float log_pdf = find_good_parameters(original_partition, params, 1000);
        model::observation_error_covariance(params) << loc_err * loc_err, 0, 0, loc_err * loc_err;
        diag("%s, log(PDF)=%.2f", print_parameters(params).c_str(), log_pdf);
        plot_tracks(*tracks, params);
        plot_states_of_tracks(*tracks, params);
        quad_pdf.push_back(log_pdf);
    }
    /*
    std::pair<float, float> sing_stat = mean_stdev(sing_pdf.begin(), sing_pdf.end());
    std::pair<float, float> quad_stat = mean_stdev(quad_pdf.begin(), quad_pdf.end());
    diag("sing %.2f +/- %.2f", sing_stat.first, sing_stat.second);
    diag("quad %.2f +/- %.2f", quad_stat.first, quad_stat.second);
    */
    return 0;
}

struct extract_state : public std::unary_function<kalman_filter::state_covariance_pair, state_t> {
    state_t operator()(const kalman_filter::state_covariance_pair &sc_pair) const {
        return sc_pair.first;
    }
};

struct vdist : public std::binary_function<state_t, state_t, float> {
    float operator()(const state_t &s1, const state_t &s2) { return sqrtf((s1-s2).transpose()*(s1-s2)); }
};

struct mahalanobis : public std::binary_function<kalman_filter::state_covariance_pair, state_t, float> {
    float operator()(const kalman_filter::state_covariance_pair &sc, const state_t &s) {
        return sqrtf( (sc.first-s).transpose() * sc.second.inverse() * (sc.first-s) );
    }
};

Eigen::Vector2f loc(const observation& o) { return Eigen::Vector2f(x(o), y(o)); }

struct maha_obs : public std::binary_function<observation, state_t, float> {
    matrix2f invR;
    Eigen::Matrix<float, 2, 4> B;
    maha_obs (const matrix2f &R) : invR(R.inverse()) {
        B << 1, 0, 0, 0,
             0, 0, 1, 0;
    }
    float operator()(const observation& o, const state_t& s) {
        Eigen::Vector2f diff(loc(o)-B*s);
        return sqrtf(diff.transpose()*invR*diff);
    }
};

void test6() {
    diag("Test 6:");
    model::parameters paras;
    model::observation_probability(paras) = 1.0f;
    const float mean_track_length = 100.f;
    model::survival_probability(paras) = 1.f - 1.f/mean_track_length;
    model::birth_rate(paras) = 0.5f;
    model::clutter_rate(paras) = 0.5f;
    model::constraint_radius(paras) = 1.0f;
    float loc_error = 0.3f;
    model::observation_error_covariance(paras) << loc_error * loc_error, 0, 0, loc_error * loc_error;
    model::process_noise_covariance(paras) = detail::initQ();
    float position_sigma = sqrt(model::process_noise_covariance(paras)(0,0));
    float velocity_sigma = sqrt(model::process_noise_covariance(paras)(1,1));

    const float thresh = sqrt(-2.f*logf(1.0-0.68));
    float total_len = 0.f;
    float dist_pred = 0.f;
    float dist_corr = 0.f;
    float dist_smth = 0.f;
    float lt1_pred = 0.f;
    float lt1_corr = 0.f;
    float lt1_smth = 0.f;
    float obs_stat = 0.f;
    float obs_pred = 0.f;
    float obs_corr = 0.f;
    float obs_smth = 0.f;
    maha_obs maha_dist(model::observation_error_covariance(paras));
    for (int loop = 0; loop < 1000; ++loop) {
        std::deque<state_t> states;
        shared_const_track_ptr syntrack = simulate::generate_track(states, paras, point_2d(), point_2d(), 0,
                                            position_sigma, velocity_sigma);

        boost::shared_ptr<kalman_filter> kf_p(new kalman_filter(*syntrack,
            model::observation_error_covariance(paras), model::process_noise_covariance(paras)));
        kalman_filter::states_and_cov_deque predictions = kf_p->predictions();
        kalman_filter::states_and_cov_deque corrections = kf_p->corrections();
        kalman_filter::states_and_cov_deque smoothed;
        rts_smooth(*kf_p, std::front_inserter(smoothed));
        std::deque<state_t> pred_states(predictions.size());
        std::deque<state_t> corr_states(corrections.size());
        std::deque<state_t> smth_states(smoothed.size());
        std::transform(predictions.begin(), predictions.end(), pred_states.begin(), extract_state());
        std::transform(corrections.begin(), corrections.end(), corr_states.begin(), extract_state());
        std::transform(smoothed.begin(), smoothed.end(), smth_states.begin(), extract_state());
        std::deque<float> dist_to_gt(states.size());
        std::transform(states.begin(), states.end(), pred_states.begin(), dist_to_gt.begin(), vdist());
        total_len += states.size();
        dist_pred += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
        std::transform(states.begin(), states.end(), corr_states.begin(), dist_to_gt.begin(), vdist());
        dist_corr += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
        std::transform(states.begin(), states.end(), smth_states.begin(), dist_to_gt.begin(), vdist());
        dist_smth += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
        std::transform(predictions.begin(), predictions.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
        lt1_pred += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
        std::transform(corrections.begin(), corrections.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
        lt1_corr += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
        std::transform(smoothed.begin(), smoothed.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
        lt1_smth += std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);

        std::transform(syntrack->begin(), syntrack->end(), states.begin(), dist_to_gt.begin(), maha_dist);
        obs_stat += count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
        std::transform(syntrack->begin(), syntrack->end(), pred_states.begin(), dist_to_gt.begin(), maha_dist);
        obs_pred += count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
        std::transform(syntrack->begin(), syntrack->end(), corr_states.begin(), dist_to_gt.begin(), maha_dist);
        obs_corr += count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
        std::transform(syntrack->begin(), syntrack->end(), smth_states.begin(), dist_to_gt.begin(), maha_dist);
        obs_smth += count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));

    }
    diag("average Euclidian GT-prediction %.2f", dist_pred/total_len);
    diag("average Euclidian GT-correction %.2f", dist_corr/total_len);
    diag("average Euclidian GT-smoothed %.2f", dist_smth/total_len);
    diag("--");
    diag("average Mahalanobis GT-prediction = %.2f", lt1_pred/total_len);
    diag("average Mahalanobis GT-correction = %.2f", lt1_corr/total_len);
    diag("average Mahalanobis GT-smooted    = %.2f", lt1_smth/total_len);
    diag("--");
    diag("Mahalanobis obs-GT         less than thresh = %.2f%%", 100.f*obs_stat/total_len);
    diag("Mahalanobis obs-prediction less than thresh = %.2f%%", 100.f*obs_pred/total_len);
    diag("Mahalanobis obs-correction less than thresh = %.2f%%", 100.f*obs_corr/total_len);
    diag("Mahalanobis obs-smoothed   less than thresh = %.2f%%", 100.f*obs_smth/total_len);
}

void test5() {
    diag("Test 5:");
    model::parameters paras;
    model::observation_probability(paras) = 1.0f;
    const float mean_track_length = 20.f;
    model::survival_probability(paras) = 1.f - 1.f/mean_track_length;
    model::birth_rate(paras) = 0.5f;
    model::clutter_rate(paras) = 0.5f;
    model::constraint_radius(paras) = 1.0f;
    float loc_error = 0.3f;
    model::observation_error_covariance(paras) << loc_error * loc_error, 0, 0, loc_error * loc_error;
    model::process_noise_covariance(paras) = detail::initQ();
    float position_sigma = sqrt(model::process_noise_covariance(paras)(0,0));
    float velocity_sigma = sqrt(model::process_noise_covariance(paras)(1,1));
    std::deque<state_t> states;
    shared_const_track_ptr syntrack = simulate::generate_track(states, paras, point_2d(), point_2d(), 0,
                                        position_sigma, velocity_sigma);

    boost::shared_ptr<kalman_filter> kf_p(new kalman_filter(*syntrack,
        model::observation_error_covariance(paras), model::process_noise_covariance(paras)));
    kalman_filter::states_and_cov_deque predictions = kf_p->predictions();
    kalman_filter::states_and_cov_deque corrections = kf_p->corrections();
    kalman_filter::states_and_cov_deque smoothed;
    rts_smooth(*kf_p, std::front_inserter(smoothed));
    std::deque<state_t> pred_states(predictions.size());
    std::deque<state_t> corr_states(corrections.size());
    std::deque<state_t> smth_states(smoothed.size());
    std::transform(predictions.begin(), predictions.end(), pred_states.begin(), extract_state());
    std::transform(corrections.begin(), corrections.end(), corr_states.begin(), extract_state());
    std::transform(smoothed.begin(), smoothed.end(), smth_states.begin(), extract_state());
    std::deque<float> dist_to_gt(states.size());
    diag("num of GT states = %zu", states.size());
    diag("num of GT observations = %zu", syntrack->size());
    diag("num of predicted states = %zu", pred_states.size());
    diag("num of corrected states = %zu", corr_states.size());
    diag("num of smoothed states = %zu", smth_states.size());
    ok(states.size() == pred_states.size(), "predicted number of states == ground truth states");
    ok(states.size() == corr_states.size(), "corrected number of states == ground truth states");
    ok(states.size() == smth_states.size(), " smoothed number of states == ground truth states");
    diag("--");
    BOOST_ASSERT(states.size() == pred_states.size());
    std::transform(states.begin(), states.end(), pred_states.begin(), dist_to_gt.begin(), vdist());
    float len(states.size());
    float sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("residual GT-prediction %.2f", sum/len);
    std::transform(states.begin(), states.end(), corr_states.begin(), dist_to_gt.begin(), vdist());
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("residual GT-correction %.2f", sum/len);
    std::transform(states.begin(), states.end(), smth_states.begin(), dist_to_gt.begin(), vdist());
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("residual GT-smoothed   %.2f", sum/len);
    float lt1;
    float thresh = sqrt(-2.f*logf(1.0-0.68));
    std::transform(predictions.begin(), predictions.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis GT-prediction %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    std::transform(corrections.begin(), corrections.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis GT-correction %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    std::transform(smoothed.begin(), smoothed.end(), states.begin(), dist_to_gt.begin(), mahalanobis());
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis GT-smoothed   %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    diag("--");
    //clutter_ptr clutter = clutter_ptr(new clutter_t());
    //mh_moves::shared_track_collection_ptr tracks = mh_moves::shared_track_collection_ptr(new track_collection());
    //tracks->insert(syntrack);
    //partition synpart(tracks, clutter);
    maha_obs maha_dist(model::observation_error_covariance(paras));
    std::transform(syntrack->begin(), syntrack->end(), states.begin(), dist_to_gt.begin(), maha_dist);
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis obs-GT %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    std::transform(syntrack->begin(), syntrack->end(), pred_states.begin(), dist_to_gt.begin(), maha_dist);
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis obs-predicted %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    std::transform(syntrack->begin(), syntrack->end(), corr_states.begin(), dist_to_gt.begin(), maha_dist);
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis obs-corrected %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);
    std::transform(syntrack->begin(), syntrack->end(), smth_states.begin(), dist_to_gt.begin(), maha_dist);
    lt1 = count_if(dist_to_gt.begin(), dist_to_gt.end(), std::bind2nd(std::less<float>(), thresh));
    sum = std::accumulate(dist_to_gt.begin(), dist_to_gt.end(), 0.f);
    diag("Mahalanobis obs-smoothed %.2f, less than %.2f = %.2f%%", sum/len, thresh, 100.f*lt1/len);

    //
    diag("--");
    diag("track length %zu, number of obs %zu", syntrack->duration(), syntrack->size());
    float pdf = log_track_given_parameters_density(syntrack, paras);
    diag("Q[0, 0] = %g, track likelihood %f", model::process_noise_covariance(paras)(0, 0), pdf);
    diag("--");
    float base = 1.8257418583505538f;
    for (int i = -6; i < 6; ++i) {
        model::process_noise_covariance(paras)(0, 0) = pow(base, i) * pow(base, i);
        model::process_noise_covariance(paras)(2, 2) = pow(base, i) * pow(base, i);
        pdf = log_track_given_parameters_density(syntrack, paras);
        diag("Q[0, 0] = %g, track likelihood %f", model::process_noise_covariance(paras)(0, 0), pdf);
    }
}

void test4() {
    general_paras gen_pars;
    gen_pars.total = 200000;
    gen_pars.seed = 0;
    gen_pars.rgp.lambda = 2.5;
    gen_pars.rgp.p_no = 0.2;
    gen_pars.rgp.p_yes = 0.2;
    gen_pars.rgp.p_tr = 0.5;
    gen_pars.rgp.min_tracks = 1;
    gen_pars.rgp.max_tracks = 2;
    partition_ptr_t part = random_debug_partition(gen_pars.rgp);
    model::parameters paras;
    generate_parameters(paras);
    /*
    std::cerr << "Q=" << std::endl;
    std::cerr << model::process_noise_covariance(paras) << std::endl;
    std::cerr << "initQ=" << std::endl;
    std::cerr << detail::initQ() << std::endl;
    */
    //find_good_parameters(*part, paras, 1000);
    diag("track likelihood %f", model::log_tracks_given_parameters_density(*part, paras));
    diag("track likelihood %f", model::log_tracks_given_parameters_density(*part, paras));
    partition_ptr_t final_part;
    float pdr;
    bool success = false;
    int attempts = 0;
    capability_recorder_ptr cap_rec_ptr(new_cap_recorder(*part));
    part->set_capability_recorder(cap_rec_ptr);
    while (not success and attempts++ < 50) {
        success = mh_moves::update(part, final_part, pdr);
    }
    diag("success: %d, attempts: %d", success, attempts);
    if (success)
        diag("track likelihood %f", model::log_tracks_given_parameters_density(*final_part, paras));
}

int main(int argc, char** argv) {
    // seed the PRNG to ensure consistent results across runs.
    general_paras gen_pars;
    gen_pars.total = 200000;
    gen_pars.seed = 0;
    gen_pars.rgp.lambda = 2.5;
    gen_pars.rgp.p_no = 0.2;
    gen_pars.rgp.p_yes = 0.2;
    gen_pars.rgp.p_tr = 0.5;
    gen_pars.rgp.min_tracks = 1;
    gen_pars.rgp.max_tracks = 2;
    if (not parse_args(argc, argv, gen_pars))
        return exit_status();
    biggles::detail::seed_prng(gen_pars.seed);
    diag("random seed = 0x%x", gen_pars.seed);
    plan_no_plan();

    //test2();
    //test3();
    test4();
    test5();
    test6();
    //diag("dev-from-line-test %.4f", max_deviation_from_line(generate_linear_track(10, 1.f, 0.f)->observations()));
    ok1(true);
    return exit_status();
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// 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)
#include <boost/yap/expression.hpp>

#include <boost/test/minimal.hpp>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

template<typename T>
using ref = boost::yap::expression_ref<boost::yap::expression, T>;

namespace yap = boost::yap;
namespace bh = boost::hana;

namespace user {

    struct number
    {
        double value;

        friend number operator+(number lhs, number rhs)
        {
            return number{lhs.value + rhs.value};
        }

        friend number operator-(number lhs, number rhs)
        {
            return number{lhs.value - rhs.value};
        }

        friend number operator*(number lhs, number rhs)
        {
            return number{lhs.value * rhs.value};
        }

        friend number operator-(number n) { return number{-n.value}; }

        friend bool operator<(number lhs, double rhs)
        {
            return lhs.value < rhs;
        }

        friend bool operator<(double lhs, number rhs)
        {
            return lhs < rhs.value;
        }
    };

    number naxpy(number a, number x, number y)
    {
        return number{a.value * x.value + y.value + 10.0};
    }

    struct empty_xform
    {};

    struct eval_xform_tag
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return n;
        }
    };

    struct eval_xform_expr
    {
        decltype(auto) operator()(term<user::number> const & expr)
        {
            return ::boost::yap::value(expr);
        }
    };

    struct eval_xform_both
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return n;
        }

        decltype(auto) operator()(term<user::number> const & expr)
        {
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return ::boost::yap::value(expr);
        }
    };

    struct plus_to_minus_xform_tag
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            user::number const & lhs,
            user::number const & rhs)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                term<user::number>{lhs}, term<user::number>{rhs});
        }
    };

    struct plus_to_minus_xform_expr
    {
        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                ::boost::yap::left(expr), ::boost::yap::right(expr));
        }
    };

    struct plus_to_minus_xform_both
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            user::number const & lhs,
            user::number const & rhs)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                term<user::number>{lhs}, term<user::number>{rhs});
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return yap::make_expression<yap::expr_kind::minus>(
                ::boost::yap::left(expr), ::boost::yap::right(expr));
        }
    };

    struct term_nonterm_xform_tag
    {
        auto operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return yap::make_terminal(n * user::number{2.0});
        }

        auto operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            user::number const & lhs,
            user::number const & rhs)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                yap::transform(::boost::yap::make_terminal(lhs), *this),
                yap::transform(::boost::yap::make_terminal(rhs), *this));
        }
    };

    struct term_nonterm_xform_expr
    {
        decltype(auto) operator()(term<user::number> const & expr)
        {
            return yap::make_terminal(
                ::boost::yap::value(expr) * user::number{2.0});
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                yap::transform(::boost::yap::left(expr), *this),
                yap::transform(::boost::yap::right(expr), *this));
        }
    };

    struct term_nonterm_xform_both
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return yap::make_terminal(n * user::number{2.0});
        }

        decltype(auto) operator()(term<user::number> const & expr)
        {
            return yap::make_terminal(
                ::boost::yap::value(expr) * user::number{2.0});
        }

        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            user::number const & lhs,
            user::number const & rhs)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                yap::transform(::boost::yap::make_terminal(lhs), *this),
                yap::transform(::boost::yap::make_terminal(rhs), *this));
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            return yap::make_expression<yap::expr_kind::minus>(
                yap::transform(::boost::yap::left(expr), *this),
                yap::transform(::boost::yap::right(expr), *this));
        }
    };

    struct eval_term_nonterm_xform_tag
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return n * user::number{2.0};
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            Expr1 const & lhs,
            Expr2 const & rhs)
        {
            return boost::yap::transform(::boost::yap::as_expr(lhs), *this) -
                   boost::yap::transform(::boost::yap::as_expr(rhs), *this);
        }
    };

    struct eval_term_nonterm_xform_expr
    {
        decltype(auto) operator()(term<user::number> const & expr)
        {
            return ::boost::yap::value(expr) * user::number{2.0};
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            return boost::yap::transform(::boost::yap::left(expr), *this) -
                   boost::yap::transform(::boost::yap::right(expr), *this);
        }
    };

    struct eval_term_nonterm_xform_both
    {
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::terminal>, user::number const & n)
        {
            return n * user::number{2.0};
        }

        decltype(auto) operator()(term<user::number> const & expr)
        {
            return ::boost::yap::value(expr) * user::number{2.0};
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::plus>,
            Expr1 const & lhs,
            Expr2 const & rhs)
        {
            return boost::yap::transform(::boost::yap::as_expr(lhs), *this) -
                   boost::yap::transform(::boost::yap::as_expr(rhs), *this);
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            return boost::yap::transform(::boost::yap::left(expr), *this) -
                   boost::yap::transform(::boost::yap::right(expr), *this);
        }
    };

    decltype(auto)
    naxpy_eager_nontemplate_xform(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<
                                      yap::expression<
                                          yap::expr_kind::multiplies,
                                          bh::tuple<
                                              ref<term<user::number> &>,
                                              ref<term<user::number> &>>>,
                                      ref<term<user::number> &>>> const & expr)
    {
        auto a = evaluate(expr.left().left());
        auto x = evaluate(expr.left().right());
        auto y = evaluate(expr.right());
        return yap::make_terminal(naxpy(a, x, y));
    }

    decltype(auto)
    naxpy_lazy_nontemplate_xform(yap::expression<
                                 yap::expr_kind::plus,
                                 bh::tuple<
                                     yap::expression<
                                         yap::expr_kind::multiplies,
                                         bh::tuple<
                                             ref<term<user::number> &>,
                                             ref<term<user::number> &>>>,
                                     ref<term<user::number> &>>> const & expr)
    {
        decltype(auto) a = expr.left().left().value();
        decltype(auto) x = expr.left().right().value();
        decltype(auto) y = expr.right().value();
        return yap::make_terminal(naxpy)(a, x, y);
    }

    struct naxpy_xform
    {
        template<typename Expr1, typename Expr2, typename Expr3>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<
                                      yap::expression<
                                          yap::expr_kind::multiplies,
                                          bh::tuple<Expr1, Expr2>>,
                                      Expr3>> const & expr)
        {
            return yap::make_terminal(naxpy)(
                transform(expr.left().left(), naxpy_xform{}),
                transform(expr.left().right(), naxpy_xform{}),
                transform(expr.right(), naxpy_xform{}));
        }
    };

    // unary transforms

    struct disable_negate_xform_tag
    {
        auto
        operator()(yap::expr_tag<yap::expr_kind::negate>, user::number value)
        {
            return yap::make_terminal(std::move(value));
        }

        template<typename Expr>
        auto
        operator()(yap::expr_tag<yap::expr_kind::negate>, Expr const & expr)
        {
            return expr;
        }
    };

    struct disable_negate_xform_expr
    {
        template<typename Expr>
        decltype(auto) operator()(
            yap::expression<yap::expr_kind::negate, bh::tuple<Expr>> const &
                expr)
        {
            return ::boost::yap::value(expr);
        }
    };

    struct disable_negate_xform_both
    {
        decltype(auto)
        operator()(yap::expr_tag<yap::expr_kind::negate>, user::number value)
        {
            return yap::make_terminal(std::move(value));
        }

        template<typename Expr>
        decltype(auto)
        operator()(yap::expr_tag<yap::expr_kind::negate>, Expr const & expr)
        {
            return expr;
        }

        template<typename Expr>
        decltype(auto) operator()(
            yap::expression<yap::expr_kind::negate, bh::tuple<Expr>> const &
                expr)
        {
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return ::boost::yap::value(expr);
        }
    };

    // ternary transforms

    //[ tag_xform
    struct ternary_to_else_xform_tag
    {
        template<typename Expr>
        decltype(auto) operator()(
            boost::yap::expr_tag<yap::expr_kind::if_else>,
            Expr const & cond,
            user::number then,
            user::number else_)
        {
            return boost::yap::make_terminal(std::move(else_));
        }
    };
    //]

    //[ expr_xform
    struct ternary_to_else_xform_expr
    {
        template<typename Cond, typename Then, typename Else>
        decltype(auto)
        operator()(boost::yap::expression<
                   boost::yap::expr_kind::if_else,
                   boost::hana::tuple<Cond, Then, Else>> const & expr)
        {
            return ::boost::yap::else_(expr);
        }
    };
    //]

    struct ternary_to_else_xform_both
    {
        template<typename Expr>
        decltype(auto) operator()(
            yap::expr_tag<yap::expr_kind::if_else>,
            Expr const & cond,
            user::number then,
            user::number else_)
        {
            return yap::make_terminal(std::move(else_));
        }

        template<typename Cond, typename Then, typename Else>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::if_else,
                                  bh::tuple<Cond, Then, Else>> const & expr)
        {
            throw std::logic_error("Oops!  Picked the wrong overload!");
            return ::boost::yap::else_(expr);
        }
    };
}

auto double_to_float(term<double> expr)
{
    return term<float>{(float)expr.value()};
}

auto check_unique_ptrs_equal_7(term<std::unique_ptr<int>> && expr)
{
    using namespace boost::hana::literals;
    BOOST_CHECK(*expr.elements[0_c] == 7);
    return std::move(expr);
}

int test_main(int, char * [])
{
    {
        term<user::number> a{{1.0}};
        term<user::number> x{{42.0}};
        term<user::number> y{{3.0}};

        {
            auto expr = a;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 1);
            }

            {
                auto transformed_expr = transform(expr, user::empty_xform{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 1);
            }

            {
                auto transformed_expr = transform(expr, user::eval_xform_tag{});
                BOOST_CHECK(transformed_expr.value == 1);
            }

            {
                auto transformed_expr =
                    transform(expr, user::eval_xform_expr{});
                BOOST_CHECK(transformed_expr.value == 1);
            }

            {
                auto transformed_expr =
                    transform(expr, user::eval_xform_both{});
                BOOST_CHECK(transformed_expr.value == 1);
            }
        }

        {
            auto expr = x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::plus_to_minus_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39);
            }

            {
                auto transformed_expr =
                    transform(expr, user::plus_to_minus_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39);
            }

            {
                auto transformed_expr =
                    transform(expr, user::plus_to_minus_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39);
            }
        }

        {
            auto expr = x + user::number{3.0};
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }
        }

        {
            auto expr = x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 39 * 2);
            }
        }

        {
            auto expr = (x + y) + user::number{1.0};
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 46);
            }

            {
                // Differs from those below, because it matches terminals, not
                // expressions.
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 40 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 38 * 2);
            }

            {
                auto transformed_expr =
                    transform(expr, user::term_nonterm_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 38 * 2);
            }
        }

        {
            auto expr = x + user::number{3.0};
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_tag{});
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_expr{});
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_both{});
                BOOST_CHECK(result.value == 39 * 2);
            }
        }

        {
            auto expr = x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_tag{});
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_expr{});
                BOOST_CHECK(result.value == 39 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_both{});
                BOOST_CHECK(result.value == 39 * 2);
            }
        }

        {
            auto expr = (x + y) + user::number{1.0};
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 46);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_tag{});
                BOOST_CHECK(result.value == 38 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_expr{});
                BOOST_CHECK(result.value == 38 * 2);
            }

            {
                user::number result =
                    transform(expr, user::eval_term_nonterm_xform_both{});
                BOOST_CHECK(result.value == 38 * 2);
            }
        }

        {
            auto expr = a * x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            auto transformed_expr =
                transform(expr, user::naxpy_eager_nontemplate_xform);
            {
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 55);
            }
        }

        {
            auto expr = a + (a * x + y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 46);
            }

            auto transformed_expr =
                transform(expr, user::naxpy_eager_nontemplate_xform);
            {
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 56);
            }
        }

        {
            auto expr = a * x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45);
            }

            auto transformed_expr =
                transform(expr, user::naxpy_lazy_nontemplate_xform);
            {
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 55);
            }
        }

        {
            auto expr = a + (a * x + y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 46);
            }

            auto transformed_expr =
                transform(expr, user::naxpy_lazy_nontemplate_xform);
            {
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 56);
            }
        }

        {
            auto expr = (a * x + y) * (a * x + y) + (a * x + y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 45 * 45 + 45);
            }

            auto transformed_expr = transform(expr, user::naxpy_xform{});
            {
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 55 * 55 + 55 + 10);
            }
        }
    }

    {
        term<double> unity{1.0};
        term<std::unique_ptr<int>> i{new int{7}};
        yap::expression<
            yap::expr_kind::plus,
            bh::tuple<ref<term<double> &>, term<std::unique_ptr<int>>>>
            expr_1 = unity + std::move(i);

        yap::expression<
            yap::expr_kind::plus,
            bh::tuple<
                ref<term<double> &>,
                yap::expression<
                    yap::expr_kind::plus,
                    bh::tuple<
                        ref<term<double> &>,
                        term<std::unique_ptr<int>>>>>>
            expr_2 = unity + std::move(expr_1);

        auto transformed_expr = transform(std::move(expr_2), double_to_float);

        transform(std::move(transformed_expr), check_unique_ptrs_equal_7);
    }

    {
        term<user::number> a{{1.0}};
        term<user::number> x{{42.0}};
        term<user::number> y{{3.0}};

        {
            auto expr = -x;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == -42);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 42);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 42);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 42);
            }
        }

        {
            auto expr = a * -x + y;
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == -39);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }
        }

        {
            auto expr = -(x + y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == -45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }

            {
                auto transformed_expr =
                    transform(expr, user::disable_negate_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 45);
            }
        }
    }

    {
        term<user::number> a{{1.0}};
        term<user::number> x{{42.0}};
        term<user::number> y{{3.0}};

        {
            auto expr = if_else(0 < a, x, y);
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 42);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 3);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 3);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 3);
            }
        }

        {
            auto expr = y * if_else(0 < a, x, y) + user::number{0.0};
            {
                user::number result = evaluate(expr);
                BOOST_CHECK(result.value == 126);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_tag{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 9);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_expr{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 9);
            }

            {
                auto transformed_expr =
                    transform(expr, user::ternary_to_else_xform_both{});
                user::number result = evaluate(transformed_expr);
                BOOST_CHECK(result.value == 9);
            }
        }
    }

    return 0;
}


#include "engine.hpp"
#include "modules/experiment/experiment.hpp"
#include "modules/solver/learner/gaussianProcess/gaussianProcess.hpp"
#include "sample/sample.hpp"

#include <Eigen/Dense>

namespace korali
{
namespace solver
{
namespace learner
{
;

/**
 * @brief Converts a vector of floats to Eigen format
 * @param v the vector to convert
 * @return An Eigen vector type
 */
Eigen::VectorXd
toEigen(const std::vector<float> &v)
{
  Eigen::VectorXd ev(v.size());
  for (size_t i = 0; i < v.size(); ++i)
    ev[i] = v[i];
  return ev;
}

/**
 * @brief Model function to evaluate the error function of the GP
 * @param sample The sample containing the proposal parameters
 * @param gp Pointer to the GP
 */
void runSample(Sample &sample, libgp::GaussianProcess *gp)
{
  size_t gpParameterDimension = gp->covf().get_param_dim();
  const Eigen::VectorXd p = toEigen(sample["Parameters"].get<std::vector<float>>());

  gp->covf().set_loghyper(p);

  sample["F(x)"] = gp->log_likelihood();
  sample["logP(x)"] = sample["F(x)"];

  Eigen::VectorXd eigenGrad = gp->log_likelihood_gradient();
  for (size_t i = 0; i < gpParameterDimension; i++)
    sample["Gradient"][i] = eigenGrad[i];
}

void GaussianProcess::initialize()
{
  _problem = dynamic_cast<problem::SupervisedLearning *>(_k->_problem);

  if (_problem->_maxTimesteps > 1) KORALI_LOG_ERROR("Training data cannot be time-dependent.");
  if (_problem->_trainingBatchSize == 0) KORALI_LOG_ERROR("Training data has not been provided for variable 0.");
  if (_problem->_solutionSize > 1) KORALI_LOG_ERROR("The solution space should be one dimensional.");

  // Checking that incoming data has a correct format
  _problem->verifyData();

  _gpInputDimension = _problem->_inputSize;
  _gp = std::make_unique<libgp::GaussianProcess>(_gpInputDimension, _covarianceFunction);

  _gpParameterDimension = _gp->covf().get_param_dim();

  // Creating evaluation lambda function for optimization
  auto evaluateProposal = [gp = _gp.get()](Sample &sample)
  {
    runSample(sample, gp);
  };

  _koraliExperiment["Problem"]["Type"] = "Optimization";
  _koraliExperiment["Problem"]["Objective Function"] = evaluateProposal;

  Eigen::VectorXd eParameters(_gpParameterDimension);

  for (size_t i = 0; i < _gpParameterDimension; i++)
  {
    _koraliExperiment["Variables"][i]["Name"] = "X" + std::to_string(i);
    eParameters[i] = _defaultHyperparameter;
    _koraliExperiment["Variables"][i]["Initial Value"] = eParameters[i];
  }
  _gp->covf().set_loghyper(eParameters);

  _koraliExperiment["Solver"] = _optimizer;
  _koraliExperiment["Solver"]["Termination Criteria"]["Max Generations"] = 1;

  _koraliExperiment["File Output"]["Frequency"] = 0;
  _koraliExperiment["File Output"]["Enabled"] = false;
  _koraliExperiment["Console Output"]["Frequency"] = 0;
  _koraliExperiment["Console Output"]["Verbosity"] = "Silent";
  _koraliExperiment["Random Seed"] = _k->_randomSeed++;

  // Pass the training data from korali to the GP library
  double inData[_gpInputDimension];
  double outData;

  // Running initialization to verify that the configuration is correct
  _koraliExperiment.initialize();

  for (size_t i = 0; i < _problem->_trainingBatchSize; i++)
  {
    for (size_t j = 0; j < _gpInputDimension; j++)
      inData[j] = _problem->_inputData[i][0][j];

    outData = _problem->_solutionData[i][0];
    _gp->add_pattern(inData, outData);
  }
}

void GaussianProcess::runGeneration()
{
  _koraliExperiment["Solver"]["Termination Criteria"]["Max Generations"] = _koraliExperiment._currentGeneration + 1;
  korali::Engine engine;
  engine.run(_koraliExperiment);
  _gpHyperparameters = _koraliExperiment["Results"]["Best Sample"]["Parameters"].get<std::vector<float>>();
}

void GaussianProcess::printGenerationAfter()
{
  return;
}

std::vector<std::vector<float>> &GaussianProcess::getEvaluation(const std::vector<std::vector<std::vector<float>>> &input)
{
  _outputValues.resize(1);
  _outputValues[0].resize(2);

  if (input.size() > 1) KORALI_LOG_ERROR("Gaussian Process does not support multi-timestep evaluation.\n");
  if (input[0].size() > 1) KORALI_LOG_ERROR("Gaussian Process does not support minibatch evaluation.\n");

  _gp->covf().set_loghyper(toEigen(_gpHyperparameters));

  std::vector<double> inputData(input[0][0].begin(), input[0][0].end());

  _outputValues[0][0] = _gp->f(inputData.data());
  _outputValues[0][1] = _gp->var(inputData.data());

  return _outputValues;
}

std::vector<float> GaussianProcess::getHyperparameters()
{
  return _gpHyperparameters;
}

void GaussianProcess::setHyperparameters(const std::vector<float> &hyperparameters)
{
  _gpHyperparameters = hyperparameters;
}

void GaussianProcess::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 if (isDefined(js, "gp Input Dimension"))
 {
 try { _gpInputDimension = js["gp Input Dimension"].get<size_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['gp Input Dimension']\n%s", e.what()); }
   eraseValue(js, "gp Input Dimension");
 }

 if (isDefined(js, "gp Parameter Dimension"))
 {
 try { _gpParameterDimension = js["gp Parameter Dimension"].get<size_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['gp Parameter Dimension']\n%s", e.what()); }
   eraseValue(js, "gp Parameter Dimension");
 }

 if (isDefined(js, "gp Hyperparameters"))
 {
 try { _gpHyperparameters = js["gp Hyperparameters"].get<std::vector<float>>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['gp Hyperparameters']\n%s", e.what()); }
   eraseValue(js, "gp Hyperparameters");
 }

 if (isDefined(js, "Covariance Function"))
 {
 try { _covarianceFunction = js["Covariance Function"].get<std::string>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['Covariance Function']\n%s", e.what()); }
   eraseValue(js, "Covariance Function");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Covariance Function'] required by gaussianProcess.\n");

 if (isDefined(js, "Default Hyperparameter"))
 {
 try { _defaultHyperparameter = js["Default Hyperparameter"].get<float>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['Default Hyperparameter']\n%s", e.what()); }
   eraseValue(js, "Default Hyperparameter");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Default Hyperparameter'] required by gaussianProcess.\n");

 if (isDefined(js, "Optimizer"))
 {
 _optimizer = js["Optimizer"].get<knlohmann::json>();

   eraseValue(js, "Optimizer");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Optimizer'] required by gaussianProcess.\n");

 if (isDefined(js, "Termination Criteria", "Terminate With Optimizer"))
 {
 try { _terminateWithOptimizer = js["Termination Criteria"]["Terminate With Optimizer"].get<int>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ gaussianProcess ] \n + Key:    ['Termination Criteria']['Terminate With Optimizer']\n%s", e.what()); }
   eraseValue(js, "Termination Criteria", "Terminate With Optimizer");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Termination Criteria']['Terminate With Optimizer'] required by gaussianProcess.\n");

 if (isDefined(_k->_js.getJson(), "Variables"))
 for (size_t i = 0; i < _k->_js["Variables"].size(); i++) {
 }
 Learner::setConfiguration(js);
 _type = "learner/gaussianProcess";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: gaussianProcess: \n%s\n", js.dump(2).c_str());
}

void GaussianProcess::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
   js["Covariance Function"] = _covarianceFunction;
   js["Default Hyperparameter"] = _defaultHyperparameter;
   js["Optimizer"] = _optimizer;
   js["Termination Criteria"]["Terminate With Optimizer"] = _terminateWithOptimizer;
   js["gp Input Dimension"] = _gpInputDimension;
   js["gp Parameter Dimension"] = _gpParameterDimension;
   js["gp Hyperparameters"] = _gpHyperparameters;
 for (size_t i = 0; i <  _k->_variables.size(); i++) {
 }
 Learner::getConfiguration(js);
}

void GaussianProcess::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{\"Default Hyperparameter\": 0.1, \"Termination Criteria\": {\"Terminate With Optimizer\": true}}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Learner::applyModuleDefaults(js);
}

void GaussianProcess::applyVariableDefaults()
{

 std::string defaultString = "{}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 if (isDefined(_k->_js.getJson(), "Variables"))
  for (size_t i = 0; i < _k->_js["Variables"].size(); i++)
   mergeJson(_k->_js["Variables"][i], defaultJs);
 Learner::applyVariableDefaults();
}

bool GaussianProcess::checkTermination()
{
 bool hasFinished = false;

 if (_terminateWithOptimizer && _koraliExperiment._solver->checkTermination())
 {
  _terminationCriteria.push_back("gaussianProcess['Terminate With Optimizer'] = " + std::to_string(_terminateWithOptimizer) + ".");
  hasFinished = true;
 }

 hasFinished = hasFinished || Learner::checkTermination();
 return hasFinished;
}

;

} //learner
} //solver
} //korali
;