//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/opencv.hpp>
//#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui_c.h>

#include <boost/gil/gil_all.hpp>

#include <string>

//using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
   //namedWindow("Before" , CV_WINDOW_AUTOSIZE);

   // Load the source image
   //Mat src = cv::imread( "Image0.jpg", 0 );

   // Create a destination Mat object
   //Mat dst;

   // display the source image
   //imshow("Before", src);

   //for (int i=1; i<51; i=i+2)
   {
      //smooth the image in the "src" and save it to "dst"
      //blur(src, dst, Size(i,i));

     // GaussianBlur( src, dst, Size( i, i ), 0, 0 );

      //show the blurred image with the text
      //imshow( "Smoothing by avaraging", dst );

      //wait for 3 seconds
      //waitKey(3000);
   }
}


/*
 * Copyright 2020 Robert Bosch GmbH
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
/**
 * \file cloe/utility/geometry.hpp
 */

#pragma once
#ifndef CLOE_UTILITY_GEOMETRY_HPP_
#define CLOE_UTILITY_GEOMETRY_HPP_

#include <Eigen/Geometry>

namespace cloe {
namespace utility {

/**
 * QuaternionFromRPY calculates a quaternion from roll, pitch and yaw.
 */
inline Eigen::Quaterniond quaternion_from_rpy(double roll, double pitch, double yaw) {
  // ZYX body flxed rotations
  Eigen::Quaterniond qt = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
                          Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
  return qt;
}

/**
 * PoseFromRotationTranslation calculates the pose from rotation and translation.
 */
inline Eigen::Isometry3d pose_from_rotation_translation(const Eigen::Quaterniond& quaternion,
                                                        const Eigen::Vector3d& trans) {
  Eigen::Isometry3d pose;
  pose.setIdentity();
  pose.linear() = quaternion.matrix();
  pose.translation() = trans;
  return pose;
}

/**
 * Change a point's frame of reference.
 * Both the point and the child frame need to have the same parent frame.
 */
inline void transform_to_child_frame(const Eigen::Isometry3d& child_frame, Eigen::Vector3d* point) {
  *point = child_frame.inverse() * (*point);
}

}  // namespace utility
}  // namespace cloe

#endif  // CLOE_UTILITY_GEOMETRY_HPP_


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/optional.hpp>
namespace hana = boost::hana;

auto odd = [](auto x) {
    return x % hana::int_c<2> != hana::int_c<0>;
};

BOOST_HANA_CONSTANT_CHECK(hana::find_if(hana::just(hana::int_c<3>), odd) == hana::just(hana::int_c<3>));
BOOST_HANA_CONSTANT_CHECK(hana::find_if(hana::just(hana::int_c<2>), odd) == hana::nothing);
BOOST_HANA_CONSTANT_CHECK(hana::find_if(hana::nothing, odd) == hana::nothing);

BOOST_HANA_CONSTANT_CHECK(hana::all_of(hana::just(hana::int_c<3>), odd));
BOOST_HANA_CONSTANT_CHECK(hana::all_of(hana::nothing, odd));

int main() { }


#include <dlib/data_io.h>
#include <dlib/dir_nav.h>
#include <dlib/dnn.h>
#include <dlib/image_processing.h>

namespace recognizer {

template <template <int, template <typename> class, int, typename> class block,
          int N, template <typename> class BN, typename SUBNET>
using residual = dlib::add_prev1<block<N, BN, 1, dlib::tag1<SUBNET>>>;

template <template <int, template <typename> class, int, typename> class block,
          int N, template <typename> class BN, typename SUBNET>
using residual_down = dlib::add_prev2<dlib::avg_pool<
    2, 2, 2, 2, dlib::skip1<dlib::tag2<block<N, BN, 2, dlib::tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block =
    BN<dlib::con<N, 3, 3, 1, 1,
                 dlib::relu<BN<dlib::con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET>
using ares = dlib::relu<residual<block, N, dlib::affine, SUBNET>>;

template <int N, typename SUBNET>
using ares_down = dlib::relu<residual_down<block, N, dlib::affine, SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256, SUBNET>;

template <typename SUBNET>
using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;

template <typename SUBNET>
using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;

template <typename SUBNET>
using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;

template <typename SUBNET> using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

using anet_type = dlib::loss_metric<dlib::fc_no_bias<
    128,
    dlib::avg_pool_everything<alevel0<alevel1<alevel2<alevel3<alevel4<
        dlib::max_pool<3, 3, 2, 2,
                       dlib::relu<dlib::affine<dlib::con<
                           32, 7, 7, 2, 2, dlib::input_rgb_image>>>>>>>>>>>>;

} // namespace recognizer

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

  // This tool expects dlib_face_recognition_resnet_model_v1.dat to be in the
  // same folder from where you would run it
  //
  // download the weights from here
  // http://dlib.net/files/dlib_face_recognition_resnet_model_v1.dat.bz2

  recognizer::anet_type net;

  // deserialize the weights
  dlib::deserialize("dlib_face_recognition_resnet_model_v1.dat") >> net;

  // serialize them as xml
  dlib::net_to_xml(net, "dlib_face_recognition_resnet_model_v1.xml");

  return 0;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "../TfLiteParser.hpp"
#include "ParserFlatbuffersFixture.hpp"
#include "ParserPrototxtFixture.hpp"
#include "ParserHelper.hpp"
#include "test/GraphUtils.hpp"

#include <armnn/utility/PolymorphicDowncast.hpp>
#include <QuantizeHelper.hpp>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct DetectionPostProcessFixture : ParserFlatbuffersFixture
{
    explicit DetectionPostProcessFixture(const std::string& custom_options)
    {
        /*
            The following values were used for the custom_options:
            use_regular_nms = true
            max_classes_per_detection = 1
            detections_per_class = 1
            nms_score_threshold = 0.0
            nms_iou_threshold = 0.5
            max_detections = 3
            max_detections = 3
            num_classes = 2
            h_scale = 5
            w_scale = 5
            x_scale = 10
            y_scale = 10
        */
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [{
                    "builtin_code": "CUSTOM",
                    "custom_code": "TFLite_Detection_PostProcess"
                }],
                "subgraphs": [{
                    "tensors": [{
                            "shape": [1, 6, 4],
                            "type": "UINT8",
                            "buffer": 0,
                            "name": "box_encodings",
                            "quantization": {
                                "min": [0.0],
                                "max": [255.0],
                                "scale": [1.0],
                                "zero_point": [ 1 ]
                            }
                        },
                        {
                            "shape": [1, 6, 3],
                            "type": "UINT8",
                            "buffer": 1,
                            "name": "scores",
                            "quantization": {
                                "min": [0.0],
                                "max": [255.0],
                                "scale": [0.01],
                                "zero_point": [0]
                            }
                        },
                        {
                            "shape": [6, 4],
                            "type": "UINT8",
                            "buffer": 2,
                            "name": "anchors",
                            "quantization": {
                                "min": [0.0],
                                "max": [255.0],
                                "scale": [0.5],
                                "zero_point": [0]
                            }
                        },
                        {
                            "type": "FLOAT32",
                            "buffer": 3,
                            "name": "detection_boxes",
                            "quantization": {}
                        },
                        {
                            "type": "FLOAT32",
                            "buffer": 4,
                            "name": "detection_classes",
                            "quantization": {}
                        },
                        {
                            "type": "FLOAT32",
                            "buffer": 5,
                            "name": "detection_scores",
                            "quantization": {}
                        },
                        {
                            "type": "FLOAT32",
                            "buffer": 6,
                            "name": "num_detections",
                            "quantization": {}
                        }
                    ],
                    "inputs": [0, 1, 2],
                    "outputs": [3, 4, 5, 6],
                    "operators": [{
                        "opcode_index": 0,
                        "inputs": [0, 1, 2],
                        "outputs": [3, 4, 5, 6],
                        "builtin_options_type": 0,
                        "custom_options": [)" + custom_options + R"(],
                        "custom_options_format": "FLEXBUFFERS"
                    }]
                }],
                "buffers": [{},
                    {},
                    { "data": [ 1, 1,   2, 2,
                                1, 1,   2, 2,
                                1, 1,   2, 2,
                                1, 21,  2, 2,
                                1, 21,  2, 2,
                                1, 201, 2, 2]},
                    {},
                    {},
                    {},
                    {},
                ]
            }
        )";
    }
};

struct ParseDetectionPostProcessCustomOptions : DetectionPostProcessFixture
{
private:
    static armnn::DetectionPostProcessDescriptor GenerateDescriptor()
    {
        static armnn::DetectionPostProcessDescriptor descriptor;
        descriptor.m_UseRegularNms          = true;
        descriptor.m_MaxDetections          = 3u;
        descriptor.m_MaxClassesPerDetection = 1u;
        descriptor.m_DetectionsPerClass     = 1u;
        descriptor.m_NumClasses             = 2u;
        descriptor.m_NmsScoreThreshold      = 0.0f;
        descriptor.m_NmsIouThreshold        = 0.5f;
        descriptor.m_ScaleH                 = 5.0f;
        descriptor.m_ScaleW                 = 5.0f;
        descriptor.m_ScaleX                 = 10.0f;
        descriptor.m_ScaleY                 = 10.0f;

        return descriptor;
    }

public:
    ParseDetectionPostProcessCustomOptions()
        : DetectionPostProcessFixture(
            GenerateDetectionPostProcessJsonString(GenerateDescriptor()))
    {}
};

BOOST_FIXTURE_TEST_CASE( ParseDetectionPostProcess, ParseDetectionPostProcessCustomOptions )
{
    Setup();

    // Inputs
    using UnquantizedContainer = std::vector<float>;
    UnquantizedContainer boxEncodings =
    {
        0.0f,  0.0f, 0.0f, 0.0f,
        0.0f,  1.0f, 0.0f, 0.0f,
        0.0f, -1.0f, 0.0f, 0.0f,
        0.0f,  0.0f, 0.0f, 0.0f,
        0.0f,  1.0f, 0.0f, 0.0f,
        0.0f,  0.0f, 0.0f, 0.0f
    };

    UnquantizedContainer scores =
    {
        0.0f, 0.9f,  0.8f,
        0.0f, 0.75f, 0.72f,
        0.0f, 0.6f,  0.5f,
        0.0f, 0.93f, 0.95f,
        0.0f, 0.5f,  0.4f,
        0.0f, 0.3f,  0.2f
    };

    // Outputs
    UnquantizedContainer detectionBoxes =
    {
        0.0f, 10.0f, 1.0f, 11.0f,
        0.0f, 10.0f, 1.0f, 11.0f,
        0.0f, 0.0f,  0.0f, 0.0f
    };

    UnquantizedContainer detectionClasses = { 1.0f,  0.0f,  0.0f };
    UnquantizedContainer detectionScores  = { 0.95f, 0.93f, 0.0f };

    UnquantizedContainer numDetections    = { 2.0f };

    // Quantize inputs and outputs
    using QuantizedContainer = std::vector<uint8_t>;

    QuantizedContainer quantBoxEncodings = armnnUtils::QuantizedVector<uint8_t>(boxEncodings, 1.00f, 1);
    QuantizedContainer quantScores       = armnnUtils::QuantizedVector<uint8_t>(scores,       0.01f, 0);

    std::map<std::string, QuantizedContainer> input =
    {
        { "box_encodings", quantBoxEncodings },
        { "scores", quantScores }
    };

    std::map<std::string, UnquantizedContainer> output =
    {
        { "detection_boxes", detectionBoxes},
        { "detection_classes", detectionClasses},
        { "detection_scores", detectionScores},
        { "num_detections", numDetections}
    };

    RunTest<armnn::DataType::QAsymmU8, armnn::DataType::Float32>(0, input, output);
}

BOOST_FIXTURE_TEST_CASE(DetectionPostProcessGraphStructureTest, ParseDetectionPostProcessCustomOptions)
{
    /*
       Inputs:            box_encodings  scores
                               \          /
                            DetectionPostProcess
                          /        /     \       \
                         /        /       \       \
       Outputs:     detection detection detection num_detections
                    boxes     classes   scores
    */

    ReadStringToBinary();

    armnn::INetworkPtr network = m_Parser->CreateNetworkFromBinary(m_GraphBinary);

    auto optimized = Optimize(*network, { armnn::Compute::CpuRef }, m_Runtime->GetDeviceSpec());

    auto optimizedNetwork = armnn::PolymorphicDowncast<armnn::OptimizedNetwork*>(optimized.get());
    auto graph = optimizedNetwork->GetGraph();

    // Check the number of layers in the graph
    BOOST_TEST((graph.GetNumInputs() == 2));
    BOOST_TEST((graph.GetNumOutputs() == 4));
    BOOST_TEST((graph.GetNumLayers() == 7));

    // Input layers
    armnn::Layer* boxEncodingLayer = GetFirstLayerWithName(graph, "box_encodings");
    BOOST_TEST((boxEncodingLayer->GetType() == armnn::LayerType::Input));
    BOOST_TEST(CheckNumberOfInputSlot(boxEncodingLayer, 0));
    BOOST_TEST(CheckNumberOfOutputSlot(boxEncodingLayer, 1));

    armnn::Layer* scoresLayer = GetFirstLayerWithName(graph, "scores");
    BOOST_TEST((scoresLayer->GetType() == armnn::LayerType::Input));
    BOOST_TEST(CheckNumberOfInputSlot(scoresLayer, 0));
    BOOST_TEST(CheckNumberOfOutputSlot(scoresLayer, 1));

    // DetectionPostProcess layer
    armnn::Layer* detectionPostProcessLayer = GetFirstLayerWithName(graph, "DetectionPostProcess:0:0");
    BOOST_TEST((detectionPostProcessLayer->GetType() == armnn::LayerType::DetectionPostProcess));
    BOOST_TEST(CheckNumberOfInputSlot(detectionPostProcessLayer, 2));
    BOOST_TEST(CheckNumberOfOutputSlot(detectionPostProcessLayer, 4));

    // Output layers
    armnn::Layer* detectionBoxesLayer = GetFirstLayerWithName(graph, "detection_boxes");
    BOOST_TEST((detectionBoxesLayer->GetType() == armnn::LayerType::Output));
    BOOST_TEST(CheckNumberOfInputSlot(detectionBoxesLayer, 1));
    BOOST_TEST(CheckNumberOfOutputSlot(detectionBoxesLayer, 0));

    armnn::Layer* detectionClassesLayer = GetFirstLayerWithName(graph, "detection_classes");
    BOOST_TEST((detectionClassesLayer->GetType() == armnn::LayerType::Output));
    BOOST_TEST(CheckNumberOfInputSlot(detectionClassesLayer, 1));
    BOOST_TEST(CheckNumberOfOutputSlot(detectionClassesLayer, 0));

    armnn::Layer* detectionScoresLayer = GetFirstLayerWithName(graph, "detection_scores");
    BOOST_TEST((detectionScoresLayer->GetType() == armnn::LayerType::Output));
    BOOST_TEST(CheckNumberOfInputSlot(detectionScoresLayer, 1));
    BOOST_TEST(CheckNumberOfOutputSlot(detectionScoresLayer, 0));

    armnn::Layer* numDetectionsLayer = GetFirstLayerWithName(graph, "num_detections");
    BOOST_TEST((numDetectionsLayer->GetType() == armnn::LayerType::Output));
    BOOST_TEST(CheckNumberOfInputSlot(numDetectionsLayer, 1));
    BOOST_TEST(CheckNumberOfOutputSlot(numDetectionsLayer, 0));

    // Check the connections
    armnn::TensorInfo boxEncodingTensor(armnn::TensorShape({ 1, 6, 4 }), armnn::DataType::QAsymmU8, 1, 1);
    armnn::TensorInfo scoresTensor(armnn::TensorShape({ 1, 6, 3 }), armnn::DataType::QAsymmU8,
                                                      0.00999999978f, 0);

    armnn::TensorInfo detectionBoxesTensor(armnn::TensorShape({ 1, 3, 4 }), armnn::DataType::Float32, 0, 0);
    armnn::TensorInfo detectionClassesTensor(armnn::TensorShape({ 1, 3 }), armnn::DataType::Float32, 0, 0);
    armnn::TensorInfo detectionScoresTensor(armnn::TensorShape({ 1, 3 }), armnn::DataType::Float32, 0, 0);
    armnn::TensorInfo numDetectionsTensor(armnn::TensorShape({ 1} ), armnn::DataType::Float32, 0, 0);

    BOOST_TEST(IsConnected(boxEncodingLayer, detectionPostProcessLayer, 0, 0, boxEncodingTensor));
    BOOST_TEST(IsConnected(scoresLayer, detectionPostProcessLayer, 0, 1, scoresTensor));
    BOOST_TEST(IsConnected(detectionPostProcessLayer, detectionBoxesLayer, 0, 0, detectionBoxesTensor));
    BOOST_TEST(IsConnected(detectionPostProcessLayer, detectionClassesLayer, 1, 0, detectionClassesTensor));
    BOOST_TEST(IsConnected(detectionPostProcessLayer, detectionScoresLayer, 2, 0, detectionScoresTensor));
    BOOST_TEST(IsConnected(detectionPostProcessLayer, numDetectionsLayer, 3, 0, numDetectionsTensor));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2019 by AutoSense Organization. All rights reserved.
 * Gary Chan <chenshj35@mail2.sysu.edu.cn>
 */

#ifndef COMMON_INCLUDE_COMMON_TRANSFORM_HPP_
#define COMMON_INCLUDE_COMMON_TRANSFORM_HPP_

#include <geometry_msgs/Point.h>  // geometry_msgs::Point
#include <ros/ros.h>
#include <tf/transform_listener.h>    // tf::TransformListener
#include <tf_conversions/tf_eigen.h>  // tf::transformTFToEigen

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

#include "common/types/object.hpp"  // ObjectPtr

namespace autosense {
namespace common {
namespace transform {

static bool getVelodynePose(const tf::TransformListener& tf_buffer,
                            const std::string& source_frame,
                            const std::string& target_frame,
                            const double& query_time,
                            Eigen::Matrix4d* trans) {
    if (trans == nullptr) {
        ROS_ERROR("Failed to get trans, the trans ptr can not be NULL.");
        return false;
    }

    ros::Time query_stamp(query_time);
    tf::StampedTransform transform_stamped;
    try {
        tf_buffer.lookupTransform(target_frame, source_frame, query_stamp,
                                  transform_stamped);
    } catch (tf2::TransformException& ex) {
        ROS_WARN(
            "Failed to query pose at %lf, use latest available pose instead.",
            query_time);
        try {
            tf_buffer.lookupTransform(target_frame, source_frame, ros::Time(0),
                                      transform_stamped);
        } catch (tf2::TransformException& ex) {
            ROS_ERROR_STREAM("Exception: " << ex.what());
            return false;
        }
    }
    Eigen::Affine3d affine_3d;
    tf::transformTFToEigen(transform_stamped, affine_3d);
    *trans = affine_3d.matrix();

    ROS_INFO_STREAM("Get " << source_frame << " to " << target_frame
                           << " trans: \n"
                           << *trans);

    return true;
}

static void transformGroundBox(const Eigen::Matrix4d& trans,
                               Eigen::Vector3d* position) {
    Eigen::Vector3d& center = *position;
    center =
        (trans * Eigen::Vector4d(center[0], center[1], center[2], 1)).head(3);
}

template <typename PointT>
static void transformPoint(const Eigen::Matrix4d& trans, PointT* position) {
    Eigen::Vector3d center =
        (trans *
         Eigen::Vector4d((*position).x, (*position).y, (*position).z, 1))
            .head(3);
    (*position).x = center(0);
    (*position).y = center(1);
    (*position).z = center(2);
}

static void transformDirection(const Eigen::Matrix4d& trans,
                               Eigen::Vector3d* position) {
    Eigen::Vector3d& center = *position;
    center =
        (trans * Eigen::Vector4d(center[0], center[1], center[2], 0)).head(3);
}

static void transformVelocity(const Eigen::Matrix4d& trans,
                              Eigen::Vector3d* position) {
    transformDirection(trans, position);
}

template <typename PointT>
static void transformPointCloud(
    const Eigen::Matrix4d& trans_mat,
    typename pcl::PointCloud<PointT>::Ptr cloud_in_out) {
    assert(cloud_in_out.get() != nullptr);

    for (size_t i = 0u; i < cloud_in_out->size(); ++i) {
        PointT& p = cloud_in_out->at(i);
        // ROS_WARN("Original: (%lf, %lf, %lf)", p.x, p.y, p.z);
        Eigen::Vector4d v(p.x, p.y, p.z, 1.);
        v = trans_mat * v;
        p.x = v(0);
        p.y = v(1);
        p.z = v(2);
        // ROS_INFO("Transformed: (%lf, %lf, %lf)", p.x, p.y, p.z);
    }
}

/**
 * @brief
 *  needed for PointICloud-converted tyoe, can't use Ptr
 *  common::geometry::transformPointCloud<PointD>
 * @tparam PointT
 * @param trans_mat
 * @param cloud_in_out
 */
template <typename PointT>
static void transformPointCloud(
    const Eigen::Matrix4d& trans_mat,
    typename pcl::PointCloud<PointT>& cloud_in_out) {  // NOLINT
    for (size_t i = 0u; i < cloud_in_out.size(); ++i) {
        PointT& p = cloud_in_out.at(i);
        Eigen::Vector4d v(p.x, p.y, p.z, 1.);
        v = trans_mat * v;
        p.x = v(0);
        p.y = v(1);
        p.z = v(2);
    }
}

/**
 * @brief transform object with given pose
 * @params[IN] pose: pose using for coordinate transformation
 * @params[OUT] obj: object for transfromation
 * @return nothing
 */
static void transformBuiltObject(const Eigen::Matrix4d& pose, ObjectPtr obj) {
    /*Eigen::Vector3d& dir = obj->direction;
    dir = (pose * Eigen::Vector4d(dir[0], dir[1], dir[2], 0)).head(3);*/
    transformDirection(pose, &(obj->direction));
    // transform center
    transformGroundBox(pose, &(obj->ground_center));
    /*Eigen::Vector3d& center = obj->ground_center;
    center = (pose * Eigen::Vector4d(center[0], center[1], center[2],
    1)).head(3);*/
    // transform cloud & polygon
    transformPointCloud<PointI>(pose, obj->cloud);
    // PointDCloudPtr polygon(&((*obj)->polygon));
    transformPointCloud<PointD>(pose, obj->polygon);
}

static void transformBuiltObjects(const Eigen::Matrix4d& transform_to_mat,
                                  std::vector<ObjectPtr>* objects) {
    for (size_t i = 0u; i < (*objects).size(); ++i) {
        transformBuiltObject(transform_to_mat, (*objects)[i]);
    }
}

}  // namespace transform
}  // namespace common
}  // namespace autosense

#endif  // COMMON_INCLUDE_COMMON_TRANSFORM_HPP_


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)
//         Torsten Sattler (sattlert@inf.ethz.ch)

#include "theia/vision/sfm/camera/camera_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "theia/alignment/alignment.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::Vector3d;

CameraPose::CameraPose() : k1_(0.0), k2_(0.0), k3_(0.0), k4_(0.0) {
  calibration_matrix_.setIdentity();
  transformation_matrix_.setIdentity();
  projection_matrix_.setIdentity();
}

CameraPose::~CameraPose() {}

void CameraPose::InitializePose(const Matrix3d& rotation,
                                const Vector3d& translation,
                                const Matrix3d& calibration, const double k1,
                                const double k2, const double k3,
                                const double k4) {
  transformation_matrix_.linear() = rotation;
  transformation_matrix_.translation() = translation;
  calibration_matrix_ = calibration;
  projection_matrix_ = calibration_matrix_ * transformation_matrix_.matrix();
  k1_ = k1;
  k2_ = k2;
  k3_ = k3;
  k4_ = k4;
}

// The projection matrix is defined such that P = K * [R | t], where K = diag(f,
// f, 1) for focal length f. The rows of the rotation matrix must have the same
// norm, so we can recover the focal length by comparing the norms of the first
// and third projection matrix row (since only one of them will be affected by
// the focal length).
void CameraPose::InitializePose(const Matrix<double, 3, 4>& projection_matrix,
                                const double k1, const double k2,
                                const double k3, const double k4) {
  projection_matrix_ = projection_matrix;
  k1_ = k1;
  k2_ = k2;
  k3_ = k3;
  k4_ = k4;

  // Recover focal length and construct the calibration matrix. NOTE: setting
  // the calibration matrix to diag(1, 1, f) is OK since we only recover the
  // focal length as the ratio of diagonal elements (see focal_length() method).
  const double inv_focal_length = sqrt(projection_matrix_.block<1, 3>(
      2, 0).squaredNorm() / projection_matrix.block<1, 3>(0, 0).squaredNorm());
  calibration_matrix_ =
      Eigen::DiagonalMatrix<double, 3>(1.0, 1.0, inv_focal_length);

  // Since P = K * T, where T = [R | t], we can recover T with T = K^-1 * P. We
  // must use a temp matrix so that the AffineCompact3d type of Eigen does not
  // fix the det of the rotation. We use the det of the rotation to recover the
  // sign of the focal length.
  const Eigen::Matrix<double, 3, 4> temp_transformation =
      calibration_matrix_.inverse() * projection_matrix_;

  // Recover the sign of the focal length using the fact that a rotation matrix
  // should have a determinant of 1.
  double det = temp_transformation.block<3, 3>(0, 0).determinant();
  if (det < 0.0) {
    calibration_matrix_(2, 2) *= -1.0;
  }
  transformation_matrix_ = calibration_matrix_.inverse() * projection_matrix_;
}

void CameraPose::InitializePose(
    const Matrix<double, 3, 4>& transformation_matrix,
    const Matrix3d& calibration, const double k1, const double k2,
    const double k3, const double k4) {
  transformation_matrix_ = transformation_matrix;
  calibration_matrix_ = calibration;
  projection_matrix_ = calibration_matrix_ * transformation_matrix_.matrix();
  k1_ = k1;
  k2_ = k2;
  k3_ = k3;
  k4_ = k4;
}

void CameraPose::InitializePose(const CameraPose& pose) {
  InitializePose(pose.transformation_matrix_.matrix(), pose.calibration_matrix_,
                 pose.k1_, pose.k2_, pose.k3_, pose.k4_);
}

void CameraPose::WorldToCamera(const Vector3d& world_point,
                               Vector3d* camera_point) const {
  *camera_point = transformation_matrix_ * world_point;
}

void CameraPose::WorldToCamera(const std::vector<Vector3d>& world_point,
                               std::vector<Vector3d>* camera_point) const {
  Map<const Matrix<double, 3, Eigen::Dynamic> > world_point_matrix(
      world_point[0].data(), 3, world_point.size());
  camera_point->clear();
  camera_point->resize(world_point.size());
  Map<Matrix<double, 3, Eigen::Dynamic> > camera_point_matrix(
      (*camera_point)[0].data(), 3, camera_point->size());
  camera_point_matrix = transformation_matrix_ * world_point_matrix;
}

void CameraPose::CameraToWorld(const Vector3d& camera_point,
                               Vector3d* world_point) const {
  *world_point = transformation_matrix_.inverse() * camera_point;
}

void CameraPose::CameraToWorld(const std::vector<Vector3d>& camera_point,
                               std::vector<Vector3d>* world_point) const {
  Map<const Matrix<double, 3, Eigen::Dynamic> > camera_point_matrix(
      camera_point[0].data(), 3, camera_point.size());
  world_point->clear();
  world_point->resize(camera_point.size());
  Map<Matrix<double, 3, Eigen::Dynamic> > world_point_matrix(
      (*world_point)[0].data(), 3, world_point->size());
  world_point_matrix = transformation_matrix_.inverse() * camera_point_matrix;
}

void CameraPose::CameraToImage(const Vector3d& camera_point,
                               Vector2d* image_point) const {
  *image_point = (calibration_matrix_ * camera_point).hnormalized();
}

void CameraPose::CameraToImage(const std::vector<Vector3d>& camera_point,
                               std::vector<Vector2d>* image_point) const {
  Map<const Matrix<double, 3, Eigen::Dynamic> > camera_point_matrix(
      camera_point[0].data(), 3, camera_point.size());
  image_point->clear();
  image_point->resize(camera_point.size());
  Map<Matrix<double, 2, Eigen::Dynamic> > image_point_matrix(
      (*image_point)[0].data(), 2, image_point->size());
  image_point_matrix = (calibration_matrix_.inverse() * camera_point_matrix)
      .colwise().hnormalized();
}

bool CameraPose::WorldToImage(const Vector3d& world_point,
                              Vector2d* image_point) const {
  const Vector3d proj_point = projection_matrix_ * world_point.homogeneous();
  // Return false if the point is behind the camera.
  if (calibration_matrix_(0, 0) * proj_point[2] <= 0.0) {
    return false;
  }
  *image_point = proj_point.hnormalized();
  return true;
}

void CameraPose::WorldToImage(const std::vector<Vector3d>& world_point,
                              std::vector<Vector2d>* image_point) const {
  Map<const Matrix<double, 3, Eigen::Dynamic> > world_point_matrix(
      world_point[0].data(), 3, world_point.size());

  // Create a temp 3xN matrix of the projected points... eigen tends to struggle
  // when using colwise().homogeneous() and colwise().hnormalized() in the same
  // expression.
  Matrix<double, 3, Eigen::Dynamic> temp_proj_mat =
      projection_matrix_ * (world_point_matrix.colwise().homogeneous());

  image_point->clear();
  image_point->resize(world_point.size());
  Map<Matrix<double, 2, Eigen::Dynamic> > image_point_matrix(
      (*image_point)[0].data(), 2, image_point->size());
  image_point_matrix = temp_proj_mat.colwise().hnormalized();
}

void CameraPose::UndistortImagePoint(const Vector2d& distorted_point,
                                     Vector2d* undistorted_point) const {
  const double r = distorted_point.squaredNorm();
  const double w_term =
      1.0 + k1_ * r + k2_ * r * r + k3_ * r * r * r + k4_ * r * r * r * r;
  *undistorted_point = distorted_point / w_term;
}

void CameraPose::UndistortImagePoint(
    const std::vector<Vector2d>& distorted_point,
    std::vector<Vector2d>* undistorted_point) const {
  undistorted_point->clear();
  undistorted_point->resize(distorted_point.size());

  Eigen::Map<const Eigen::Matrix<double, 2, Eigen::Dynamic> > distorted_map(
      distorted_point[0].data(), 2, distorted_point.size());
  Eigen::ArrayXd radius = distorted_map.colwise().squaredNorm();
  radius =
      1.0 + radius * k1_ + radius * radius * k2_ +
      radius * radius * radius * k3_ + radius * radius * radius * radius * k4_;

  Eigen::Map<Eigen::Matrix<double, 2, Eigen::Dynamic> > undistorted_map(
      (*undistorted_point)[0].data(), 2, undistorted_point->size());
  undistorted_map = distorted_map;
  undistorted_map = undistorted_map.array().rowwise() / radius.transpose();
}

}  // namespace theia


#include <boost/gil/extension/io/jpeg_dynamic_io.hpp>
#include <chrono>
#include <iostream>

#include"x_gradient.h"

using namespace boost::gil;

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

    int num_threads=1;
    char file_name[256] = "tum.jpg";
    int no_output = 0;

	int c;
	while ((c = getopt(argc, argv, "t:n:f:")) != -1)
	{
		switch (c)
		{
		case 't':
			if (sscanf(optarg, "%d", &num_threads) != 1)
				goto error;
			break;
		case 'n':
			if (sscanf(optarg, "%d", &no_output) != 1)
				goto error;
			break;
		case 'f':
			strncpy(file_name, optarg, sizeof(file_name));
			break;
		case '?':
        error: printf(
			    "Usage:\n"
			    "-t \t number of threads used in computation(default: 1)\n"
			    "-f \t output file name(default: tum.jpg)\n"
				"-n \t no output(default: 0)\n"
			    "\n"
			    "Example:\n"
			    "%s -t 4 -f tum.jpg\n",
			    argv[0]);
			exit(EXIT_FAILURE);
			break;
		}
	}

    rgb8_image_t img;
    jpeg_read_image(file_name,img);

    gray8s_image_t img_out(img.dimensions());
    fill_pixels(view(img_out),int8_t(0));

    auto start = std::chrono::high_resolution_clock::now();
    x_luminosity_gradient(const_view(img), view(img_out), num_threads);
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end-start;

    if (!no_output)
        jpeg_write_view("output.jpg",color_converted_view<gray8_pixel_t>(const_view(img_out)));

    std::cout << "Time: " << diff.count() << " seconds\n";

    return 0;
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */

#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>

#include <toolkits/object_detection/one_shot_object_detection/util/mapping_function.hpp>
#include <toolkits/object_detection/one_shot_object_detection/util/superposition.hpp>

static const boost::gil::rgb8_pixel_t RGB_WHITE(255,255,255);
static const boost::gil::rgba8_pixel_t RGBA_WHITE(255,255,255,0);

namespace turi {
namespace one_shot_object_detection {
namespace data_augmentation {

void superimpose_image(const boost::gil::rgb8_image_t::view_t &superimposed,
                       const boost::gil::rgba8_image_t::view_t &transformed,
                       const boost::gil::rgba8_image_t::view_t &background) {
  boost::gil::transform_pixels(transformed, background, superimposed,
    [](boost::gil::rgba8_ref_t a, boost::gil::rgba8_ref_t b) {
          boost::gil::red_t   R;
          boost::gil::green_t G;
          boost::gil::blue_t  B;
          boost::gil::alpha_t A;
          auto AoverB = [](size_t C_a, size_t C_b, size_t alpha_a, size_t alpha_b) {
            return ((C_a * alpha_a + C_b * alpha_b * (1 - alpha_a))/(
                      alpha_a + alpha_b * (1 - alpha_a)));
          };
          return  boost::gil::rgb8_image_t::value_type (
                    AoverB(get_color(a,R), get_color(b,R), get_color(a,A)/255, get_color(b,A)/255),
                    AoverB(get_color(a,G), get_color(b,G), get_color(a,A)/255, get_color(b,A)/255),
                    AoverB(get_color(a,B), get_color(b,B), get_color(a,A)/255, get_color(b,A)/255)
          );
  });
}

flex_image create_synthetic_image(const boost::gil::rgba8_image_t::view_t &starter_image_view,
                                  const boost::gil::rgb8_image_t::view_t &background_view,
                                  ParameterSampler &parameter_sampler,
                                  const flex_image &object) {
  boost::gil::rgba8_image_t background_rgba(boost::gil::rgba8_image_t::point_t(background_view.dimensions()));
  boost::gil::copy_and_convert_pixels(
    background_view,
    view(background_rgba)
  );
  boost::gil::rgba8_image_t transformed(boost::gil::rgba8_image_t::point_t(background_view.dimensions()));
  fill_pixels(view(transformed), RGBA_WHITE);
  boost::gil::rgb8_image_t superimposed(boost::gil::rgba8_image_t::point_t(background_view.dimensions()));
  fill_pixels(view(superimposed), RGB_WHITE);
  Eigen::Matrix<float, 3, 3> M = parameter_sampler.get_transform().inverse();
  resample_pixels(starter_image_view, view(transformed), M, boost::gil::nearest_neighbor_sampler());
  superimpose_image(view(superimposed), view(transformed), view(background_rgba));
  return flex_image(superimposed);
}

} // data_augmentation
} // one_shot_object_detection
} // turi


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>

namespace ipc::rigid {

Eigen::SparseMatrix<double> multiply(
    const Eigen::VectorXd& x,
    const std::vector<Eigen::SparseMatrix<double>>& y);

} // namespace ipc::rigid


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <boost/timer.hpp>

#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/matrix_mult.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>

using namespace mtl;
using namespace mtl::recursion;
using namespace std;

typedef gen_tiling_44_dense_mat_mat_mult_t<assign::plus_sum>  tiling_44_base_mult_t;

#if 0
    // Bitmasks:
    const unsigned long
	doppled_64_row_mask= generate_mask<true, 6, row_major, 0>::value,
	doppled_64_col_mask= generate_mask<true, 6, col_major, 0>::value;

void hybrid_ext_mult_44(const morton_dense<double,  doppled_64_row_mask>& a,
			const morton_dense<double,  doppled_64_col_mask>& b,
			morton_dense<double,  doppled_64_row_mask>& c)
{
  tiling_44_base_mult_t()(a, b, c);
}
#endif

void dense_ext_mult_44(const dense2D<double>& a,
		       const dense2D<double, mat::parameters<col_major> >& b,
		       dense2D<double>& c)
{
  tiling_44_base_mult_t()(a, b, c);
}


/*
 * gpcxx/performance/eval_basic_ecj_cmp/parser.hpp
 * Date: 2013-09-12
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef GPCXX_PERFORMANCE_EVAL_BASIC_ECJ_CMP_PARSER_HPP_INCLUDED
#define GPCXX_PERFORMANCE_EVAL_BASIC_ECJ_CMP_PARSER_HPP_INCLUDED

#include <gpcxx/tree/basic_tree.hpp>
#include <gpcxx/io/simple.hpp>

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_function.hpp>

#include <iostream>
#include <vector>
#include <string>

namespace parser
{
    namespace qi = boost::spirit::qi;
    namespace ascii = boost::spirit::ascii;

    ///////////////////////////////////////////////////////////////////////////
    //  Our AST
    ///////////////////////////////////////////////////////////////////////////
    struct binary_op;
    struct unary_op;
    struct nil {};

    struct expression_ast
    {
        typedef
            boost::variant<
                nil // can't happen!
                , unsigned int
                , boost::recursive_wrapper<expression_ast>
                , boost::recursive_wrapper<binary_op>
                , boost::recursive_wrapper<unary_op>
            >
        type;

        expression_ast( void ) : expr( nil() ) {}

        template <typename Expr>
        expression_ast( Expr const& expr ) : expr( expr ) {}

        expression_ast& operator+=( expression_ast const& rhs );
        expression_ast& operator-=( expression_ast const& rhs );
        expression_ast& operator*=( expression_ast const& rhs );
        expression_ast& operator/=( expression_ast const& rhs );

        type expr;
    };

    struct binary_op
    {
        binary_op( char op , expression_ast const& left , expression_ast const& right )
        : op(op), left(left), right(right) {}

        char op;
        expression_ast left , right;
    };

    struct unary_op
    {
        unary_op( char op , expression_ast const& subject )
        : op(op), subject(subject) {}

        char op;
        expression_ast subject;
    };

    expression_ast& expression_ast::operator+=( expression_ast const& rhs )
    {
        expr = binary_op( '+' , expr , rhs );
        return *this;
    }

    expression_ast& expression_ast::operator-=( expression_ast const& rhs )
    {
        expr = binary_op( '-' , expr , rhs );
        return *this;
    }

    expression_ast& expression_ast::operator*=( expression_ast const& rhs )
    {
        expr = binary_op( '*' , expr , rhs );
        return *this;
    }

    expression_ast& expression_ast::operator/=( expression_ast const& rhs )
    {
        expr = binary_op( '/' , expr , rhs );
        return *this;
    }

    struct unary_expr
    {
        template <typename T> struct result { typedef T type; };
        char op;
        unary_expr( char _op ) : op( _op ) { }

        expression_ast operator()(expression_ast const& expr) const
        {
            return expression_ast( unary_op( op , expr ) );
        }
    };

    boost::phoenix::function< unary_expr > sin_expr( 's' );
    boost::phoenix::function< unary_expr > cos_expr( 'c' );
    boost::phoenix::function< unary_expr > exp_expr( 'e' );
    boost::phoenix::function< unary_expr > log_expr( 'l' );

    ///////////////////////////////////////////////////////////////////////////
    //  Our calculator grammar
    ///////////////////////////////////////////////////////////////////////////
    template <typename Iterator>
    struct grammar : qi::grammar<Iterator, expression_ast(), ascii::space_type>
    {
        grammar() : grammar::base_type(expression)
        {
            using qi::_val;
            using qi::_1;
            using qi::uint_;
            using qi::lit;

            expression =
                term                            [_val = _1 ]
                >> *(   ('+' >> term            [_val += _1])
                    |   ('-' >> term            [_val -= _1])
                    )
                ;

            term =
                factor                          [_val = _1]
                >> *(   ('*' >> factor          [_val *= _1])
                    |   ('%' >> factor          [_val /= _1])
                    )
                ;

            factor =
                  lit( "x1" )                                    [ _val = 'x' ]
                | lit( "x2" )                                    [ _val = 'y' ]
                | lit( "x3" )                                    [ _val = 'z' ]
                | ( lit( "sin" ) >> '(' >> expression >> ')' )   [ _val = sin_expr( _1 ) ]
                | ( lit( "cos" ) >> '(' >> expression >> ')' )   [ _val = cos_expr( _1 ) ]
                | ( lit( "rlog" ) >> '(' >> expression >> ')' )  [ _val = log_expr( _1 ) ]
                | ( lit( "exp" ) >> '(' >> expression >> ')' )   [ _val = exp_expr( _1 ) ]
                | ( '(' >> expression >> ')' )                   [ _val = _1 ]
                ;
        }

        qi::rule<Iterator, expression_ast(), ascii::space_type>
        expression, term, factor;
    };

    ///////////////////////////////////////////////////////////////////////////
    //  Walk the tree
    ///////////////////////////////////////////////////////////////////////////
    struct ast_print
    {
        typedef void result_type;

        void operator()( nil ) const {}
        void operator()( char n ) const { std::cout << n; }

        void operator()( expression_ast const& ast ) const
        {
            boost::apply_visitor(*this, ast.expr);
        }

        void operator()(binary_op const& expr) const
        {
            std::cout << "op:" << expr.op << "(";
            boost::apply_visitor(*this, expr.left.expr);
            std::cout << ", ";
            boost::apply_visitor(*this, expr.right.expr);
            std::cout << ')';
        }

        void operator()(unary_op const& expr) const
        {
            std::cout << "op:" << expr.op << "(";
            boost::apply_visitor(*this, expr.subject.expr);
            std::cout << ')';
        }
    };

    template< typename Tree >
    struct tree_transformator : public boost::static_visitor< void >
    {
        typedef Tree tree_type;
        typedef typename Tree::cursor cursor;

        tree_transformator( tree_type &tree , cursor c ) : tree_( tree ) , c_( c ) { }

        void operator()( nil ) const {}

        void operator()( char n ) const
        {
            tree_.insert_below( c_ , n );
        }

        void operator()( expression_ast const& ast ) const
        {
            boost::apply_visitor( *this , ast.expr );
        }

        void operator()( binary_op const& expr ) const
        {
            cursor c1 = tree_.insert_below( c_ , expr.op );
            boost::apply_visitor( tree_transformator( tree_ , c1 ) , expr.left.expr );
            boost::apply_visitor( tree_transformator( tree_ , c1 ) , expr.right.expr );
        }

        void operator()( unary_op const& expr ) const
        {
            cursor c1 = tree_.insert_below( c_ , expr.op );
            boost::apply_visitor( tree_transformator( tree_ , c1 ) , expr.subject.expr );
        }

        tree_type &tree_;
        cursor c_;
    };

    template< typename Trafo >
    void parse_tree( std::string str , Trafo & trafo )
    {
        using boost::spirit::ascii::space;

        typedef std::string::const_iterator iterator_type;
        typedef grammar< iterator_type > grammar_type;

        grammar_type g;

        std::string::const_iterator iter = str.begin();
        std::string::const_iterator end = str.end();
        expression_ast ast;
        bool r = phrase_parse( iter , end , g , space , ast );

        if (r && iter == end)
        {
            trafo( ast );
        }
        else
        {
            std::string rest(iter, end);
            throw std::runtime_error( std::string( "Parsing of expr " ) + str + " failed! Stopped at " + rest );
            // std::cout << "stopped at: \": " << rest << "\"\n";
        }
    }

} // namespace parser

#endif // GPCXX_PERFORMANCE_EVAL_BASIC_ECJ_CMP_PARSER_HPP_INCLUDED


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief kitchen_sink.cpp

\details
More extensive quantity tests.

Output:
@verbatim

//[kitchen_sink_output_1
S1 :    2
X1 :    2
X2 :    (4/3)
U1 :    N
U2 :    J
Q1 :    1 N
Q2 :    2 J
//]

//[kitchen_sink_output_2
U1*S1 : 2 N
S1*U1 : 2 N
U1/S1 : 0.5 N
S1/U1 : 2 m^-1 kg^-1 s^2
//]

//[kitchen_sink_output_3
U1+U1 : N
U1-U1 : N
U1*U1 : m^2 kg^2 s^-4
U1/U1 : dimensionless
U1*U2 : m^3 kg^2 s^-4
U1/U2 : m^-1
U1^X  : m^2 kg^2 s^-4
X1vU1 : m^(1/2) kg^(1/2) s^-1
U1^X2 : m^(4/3) kg^(4/3) s^(-8/3)
X2vU1 : m^(3/4) kg^(3/4) s^(-3/2)
//]

//[kitchen_sink_output_4
Q1*S1 : 2 N
S1*Q1 : 2 N
Q1/S1 : 0.5 N
S1/Q1 : 2 m^-1 kg^-1 s^2
//]

//[kitchen_sink_output_5
U1*Q1 : 1 m^2 kg^2 s^-4
Q1*U1 : 1 m^2 kg^2 s^-4
U1/Q1 : 1 dimensionless
Q1/U1 : 1 dimensionless
//]

//[kitchen_sink_output_6
+Q1   : 1 N
-Q1   : -1 N
Q1+Q1 : 2 N
Q1-Q1 : 0 N
Q1*Q1 : 1 m^2 kg^2 s^-4
Q1/Q1 : 1 dimensionless
Q1*Q2 : 2 m^3 kg^2 s^-4
Q1/Q2 : 0.5 m^-1
Q1^X1 : 1 m^2 kg^2 s^-4
X1vQ1 : 1 m^(1/2) kg^(1/2) s^-1
Q1^X2 : 1 m^(4/3) kg^(4/3) s^(-8/3)
X2vQ1 : 1 m^(3/4) kg^(3/4) s^(-3/2)
//]

//[kitchen_sink_output_7
l1 == l2    false
l1 != l2    true
l1 <= l2    true
l1 < l2     true
l1 >= l2    false
l1 > l2     false
//]

dimless = 1

//[kitchen_sink_output_8
v1 = 2 m s^-1
//]

//[kitchen_sink_output_9
F  = 1 N
dx = 1 m
E  = 1 J
//]

//[kitchen_sink_output_10
r = 5e-07 m
P = 101325 Pa
V = 5.23599e-19 m^3
T = 310 K
n = 2.05835e-17 mol
R = 8.314472 m^2 kg s^-2 K^-1 mol^-1 (rel. unc. = 1.8e-06)
//]

//[kitchen_sink_output_11
theta            = 0.375 rd
sin(theta)       = 0.366273 dimensionless
asin(sin(theta)) = 0.375 rd
//]

//[kitchen_sink_output_12
V   = (12.5,0) V
I   = (3,4) A
Z   = (1.5,-2) Ohm
I*Z = (12.5,0) V
//]

//[kitchen_sink_output_13
x+y-w         = 0.48(+/-0.632772) m
w*x           = 9.04(+/-0.904885) m^2
x/y           = 0.666667(+/-0.149071) dimensionless
//]

//[kitchen_sink_output_14
w*y^2/(u*x)^2 = 10.17(+/-3.52328) m^-1
w/(u*x)^(1/2) = 3.19612(+/-0.160431) dimensionless
//]

//[kitchen_sink_output_15
I*w   = m^2 kg s^-1 rad^-1
I*w/L = dimensionless
I*w^2 = J
//]

//[kitchen_sink_output_16
1 F
1 kat
1 S
1 C
1 V
1 J
1 N
1 Hz
1 lx
1 H
1 lm
1 Wb
1 T
1 W
1 Pa
1 Ohm
//]

//[kitchen_sink_output_18
1 farad
1 katal
1 siemen
1 coulomb
1 volt
1 joule
1 newton
1 hertz
1 lux
1 henry
1 lumen
1 weber
1 tesla
1 watt
1 pascal
1 ohm
//]

@endverbatim
**/

#include <cmath>
#include <complex>
#include <iostream>

#include <boost/typeof/std/complex.hpp>

#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/codata/physico-chemical_constants.hpp>
#include <boost/units/systems/si/io.hpp>

#include "measurement.hpp"

namespace boost {

namespace units {

//[kitchen_sink_function_snippet_3
/// the physical definition of work - computed for an arbitrary unit system
template<class System,class Y>
constexpr
quantity<unit<energy_dimension,System>,Y>
work(quantity<unit<force_dimension,System>,Y> F,
     quantity<unit<length_dimension,System>,Y> dx)
{
    return F*dx;
}
//]

//[kitchen_sink_function_snippet_4
/// the ideal gas law in si units
template<class Y>
constexpr
quantity<si::amount,Y>
idealGasLaw(const quantity<si::pressure,Y>& P,
            const quantity<si::volume,Y>& V,
            const quantity<si::temperature,Y>& T)
{
    using namespace boost::units::si;

    using namespace constants::codata;
    return (P*V/(R*T));
}
//]

} // namespace units

} // namespace boost

int main()
{
    using namespace boost::units;
    using namespace boost::units::si;

    {
    //[kitchen_sink_snippet_1
    /// scalar
    const double    s1 = 2;

    const long                  x1 = 2;
    const static_rational<4,3>  x2;

    /// define some units
    force       u1 = newton;
    energy      u2 = joule;

    /// define some quantities
    quantity<force>      q1(1.0*u1);
    quantity<energy>     q2(2.0*u2);
    //]

    /// check scalar, unit, and quantity io
    std::cout << "S1 :    " << s1 << std::endl
              << "X1 :    " << x1 << std::endl
              << "X2 :    " << x2 << std::endl
              << "U1 :    " << u1 << std::endl
              << "U2 :    " << u2 << std::endl
              << "Q1 :    " << q1 << std::endl
              << "Q2 :    " << q2 << std::endl
              << std::endl;

    /// check scalar-unit algebra
    std::cout //<< "U1+S1 : " << u1+s1 << std::endl    // illegal
              //<< "S1+U1 : " << s1+u1 << std::endl    // illegal
              //<< "U1-S1 : " << u1-s1 << std::endl    // illegal
              //<< "S1-U1 : " << s1-u1 << std::endl    // illegal
              << "U1*S1 : " << u1*s1 << std::endl
              << "S1*U1 : " << s1*u1 << std::endl
              << "U1/S1 : " << u1/s1 << std::endl
              << "S1/U1 : " << s1/u1 << std::endl
              << std::endl;

    /// check unit-unit algebra
    std::cout << "U1+U1 : " << u1+u1 << std::endl
              << "U1-U1 : " << u1-u1 << std::endl
              << "U1*U1 : " << u1*u1 << std::endl
              << "U1/U1 : " << u1/u1 << std::endl
              //<< "U1+U2 : " << u1+u2 << std::endl     // illegal
              //<< "U1-U2 : " << u1-u2 << std::endl     // illegal
              << "U1*U2 : " << u1*u2 << std::endl
              << "U1/U2 : " << u1/u2 << std::endl
              << "U1^X  : " << pow<2>(u1) << std::endl
              << "X1vU1 : " << root<2>(u1) << std::endl
              << "U1^X2 : " << pow<static_rational<4,3> >(u1) << std::endl
              << "X2vU1 : " << root<static_rational<4,3> >(u1) << std::endl
              << std::endl;

    /// check scalar-quantity algebra
    std::cout //<< "Q1+S1 : " << q1+s1 << std::endl    // illegal
              //<< "S1+Q1 : " << s1+q1 << std::endl    // illegal
              //<< "Q1-S1 : " << q1-s1 << std::endl    // illegal
              //<< "S1-Q1 : " << s1-q1 << std::endl    // illegal
              << "Q1*S1 : " << q1*s1 << std::endl
              << "S1*Q1 : " << s1*q1 << std::endl
              << "Q1/S1 : " << q1/s1 << std::endl
              << "S1/Q1 : " << s1/q1 << std::endl
              << std::endl;

    /// check unit-quantity algebra
    std::cout //<< "U1+Q1 : " << u1+q1 << std::endl    // illegal
              //<< "Q1+U1 : " << q1+u1 << std::endl    // illegal
              //<< "U1-Q1 : " << u1-q1 << std::endl    // illegal
              //<< "Q1-U1 : " << q1-u1 << std::endl    // illegal
              << "U1*Q1 : " << u1*q1 << std::endl
              << "Q1*U1 : " << q1*u1 << std::endl
              << "U1/Q1 : " << u1/q1 << std::endl
              << "Q1/U1 : " << q1/u1 << std::endl
              << std::endl;

    /// check quantity-quantity algebra
    std::cout << "+Q1   : " << +q1 << std::endl
              << "-Q1   : " << -q1 << std::endl
              << "Q1+Q1 : " << q1+q1 << std::endl
              << "Q1-Q1 : " << q1-q1 << std::endl
              << "Q1*Q1 : " << q1*q1 << std::endl
              << "Q1/Q1 : " << q1/q1 << std::endl
              //<< "Q1+Q2 : " << q1+q2 << std::endl    // illegal
              //<< "Q1-Q2 : " << q1-q2 << std::endl    // illegal
              << "Q1*Q2 : " << q1*q2 << std::endl
              << "Q1/Q2 : " << q1/q2 << std::endl
              << "Q1^X1 : " << pow<2>(q1) << std::endl
              << "X1vQ1 : " << root<2>(q1) << std::endl
              << "Q1^X2 : " << pow<static_rational<4,3> >(q1) << std::endl
              << "X2vQ1 : " << root<static_rational<4,3> >(q1) << std::endl
              << std::endl;

    //[kitchen_sink_snippet_2
    /// check comparison tests
    quantity<length>    l1(1.0*meter),
                        l2(2.0*meters);
    //]

    std::cout << std::boolalpha
              << "l1 == l2" << "\t" << (l1 == l2) << std::endl
              << "l1 != l2" << "\t" << (l1 != l2) << std::endl
              << "l1 <= l2" << "\t" << (l1 <= l2) << std::endl
              << "l1 < l2 " << "\t" << (l1  < l2) << std::endl
              << "l1 >= l2" << "\t" << (l1 >= l2) << std::endl
              << "l1 > l2 " << "\t" << (l1  > l2) << std::endl
              << std::endl;

    //[kitchen_sink_snippet_3
    /// check implicit unit conversion from dimensionless to value_type
    const double    dimless = (q1/q1);
    //]

    std::cout << "dimless = " << dimless << std::endl
              << std::endl;

    quantity<velocity>  v1 = 2.0*meters/second;

    std::cout << "v1 = " << v1 << std::endl
              << std::endl;

    //[kitchen_sink_snippet_4
    /// test calcuation of work
    quantity<force>       F(1.0*newton);
    quantity<length>      dx(1.0*meter);
    quantity<energy>      E(work(F,dx));
    //]

    std::cout << "F  = " << F << std::endl
              << "dx = " << dx << std::endl
              << "E  = " << E << std::endl
              << std::endl;

    {
    //[kitchen_sink_snippet_5
    /// test ideal gas law
    quantity<temperature>   T = (273.+37.)*kelvin;
    quantity<pressure>      P = 1.01325e5*pascals;
    quantity<length>        r = 0.5e-6*meters;
    quantity<volume>        V = (4.0/3.0)*3.141592*pow<3>(r);
    quantity<amount>        n(idealGasLaw(P,V,T));
    //]

    std::cout << "r = " << r << std::endl
              << "P = " << P << std::endl
              << "V = " << V << std::endl
              << "T = " << T << std::endl
              << "n = " << n << std::endl
              #if BOOST_UNITS_HAS_TYPEOF
              << "R = " << constants::codata::R << std::endl
              #else
              << "no typeof" << std::endl
              #endif // BOOST_UNITS_HAS_TYPEOF
              << std::endl;
    }

    //[kitchen_sink_snippet_6
    /// test trig stuff
    quantity<plane_angle>           theta = 0.375*radians;
    quantity<dimensionless>         sin_theta = sin(theta);
    quantity<plane_angle>           thetap = asin(sin_theta);
    //]

    std::cout << "theta            = " << theta << std::endl
              << "sin(theta)       = " << sin_theta << std::endl
              << "asin(sin(theta)) = " << thetap << std::endl
              << std::endl;

    /// test implicit conversion of dimensionless to value
    double  tmp = sin_theta;

    tmp = sin_theta;

    /// test implicit conversion from value to dimensionless
    quantity<dimensionless>     tmpp = tmp;

    tmpp = tmp;

    /// check complex quantities
    typedef std::complex<double>    complex_type;

    //[kitchen_sink_snippet_7
    quantity<electric_potential,complex_type> v = complex_type(12.5,0.0)*volts;
    quantity<current,complex_type>            i = complex_type(3.0,4.0)*amperes;
    quantity<resistance,complex_type>         z = complex_type(1.5,-2.0)*ohms;
    //]

    std::cout << "V   = " << v << std::endl
              << "I   = " << i << std::endl
              << "Z   = " << z << std::endl
              << "I*Z = " << i*z << std::endl
              << std::endl;

    /// check quantities using user-defined type encapsulating error propagation

    //[kitchen_sink_snippet_8
    quantity<length,measurement<double> >
        u(measurement<double>(1.0,0.0)*meters),
        w(measurement<double>(4.52,0.02)*meters),
        x(measurement<double>(2.0,0.2)*meters),
        y(measurement<double>(3.0,0.6)*meters);
    //]

    std::cout << "x+y-w         = " << x+y-w << std::endl
              << "w*x           = " << w*x << std::endl
              << "x/y           = " << x/y << std::endl
              << "w*y^2/(u*x)^2 = " << w*y*y/pow<2>(u*x) << std::endl
              << "w/(u*x)^(1/2) = " << w/pow< static_rational<1,2> >(u*x)
              << std::endl << std::endl;
    }

    /// check moment of inertia/angular momentum/rotational energy

    //[kitchen_sink_snippet_9
    std::cout << symbol_format
              << "I*w   = " << moment_of_inertia()*angular_velocity() << std::endl
              << "I*w/L = " << moment_of_inertia()*angular_velocity()/angular_momentum() << std::endl
              << "I*w^2 = " << moment_of_inertia()*pow<2>(angular_velocity()) << std::endl
              << std::endl;
    //]

    //[kitchen_sink_snippet_10
//    std::cout << typename_format
//              << quantity<capacitance>(1.0*farad) << std::endl
//              << quantity<catalytic_activity>(1.0*katal) << std::endl
//              << quantity<conductance>(1.0*siemen) << std::endl
//              << quantity<electric_charge>(1.0*coulomb) << std::endl
//              << quantity<electric_potential>(1.0*volt) << std::endl
//              << quantity<energy>(1.0*joule) << std::endl
//              << quantity<force>(1.0*newton) << std::endl
//              << quantity<frequency>(1.0*hertz) << std::endl
//              << quantity<illuminance>(1.0*lux) << std::endl
//              << quantity<inductance>(1.0*henry) << std::endl
//              << quantity<luminous_flux>(1.0*lumen) << std::endl
//              << quantity<magnetic_flux>(1.0*weber) << std::endl
//              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
//              << quantity<power>(1.0*watt) << std::endl
//              << quantity<pressure>(1.0*pascals) << std::endl
//              << quantity<resistance>(1.0*ohm) << std::endl
//              << std::endl;
    //]

    //[kitchen_sink_snippet_11
//    std::cout << raw_format
//              << quantity<capacitance>(1.0*farad) << std::endl
//              << quantity<catalytic_activity>(1.0*katal) << std::endl
//              << quantity<conductance>(1.0*siemen) << std::endl
//              << quantity<electric_charge>(1.0*coulomb) << std::endl
//              << quantity<electric_potential>(1.0*volt) << std::endl
//              << quantity<energy>(1.0*joule) << std::endl
//              << quantity<force>(1.0*newton) << std::endl
//              << quantity<frequency>(1.0*hertz) << std::endl
//              << quantity<illuminance>(1.0*lux) << std::endl
//              << quantity<inductance>(1.0*henry) << std::endl
//              << quantity<luminous_flux>(1.0*lumen) << std::endl
//              << quantity<magnetic_flux>(1.0*weber) << std::endl
//              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
//              << quantity<power>(1.0*watt) << std::endl
//              << quantity<pressure>(1.0*pascals) << std::endl
//              << quantity<resistance>(1.0*ohm) << std::endl
//              << std::endl;
    //]

    //[kitchen_sink_snippet_12
    std::cout << symbol_format
              << quantity<capacitance>(1.0*farad) << std::endl
              << quantity<catalytic_activity>(1.0*katal) << std::endl
              << quantity<conductance>(1.0*siemen) << std::endl
              << quantity<electric_charge>(1.0*coulomb) << std::endl
              << quantity<electric_potential>(1.0*volt) << std::endl
              << quantity<energy>(1.0*joule) << std::endl
              << quantity<force>(1.0*newton) << std::endl
              << quantity<frequency>(1.0*hertz) << std::endl
              << quantity<illuminance>(1.0*lux) << std::endl
              << quantity<inductance>(1.0*henry) << std::endl
              << quantity<luminous_flux>(1.0*lumen) << std::endl
              << quantity<magnetic_flux>(1.0*weber) << std::endl
              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
              << quantity<power>(1.0*watt) << std::endl
              << quantity<pressure>(1.0*pascals) << std::endl
              << quantity<resistance>(1.0*ohm) << std::endl
              << std::endl;
    //]

    //[kitchen_sink_snippet_13
    std::cout << name_format
              << quantity<capacitance>(1.0*farad) << std::endl
              << quantity<catalytic_activity>(1.0*katal) << std::endl
              << quantity<conductance>(1.0*siemen) << std::endl
              << quantity<electric_charge>(1.0*coulomb) << std::endl
              << quantity<electric_potential>(1.0*volt) << std::endl
              << quantity<energy>(1.0*joule) << std::endl
              << quantity<force>(1.0*newton) << std::endl
              << quantity<frequency>(1.0*hertz) << std::endl
              << quantity<illuminance>(1.0*lux) << std::endl
              << quantity<inductance>(1.0*henry) << std::endl
              << quantity<luminous_flux>(1.0*lumen) << std::endl
              << quantity<magnetic_flux>(1.0*weber) << std::endl
              << quantity<magnetic_flux_density>(1.0*tesla) << std::endl
              << quantity<power>(1.0*watt) << std::endl
              << quantity<pressure>(1.0*pascals) << std::endl
              << quantity<resistance>(1.0*ohm) << std::endl
              << std::endl;
    //]

    return 0;
}


#ifndef SHASTA_MAKE_BICONNECTED_HPP
#define SHASTA_MAKE_BICONNECTED_HPP

// Boost libraries.
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/iteration_macros.hpp>

// Standard library.
#include <vector>

// Recursively remove articulation points from a graph
// until there are none left, at which point the graph is biconnected.
// Also computes a map containing the index of the biconnected component
// that each edge belongs to.

namespace shasta {
    template<class Graph> void makeBiconnected(
        Graph& graph,
        std::map<typename Graph::edge_descriptor, uint64_t>& componentMap);
}

template<class Graph> void shasta::makeBiconnected(
    Graph& graph,
    std::map<typename Graph::edge_descriptor, uint64_t>& componentMap)
{
    using boost::vertex_index_map;
    using boost::make_assoc_property_map;
    using vertex_descriptor = typename Graph::vertex_descriptor;

    std::map<vertex_descriptor, uint64_t> vertexMap;
    vector<vertex_descriptor> articulationPoints;
    while(true) {

        vertexMap.clear();
        uint64_t vertexIndex = 0;
        BGL_FORALL_VERTICES_T(v, graph, DynamicConflictReadGraph) {
            vertexMap.insert(make_pair(v, vertexIndex++));
        }
        articulationPoints.clear();
        componentMap.clear();
        boost::biconnected_components(
            graph,
            make_assoc_property_map(componentMap),
            back_inserter(articulationPoints),
            vertex_index_map(make_assoc_property_map(vertexMap)));

        if(articulationPoints.empty()) {
            return;
        }

        // Remove the articulation points.
        for(const vertex_descriptor v: articulationPoints) {
            clear_vertex(v, graph);
            remove_vertex(v, graph);
        }

    }

}

#endif


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_data_structure/sources/ai/cost_grid_space.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(cost_grid_space)

BOOST_AUTO_TEST_CASE(cost_grid_space_basic)
{
    cor::data_structure::CostGridSpace g;

    g.resize(10, 10);

    g.set_neighbor_mode(cor::data_structure::CostGridSpaceNeighborMode::L2);

    cor::RInt32 i, isz;

    isz = 5;
    for(i = 0 ; i < isz ; i++)
    {
        auto& c = g.ref(cor::type::Vector2I(i, 1));
        c.passable = cor::rfalse;
    }

    cor::type::Vector2I start(0, 9);
    cor::type::Vector2I goal(0, 0);

    auto& st = g.ref(goal);
    st.terminated = cor::rtrue;

    auto path = g.search_nearest_path(start);
    auto rpath = path.path;
    std::reverse(rpath.begin(), rpath.end());

    auto oc = rpath.front();
    BOOST_CHECK((start - oc).get_magnitude() < 0.1f);
    BOOST_CHECK((goal - rpath.back()).get_magnitude() < 0.1f);

    for(auto v : rpath)
    {
        auto& c = g.ref(v);
        c.enter_cost = 2.0f;

        BOOST_CHECK((v - oc).get_magnitude() < 1.5f);
        oc = v;

        BOOST_CHECK(c.passable);
    }

    {
        cor::RInt32 i, isz;
        cor::RInt32 j, jsz;
        auto sz = g.get_size();
        isz = sz.y;
        jsz = sz.x;

        for(i = 0 ; i < isz ; i++)
        {
            cor::RStringStream s;
            for(j = 0 ; j < jsz ; j++)
            {
                cor::type::Vector2I cp(j, i);
                auto& c = g.ref(cp);
                if((cp - start).get_magnitude() < 0.1)
                {
                    s << "s";
                }
                else if((cp - goal).get_magnitude() < 0.1)
                {
                    s << "g";
                }
                else if(!c.passable)
                {
                    s << "#";
                }
                else if(c.enter_cost > 1.5f)
                {
                    s << "*";
                }
                else
                {
                    s << " ";
                }
                c.enter_cost = 1.0f;
            }
            //cor::log_debug(s.str());
        }
    }

    {
        auto path = g.search_nearest_path(start);
        auto rpath = path.path;
        std::reverse(rpath.begin(), rpath.end());

        auto oc = rpath.front();
        BOOST_CHECK((start - oc).get_magnitude() < 0.1f);
        BOOST_CHECK((goal - rpath.back()).get_magnitude() < 0.1f);

        for(auto v : rpath)
        {
            auto& c = g.ref(v);
            c.enter_cost = 2.0f;

            BOOST_CHECK((v - oc).get_magnitude() < 1.5f);
            oc = v;

            BOOST_CHECK(c.passable);
        }

        {
            cor::RInt32 i, isz;
            cor::RInt32 j, jsz;
            auto sz = g.get_size();
            isz = sz.y;
            jsz = sz.x;

            for(i = 0 ; i < isz ; i++)
            {
                cor::RStringStream s;
                for(j = 0 ; j < jsz ; j++)
                {
                    cor::type::Vector2I cp(j, i);
                    auto& c = g.ref(cp);
                    if((cp - start).get_magnitude() < 0.1)
                    {
                        s << "s";
                    }
                    else if((cp - goal).get_magnitude() < 0.1)
                    {
                        s << "g";
                    }
                    else if(!c.passable)
                    {
                        s << "#";
                    }
                    else if(c.enter_cost > 1.5f)
                    {
                        s << "*";
                    }
                    else
                    {
                        s << " ";
                    }
                    c.enter_cost = 1.0f;
                }
                //cor::log_debug(s.str());
            }
        }
    }

}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <vector>
#include <Eigen/Dense>

using namespace Eigen;

template<class MatrixType>
struct Reftable {
    size_t nrec;
    std::vector<size_t> nrecscen;
    std::vector<size_t> nparam;
    std::vector<std::string> params_names;
    std::vector<std::string> stats_names;
    MatrixType stats;
    MatrixType params;
    std::vector<double> scenarios;
//    Reftable(MatrixType _stats, MatrixType _params) : stats(_stats), params(_params) {}
    Reftable(size_t _nrec,
    std::vector<size_t> _nrecscen,
    std::vector<size_t> _nparam,
    std::vector<std::string> _params_names,
    std::vector<std::string> _stats_names,
    MatrixType _stats,
    MatrixType _params,
    std::vector<double> _scenarios) : nrec(_nrec),
                nrecscen(_nrecscen),
                nparam(_nparam),
                params_names(_params_names),
                stats_names(_stats_names),
                stats(_stats),
                params(_params),
                scenarios(_scenarios) {}

};


#define BOOST_TEST_DYN_LINK

// clang-format off
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
// clang-format on

#include <nuschl/builtins.hpp>
#include <nuschl/s_exp.hpp>
#include <nuschl/memory/s_exp_pool.hpp>

#include <nuschl/unittests/helper.hpp>
#include <nuschl/unittests/vector_printer.hpp>

#include <sstream>
#include <memory>

using namespace nuschl;
using namespace std::string_literals;
namespace bdata = boost::unit_test::data;

BOOST_AUTO_TEST_SUITE(BuiltinsPlus)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("+", primitives::plus.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::plus)>(primitives::plus);
}

BOOST_AUTO_TEST_CASE(Emtpy) {
    std::vector<s_exp_ptr> args;
    auto res = primitives::plus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    auto res = primitives::plus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(One) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    auto res = primitives::plus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(1, &pool));
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(2, &pool));
    args.push_back(make_number(3, &pool));
    auto res = primitives::plus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(6, &pool));
}

BOOST_AUTO_TEST_CASE(NotANumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(pool.create(make_atom("fo")));
    args.push_back(make_number(3, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::plus.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "+ expects only numbers as arguments."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsMinus)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("-", primitives::minus.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::minus)>(primitives::minus);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    auto res = primitives::minus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}
BOOST_AUTO_TEST_CASE(Minus) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    auto res = primitives::minus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(One) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    auto res = primitives::minus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(-1, &pool));
}

BOOST_AUTO_TEST_CASE(SimpleMinus) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(1, &pool));
    auto res = primitives::minus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(LongMinus) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    args.push_back(make_number(1, &pool));
    auto res = primitives::minus.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(NotANumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(pool.create(make_atom("fo")));
    args.push_back(make_number(3, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::minus.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "- expects only numbers as arguments."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsTimes)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("*", primitives::times.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::times)>(primitives::times);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    auto res = primitives::times.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(1, &pool));
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    auto res = primitives::times.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(0, &pool));
}

BOOST_AUTO_TEST_CASE(OneElement) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(4, &pool));
    auto res = primitives::times.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(4, &pool));
}

BOOST_AUTO_TEST_CASE(TwoElements) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(4, &pool));
    args.push_back(make_number(2, &pool));
    auto res = primitives::times.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(8, &pool));
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(4, &pool));
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    auto res = primitives::times.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(24, &pool));
}

BOOST_AUTO_TEST_CASE(NotANumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(pool.create(make_atom("fo")));
    args.push_back(make_number(3, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::times.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "* expects only numbers as arguments."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsDivide)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("/", primitives::divide.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::divide)>(primitives::divide);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::divide.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "/ expects exactly 2 arguments, got 0."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::divide.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "/ expects exactly 2 arguments, got 1."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(TwoElements) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(4, &pool));
    args.push_back(make_number(2, &pool));
    auto res = primitives::divide.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(2, &pool));
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::divide.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "/ expects exactly 2 arguments, got 3."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsEQ)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("eq", primitives::eq.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::eq)>(primitives::eq);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::eq.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "eq expects exactly 2 arguments, got 0."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::eq.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "eq expects exactly 2 arguments, got 1."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::eq.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "eq expects exactly 2 arguments, got 3."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(NumbersEqual) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(2, &pool));
    args.push_back(make_number(2, &pool));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::tru);
}

BOOST_AUTO_TEST_CASE(NumbersNotEqual) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(4, &pool));
    args.push_back(make_number(2, &pool));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::fals);
}

BOOST_AUTO_TEST_CASE(SymbolsEqual) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_atom("foo")));
    args.push_back(pool.create(make_atom("foo")));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::tru);
}

BOOST_AUTO_TEST_CASE(SymbolsNotEqual) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_atom("foo"s)));
    args.push_back(pool.create(make_atom("bar"s)));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::fals);
}

BOOST_AUTO_TEST_CASE(SymbolsNotEqualNumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_atom("1"s)));
    args.push_back(pool.create(make_atom(number{1})));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::fals);
}

BOOST_AUTO_TEST_CASE(NilEqEmptyList) {
    std::vector<s_exp_ptr> args;
    args.push_back(s_exp::nil);
    args.push_back(pool.create(s_exp::nil, s_exp::nil));
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, s_exp::tru);
}

std::vector<s_exp_ptr> val = {
    s_exp::nil,
    pool.create(s_exp::nil, s_exp::nil),
    s_exp::tru,
    s_exp::fals,
    make_number(1, &pool),
    pool.create(make_atom("a")),
    pool.create(pool.create(make_atom("a")), pool.create(make_atom("a"))),
    pool.create(pool.create(make_atom("a")), s_exp::nil)};

std::vector<s_exp_ptr> exp = {s_exp::tru,  s_exp::tru,  s_exp::fals,
                              s_exp::fals, s_exp::fals, s_exp::fals,
                              s_exp::fals, s_exp::fals};

BOOST_DATA_TEST_CASE(Mix, bdata::make(val) ^ bdata::make(exp), a, e) {
    std::vector<s_exp_ptr> args;
    args.push_back(a);
    args.push_back(s_exp::nil);
    auto res = primitives::eq.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, e);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsLess)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("<", primitives::less.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::less)>(primitives::less);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::less.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "< expects exactly 2 arguments, got 0."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::less.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "< expects exactly 2 arguments, got 1."s == e.what();
        });
}

std::vector<s_exp_ptr> val1 = {make_number(1, &pool), make_number(1, &pool),
                               make_number(2, &pool)};

std::vector<s_exp_ptr> val2 = {make_number(2, &pool), make_number(1, &pool),
                               make_number(1, &pool)};

std::vector<s_exp_ptr> exp = {s_exp::tru, s_exp::fals, s_exp::fals};

BOOST_DATA_TEST_CASE(Less,
                     bdata::make(val1) ^ bdata::make(val2) ^ bdata::make(exp),
                     a, b, e) {
    std::vector<s_exp_ptr> args;
    args.push_back(a);
    args.push_back(b);
    auto res = primitives::less.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, e);
}

BOOST_AUTO_TEST_CASE(NotANumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_atom("a")));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::less.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "< expects only numbers as arguments."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::less.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "< expects exactly 2 arguments, got 3."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsGreater)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL(">", primitives::greater.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::greater)>(primitives::greater);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::greater.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "> expects exactly 2 arguments, got 0."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(Zero) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(0, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::greater.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "> expects exactly 2 arguments, got 1."s == e.what();
        });
}

std::vector<s_exp_ptr> val1 = {make_number(1, &pool), make_number(1, &pool),
                               make_number(2, &pool)};

std::vector<s_exp_ptr> val2 = {make_number(2, &pool), make_number(1, &pool),
                               make_number(1, &pool)};

std::vector<s_exp_ptr> exp = {s_exp::fals, s_exp::fals, s_exp::tru};

BOOST_DATA_TEST_CASE(Less,
                     bdata::make(val1) ^ bdata::make(val2) ^ bdata::make(exp),
                     a, b, e) {
    std::vector<s_exp_ptr> args;
    args.push_back(a);
    args.push_back(b);
    auto res = primitives::greater.execute(args, &pool);
    BOOST_CHECK_EQUAL(res, e);
}

BOOST_AUTO_TEST_CASE(NotANumber) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_atom("a")));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::greater.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "> expects only numbers as arguments."s == e.what();
        });
}

BOOST_AUTO_TEST_CASE(List) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(make_number(3, &pool));
    args.push_back(make_number(2, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::greater.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "> expects exactly 2 arguments, got 3."s == e.what();
        });
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsCar)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("car", primitives::car.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::car)>(primitives::car);
}

BOOST_AUTO_TEST_CASE(car) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_number(12, &pool), make_number(3, &pool)));
    auto res = primitives::car.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(12, &pool));
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::car.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "car expects exactly 1 arguments, got 0."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(Nil) {
    std::vector<s_exp_ptr> args;
    args.push_back(s_exp::nil);
    BOOST_CHECK_EXCEPTION(
        primitives::car.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "car expects a list as argument."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(Number) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::car.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "car expects a list as argument."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(ToManyArguments) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(1, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::car.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "car expects exactly 1 arguments, got 2."s == e.what();
        });
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsCdr)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("cdr", primitives::cdr.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::cdr)>(primitives::cdr);
}

BOOST_AUTO_TEST_CASE(cdr) {
    std::vector<s_exp_ptr> args;
    args.push_back(pool.create(make_number(12, &pool), make_number(3, &pool)));
    auto res = primitives::cdr.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *make_number(3, &pool));
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::cdr.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cdr expects exactly 1 arguments, got 0."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(Nil) {
    std::vector<s_exp_ptr> args;
    args.push_back(s_exp::nil);
    BOOST_CHECK_EXCEPTION(
        primitives::cdr.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cdr expects a list as argument."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(Number) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::cdr.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cdr expects a list as argument."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(ToManyArguments) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(1, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::cdr.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cdr expects exactly 1 arguments, got 2."s == e.what();
        });
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsCons)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL("cons", primitives::cons.representation());
}

BOOST_AUTO_TEST_CASE(Destructor) {
    std::make_unique<decltype(primitives::cons)>(primitives::cons);
}

BOOST_AUTO_TEST_CASE(cons) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(make_number(3, &pool));
    auto res = primitives::cons.execute(args, &pool);
    auto a = make_number(12, &pool);
    auto b = make_number(3, &pool);
    auto exp = pool.create(a, b);
    BOOST_CHECK_EQUAL(*res, *exp);
}

BOOST_AUTO_TEST_CASE(cons2) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(12, &pool));
    args.push_back(s_exp::nil);
    auto res = primitives::cons.execute(args, &pool);
    auto a = make_number(12, &pool);
    auto b = s_exp::nil;
    auto exp = pool.create(a, b);
    BOOST_CHECK_EQUAL(*res, *exp);
}

BOOST_AUTO_TEST_CASE(Empty) {
    std::vector<s_exp_ptr> args;
    BOOST_CHECK_EXCEPTION(
        primitives::cons.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cons expects exactly 2 arguments, got 0."s == e.what();
        });
}
BOOST_AUTO_TEST_CASE(Nil) {
    std::vector<s_exp_ptr> args;
    args.push_back(s_exp::nil);
    args.push_back(s_exp::nil);
    auto res = primitives::cons.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *s_exp::nil);
}

BOOST_AUTO_TEST_CASE(ToManyArguments) {
    std::vector<s_exp_ptr> args;
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(1, &pool));
    args.push_back(make_number(1, &pool));
    BOOST_CHECK_EXCEPTION(
        primitives::cons.execute(args, &pool), nuschl::eval_argument_error,
        [](const nuschl::eval_argument_error &e) {
            return "cons expects exactly 2 arguments, got 3."s == e.what();
        });
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(BuiltinsList)

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Destructor) { std::make_unique<primitives::list_impl>(); }

BOOST_AUTO_TEST_CASE(Representation) {
    BOOST_CHECK_EQUAL(primitives::list_impl{}.representation(), "list");
}

auto one = make_number(1, &pool);
auto two = make_number(2, &pool);
auto three = make_number(3, &pool);

std::vector<std::vector<s_exp_ptr>> val = {
    {}, {one}, {one, two}, {three, one, two}};

auto list1 = pool.create(one, s_exp::nil);
auto list2 = pool.create(one, pool.create(two, s_exp::nil));
auto list3 = pool.create(three, list2);

std::vector<s_exp_ptr> exp = {s_exp::nil, list1, list2, list3};

BOOST_DATA_TEST_CASE(List, bdata::make(val) ^ bdata::make(exp), args, e) {
    auto res = primitives::list.execute(args, &pool);
    BOOST_CHECK_EQUAL(*res, *e);
}

BOOST_AUTO_TEST_SUITE_END()