// This file is part of snark, a generic and flexible library
// for robotics research.
//
// Copyright (C) 2011 The University of Sydney
//
// snark is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// snark is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
// for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with snark. If not, see <http://www.gnu.org/licenses/>.

#include "rectified.h"
#include <boost/thread/thread_time.hpp>
#include <opencv/cv.h>

namespace snark { namespace imaging {

rectified::rectified ( const snark::imaging::camera_parser& left, const snark::imaging::camera_parser& right, unsigned int width, unsigned int height, const comma::csv::options& csv, bool input_rectified ):
    m_rotation( right.rotation() * left.rotation().transpose() ),
    m_translation( right.translation() - left.translation() ),
    m_rectify( left.camera(), left.distortion(), right.camera(), right.distortion(), width, height, m_rotation, m_translation, input_rectified ),
    m_input_rectified( input_rectified ),
    m_serialization( csv.fields, csv.format() )
{

}

rectified::rectified ( const camera_parser& left, const camera_parser& right,
                 const cv::Mat& left_x, const cv::Mat& left_y, const cv::Mat& right_x, const cv::Mat& right_y,
                 const comma::csv::options& csv, bool input_rectified ):
    m_rotation( Eigen::Matrix3d::Identity() ),
    m_translation( right.translation() - left.translation() ),
    m_rectify ( left.camera(), right.camera(), m_translation, left_x, left_y, right_x, right_y, input_rectified ),
    m_input_rectified( input_rectified ),
    m_serialization( csv.fields, csv.format() )
{

}

cv::Mat rectified::concatenate( const cv::Mat& left, const cv::Mat& right )
{
    cv::Size sz_left = left.size();
    cv::Size sz_right = right.size();
    cv::Mat concatenated( sz_left.height, sz_left.width + sz_right.width, left.type() );
    cv::Mat left_roi = cv::Mat( concatenated, cv::Rect(0, 0,  left.cols, left.rows) );
    cv::Mat right_roi = cv::Mat( concatenated, cv::Rect(left.cols, 0,  right.cols, right.rows) );
    left.copyTo( left_roi );
    right.copyTo( right_roi );
    return concatenated;
}

void rectified::process( const cv::Mat& left, const cv::Mat& right, const cv::StereoSGBM& sgbm, boost::posix_time::ptime time )
{
    snark::imaging::point_cloud cloud( sgbm );

    if (!m_input_rectified)
    {
        cv::Mat leftRectified = m_rectify.remap_left( left );
        cv::Mat rightRectified = m_rectify.remap_right( right );
        m_serialization.write( std::cout, std::make_pair( time, concatenate( leftRectified, rightRectified ) ) );
    }
    else
    {
        m_serialization.write( std::cout, std::make_pair( time, concatenate( left, right ) ) );
    }
}

} }


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_GREATER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-predicates
    This function object returns @ref True or @ref False according
    x is greater than y or not.

    Infix notation can be used with operator '>'.

    @par Header <boost/simd/function/is_greater.hpp>

    @par Note

    Using `is_greater(x,y)` is equivalent to `x > y`

    @see  is_greater_equal, is_gtz, is_not_less_equal, is_not_less

    @par Example:

      @snippet is_greater.cpp is_greater

    @par Possible output:

      @snippet is_greater.txt is_greater

  **/
  as_logical_t<Value> is_greater(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/is_greater.hpp>
#include <boost/simd/function/simd/is_greater.hpp>

#endif


/**
* Copyright (c) 2020 Intel Corporation
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

/*
DynaQ-Learning unit test. Learn the best path out of a simple maze.

5 == Outside the maze
________________________________________________
|                       |                       |
|                       |                       |
|           0           |          1             / 5
|                       |                       |
|____________/  ________|__/  __________________|_______________________
|                       |                       |                       |
|                       |                        /                      |
|           4           |          3            |           2           |
|                        /                      |                       |
|__/  __________________|_______________________|_______________________|
    5

The paths out of the maze:

0->4->5
0->4->3->1->5
1->5
1->3->4->5
2->3->1->5
2->3->4->5
3->1->5
3->4->5
4->5
4->3->1->5
*/

#define BOOST_TEST_MODULE test module name
#include <boost/test/included/unit_test.hpp>

#include <cstdint>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <random>
#include <vector>
#include <iostream>

#include "qformat.hpp"
#include "fixedPointTransferFunctions.hpp"
#include "neuralnet.hpp"
#include "qlearn.hpp"
#include "dynaq.hpp"

static std::random_device rd;

struct MazeEnvironmentRandomNumberGeneratorPolicy
{
    MazeEnvironmentRandomNumberGeneratorPolicy() : mRandomEngine(rd())
    {
    }

    size_t randInt(const size_t min, const size_t max)
    {
        std::uniform_int_distribution<size_t> distribution(min, max);
        const size_t value = distribution(this->mRandomEngine);

        return value;
    }
private:
    std::default_random_engine mRandomEngine;
};

template<typename ValueType>
struct RandomWeightGenerator
{
    static ValueType generateRandomWeight()
    {
        static std::default_random_engine generator(rd());
        static std::uniform_real_distribution<double> distribution(-1.0, 1.0);
        const double temp = distribution(generator);
        const typename ValueType::FullWidthValueType value = static_cast<typename ValueType::FullWidthValueType>(temp * static_cast<double>(1 << ValueType::NumberOfFractionalBits));
        const ValueType weight(value);

        return weight;
    }

private:
    static std::default_random_engine generator;
    static std::uniform_real_distribution<double> distribution;
};

template<typename ValueType, size_t NumberOfStates, size_t NumberOfActions>
struct MazeEnvironment : public tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>
{
    typedef tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy> ParentType;
    static const size_t EnvironmentNumberOfStates = tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>::EnvironmentNumberOfStates;
    static const size_t EnvironmentNumberOfActions = tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>::EnvironmentNumberOfActions;
    static const size_t EnvironmentInvalidState = tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>::EnvironmentInvalidState;
    static const size_t EnvironmentInvalidAction = tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>::EnvironmentInvalidAction;
    static const ValueType EnvironmentNoRewardValue;
    static const ValueType EnvironmentInvalidActionValue;

    MazeEnvironment(const ValueType& learningRate, const ValueType& discountFactor, const uint32_t randomActionDecisionPoint) :
        tinymind::QLearningEnvironment<ValueType, NumberOfStates, NumberOfActions, MazeEnvironmentRandomNumberGeneratorPolicy>(learningRate, discountFactor, randomActionDecisionPoint)
    {
    }

    size_t getNextStateForStateActionPair(const size_t state, const size_t action) const
    {
        return action;
    }

    size_t getValidActionsForState(const size_t state, size_t* pActions) const
    {
        size_t numberOfValidActions = 0;

        for(size_t action = 0;action < NumberOfActions;++action)
        {
            if(EnvironmentInvalidActionValue != this->getRewardForStateAndAction(state, action))
            {
                *pActions = action;
                ++pActions;
                ++numberOfValidActions;
            }
        }

        return numberOfValidActions;
    }

    void takeAction(const typename ParentType::experience_t& experience)
    {
        this->mExperience = experience;
    }

    typename ParentType::experience_t mExperience;
};

template<typename ValueType, size_t NumberOfStates, size_t NumberOfActions>
const ValueType MazeEnvironment<ValueType, NumberOfStates, NumberOfActions>::EnvironmentNoRewardValue = ValueType(0);

template<typename ValueType, size_t NumberOfStates, size_t NumberOfActions>
const ValueType MazeEnvironment<ValueType, NumberOfStates, NumberOfActions>::EnvironmentInvalidActionValue = ValueType(-1, 0);

#define NUMBER_OF_STATES 6
#define NUMBER_OF_ACTIONS 6

typedef tinymind::QValue<16,16,true> QValueType;
typedef MazeEnvironment<QValueType, NUMBER_OF_STATES, NUMBER_OF_ACTIONS> MazeEnvironmentType;
typedef tinymind::QLearner<MazeEnvironmentType> QLearnerType;
typedef tinymind::FixedPointTransferFunctions<QValueType, RandomWeightGenerator<QValueType> > TransferFunctionsType;
typedef tinymind::ElmanNetwork<TransferFunctionsType::TransferFunctionsValueType, 2, 3, 1, TransferFunctionsType> FixedPointElmanNetworkType;
typedef tinymind::DynaQLearner<QLearnerType, FixedPointElmanNetworkType> DynaQLearnerType;

static const QValueType learningRate = (QValueType(1,0) / QValueType(5,0));
static const QValueType discountFactor = (QValueType(1,0) / QValueType(2,0));
static QLearnerType qLearner(learningRate, discountFactor, 100);
static DynaQLearnerType dynaqLearner(learningRate, discountFactor, 100);
static const QValueType reward(100,0);

static void initializeRewardMatrix(void)
{
    size_t state;

    state = 0;
    for(size_t action = 0;action < 4;++action)
    {
        qLearner.getEnvironment().setRewardForStateAndAction(state, action, MazeEnvironmentType::EnvironmentInvalidActionValue);
    }
    qLearner.getEnvironment().setRewardForStateAndAction(state, 4, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 5, MazeEnvironmentType::EnvironmentInvalidActionValue);

    state = 1;
    for(size_t action = 0;action < 3;++action)
    {
        qLearner.getEnvironment().setRewardForStateAndAction(state, action, MazeEnvironmentType::EnvironmentInvalidActionValue);
    }
    qLearner.getEnvironment().setRewardForStateAndAction(state, 3, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 4, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 5, reward);

    state = 2;
    for(size_t action = 0;action < NUMBER_OF_ACTIONS;++action)
    {
        qLearner.getEnvironment().setRewardForStateAndAction(state, action, MazeEnvironmentType::EnvironmentInvalidActionValue);
    }
    qLearner.getEnvironment().setRewardForStateAndAction(state, 3, MazeEnvironmentType::EnvironmentNoRewardValue);

    state = 3;
    qLearner.getEnvironment().setRewardForStateAndAction(state, 0, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 1, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 2, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 3, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 4, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 5, MazeEnvironmentType::EnvironmentInvalidActionValue);

    state = 4;
    qLearner.getEnvironment().setRewardForStateAndAction(state, 0, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 1, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 2, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 3, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 4, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 5, reward);

    state = 5;
    qLearner.getEnvironment().setRewardForStateAndAction(state, 0, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 1, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 2, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 3, MazeEnvironmentType::EnvironmentInvalidActionValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 4, MazeEnvironmentType::EnvironmentNoRewardValue);
    qLearner.getEnvironment().setRewardForStateAndAction(state, 5, reward);
}

BOOST_AUTO_TEST_SUITE(test_suite_dynaqlearn)

BOOST_AUTO_TEST_CASE(test_dynaqlearn_init)
{
    BOOST_TEST(QValueType(0) == qLearner.getAccumulatedReward());

    for(size_t state = 0;state < NUMBER_OF_STATES;++state)
    {
        for(size_t action = 0;action < NUMBER_OF_ACTIONS;++action)
        {
            BOOST_TEST(static_cast<QValueType>(0) == qLearner.getQTableValue(state, action));
            BOOST_TEST(static_cast<QValueType>(0) == qLearner.getEnvironment().getRewardForStateAndAction(state, action));
        }
    }
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_setreward)
{
    initializeRewardMatrix();

    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(0, 4));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(1, 3));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 3));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 4));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(4, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(4, 3));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(5, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == qLearner.getEnvironment().getRewardForStateAndAction(5, 4));

    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(0, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(0, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(0, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(0, 5));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(1, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(1, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(1, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(1, 4));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 4));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(2, 5));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 3));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(3, 5));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(4, 1));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(4, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(4, 4));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(5, 0));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(5, 2));
    BOOST_TEST(MazeEnvironmentType::EnvironmentInvalidActionValue == qLearner.getEnvironment().getRewardForStateAndAction(5, 3));

    BOOST_TEST(reward == qLearner.getEnvironment().getRewardForStateAndAction(1, 5));
    BOOST_TEST(reward == qLearner.getEnvironment().getRewardForStateAndAction(4, 5));
    BOOST_TEST(reward == qLearner.getEnvironment().getRewardForStateAndAction(5, 5));
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_text_next_state)
{
    BOOST_TEST(4 == qLearner.getEnvironment().getNextStateForStateActionPair(0, 4));
    BOOST_TEST(3 == qLearner.getEnvironment().getNextStateForStateActionPair(1, 3));
    BOOST_TEST(5 == qLearner.getEnvironment().getNextStateForStateActionPair(1, 5));
    BOOST_TEST(3 == qLearner.getEnvironment().getNextStateForStateActionPair(2, 3));
    BOOST_TEST(1 == qLearner.getEnvironment().getNextStateForStateActionPair(3, 1));
    BOOST_TEST(2 == qLearner.getEnvironment().getNextStateForStateActionPair(3, 2));
    BOOST_TEST(4 == qLearner.getEnvironment().getNextStateForStateActionPair(3, 4));
    BOOST_TEST(0 == qLearner.getEnvironment().getNextStateForStateActionPair(4, 0));
    BOOST_TEST(3 == qLearner.getEnvironment().getNextStateForStateActionPair(4, 3));
    BOOST_TEST(5 == qLearner.getEnvironment().getNextStateForStateActionPair(4, 5));
    BOOST_TEST(1 == qLearner.getEnvironment().getNextStateForStateActionPair(5, 1));
    BOOST_TEST(4 == qLearner.getEnvironment().getNextStateForStateActionPair(5, 4));
    BOOST_TEST(5 == qLearner.getEnvironment().getNextStateForStateActionPair(5, 5));
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_change_learning_rate)
{
    static const QValueType newLearningRate = (QValueType(1,0) / QValueType(100,0));
    BOOST_TEST(learningRate != newLearningRate);
    BOOST_TEST(learningRate == qLearner.getEnvironment().getLearningRate());
    qLearner.getEnvironment().setLearningRate(newLearningRate);
    BOOST_TEST(newLearningRate == qLearner.getEnvironment().getLearningRate());
    qLearner.getEnvironment().setLearningRate(learningRate);
    BOOST_TEST(learningRate == qLearner.getEnvironment().getLearningRate());
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_change_discount_factor)
{
    static const QValueType delta = (QValueType(1,0) / QValueType(100,0));
    static const QValueType discountFactor = qLearner.getEnvironment().getDiscountFactor();
    static const QValueType newDiscountFactor = (discountFactor + delta);

    BOOST_TEST(discountFactor != newDiscountFactor);
    qLearner.getEnvironment().setDiscountFactor(newDiscountFactor);
    BOOST_TEST(newDiscountFactor == qLearner.getEnvironment().getDiscountFactor());
    qLearner.getEnvironment().setDiscountFactor(discountFactor);
    BOOST_TEST(discountFactor == qLearner.getEnvironment().getDiscountFactor());
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_choose_random_action_test)
{
    const uint32_t oldDecisionPoint = qLearner.getEnvironment().getRandomActionDecisionPoint();
    std::vector<bool> choices;
    std::size_t t;

    qLearner.getEnvironment().setRandomActionDecisionPoint(50);
    for(auto i = 0;i < 1000;++i)
    {
        choices.push_back(qLearner.getEnvironment().shouldChooseRandomAction());
    }

    t = std::count_if(choices.begin(), choices.end(), [](const bool value){return value;});

    BOOST_TEST(((t >= 400) && (t <= 600)));

    qLearner.getEnvironment().setRandomActionDecisionPoint(90);
    choices.clear();
    for(auto i = 0;i < 1000;++i)
    {
        choices.push_back(qLearner.getEnvironment().shouldChooseRandomAction());
    }

    t = std::count_if(choices.begin(), choices.end(), [](const bool value){return value;});

    BOOST_TEST(((t >= 800) && (t <= 1000)));

    qLearner.getEnvironment().setRandomActionDecisionPoint(0);
    choices.clear();
    for(auto i = 0;i < 1000;++i)
    {
        choices.push_back(qLearner.getEnvironment().shouldChooseRandomAction());
    }

    t = std::count_if(choices.begin(), choices.end(), [](const bool value){return value;});

    BOOST_TEST(0 == t);

    qLearner.getEnvironment().setRandomActionDecisionPoint(100);
    choices.clear();
    for(auto i = 0;i < 1000;++i)
    {
        choices.push_back(qLearner.getEnvironment().shouldChooseRandomAction());
    }

    t = std::count_if(choices.begin(), choices.end(), [](const bool value){return value;});

    BOOST_TEST(1000 == t);

    qLearner.getEnvironment().setRandomActionDecisionPoint(oldDecisionPoint);
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_goal_state)
{
    BOOST_TEST(-1 == qLearner.getGoalState());

    qLearner.setGoalState(0);
    BOOST_TEST(0 == qLearner.getGoalState());

    qLearner.setGoalState(-1);
    BOOST_TEST(0 == qLearner.getGoalState());

    qLearner.setGoalState(QLearnerType::NumberOfStates);
    BOOST_TEST(0 == qLearner.getGoalState());

    qLearner.setGoalState(NUMBER_OF_STATES - 1);
    BOOST_TEST((NUMBER_OF_STATES - 1) == qLearner.getGoalState());
}

BOOST_AUTO_TEST_CASE(test_qlearn_argmax_best_action)
{
    typedef tinymind::QTablePolicy<MazeEnvironmentType> QValuePolicyType;
    QValuePolicyType qValuePolicy;
    QValueType qValue(0, 0);
    size_t state = 1;
    size_t actions[NUMBER_OF_ACTIONS] = { 0, 1, 2, 3, 4, 5 };
    size_t bestAction;

    for (size_t action = 0; action < NUMBER_OF_ACTIONS; ++action)
    {
        qValuePolicy.setQValue(state, action, qValue);
        ++qValue;
    }

    bestAction = tinymind::ArgMaxPolicy<MazeEnvironmentType, QValuePolicyType>::selectActionForState(state, &actions[0], NUMBER_OF_ACTIONS, qValuePolicy);
    BOOST_TEST(5 == bestAction);

    qValue = 0;
    for (size_t action = NUMBER_OF_ACTIONS; action > 0; --action)
    {
        qValuePolicy.setQValue(state, action - 1, qValue);
        ++qValue;
    }

    bestAction = tinymind::ArgMaxPolicy<MazeEnvironmentType, QValuePolicyType>::selectActionForState(state, &actions[0], NUMBER_OF_ACTIONS, qValuePolicy);
    BOOST_TEST(0 == bestAction);

    qValuePolicy.setQValue(state, 1, QValueType(100, 0));
    bestAction = tinymind::ArgMaxPolicy<MazeEnvironmentType, QValuePolicyType>::selectActionForState(state, &actions[0], NUMBER_OF_ACTIONS - 2, qValuePolicy);
    BOOST_TEST(1 == bestAction);
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_take_action_test)
{
    typename MazeEnvironmentType::ParentType::experience_t experience;
    size_t state = 1;
    size_t action = 5;

    experience.state = state;
    experience.action = action;
    qLearner.getEnvironment().takeAction(experience);
    experience.reward = qLearner.getEnvironment().getRewardForStateAndAction(experience.state, experience.action);
    experience.newState =  experience.action;
    qLearner.updateFromExperience(experience);

    BOOST_TEST(1 == experience.state);
    BOOST_TEST(5 == experience.action);
    BOOST_TEST(reward == experience.reward);
    BOOST_TEST(5 == experience.newState);

    state = 0;
    action = 4;

    experience.state = state;
    experience.action = action;
    qLearner.getEnvironment().takeAction(experience);
    experience.reward = qLearner.getEnvironment().getRewardForStateAndAction(experience.state, experience.action);
    experience.newState =  experience.action;
    qLearner.updateFromExperience(experience);

    BOOST_TEST(0 == experience.state);
    BOOST_TEST(4 == experience.action);
    BOOST_TEST(MazeEnvironmentType::EnvironmentNoRewardValue == experience.reward);
    BOOST_TEST(4 == experience.newState);
}

BOOST_AUTO_TEST_CASE(test_dynaqlearn_iterate_test)
{
    char const* const rewardFilePath = "qlearn_accum_reward.txt";
    char const* const qTableFilePath = "qtable.txt";
    typename MazeEnvironmentType::ParentType::experience_t experience;
    std::default_random_engine engine(rd());
    std::uniform_int_distribution<size_t> stateDistribution(0, NUMBER_OF_STATES - 1);
    size_t state;
    size_t decisionPoint = 100;
    std::ofstream rewardFile(rewardFilePath);
    std::ofstream qTableFile(qTableFilePath);

    for (auto i = 0; i < 500; ++i)
    {
        qLearner.getEnvironment().setRandomActionDecisionPoint(decisionPoint);

        qLearner.startNewEpisode();
        state = stateDistribution(engine);
        qLearner.setState(state);
        qLearner.setGoalState(5);
        qLearner.iterate();

        experience = qLearner.getEnvironment().mExperience;
        experience.reward = qLearner.getEnvironment().getRewardForStateAndAction(experience.state, experience.action);
        experience.newState =  experience.action;
        qLearner.updateFromExperience(experience);

        while (qLearner.getState() != 5)
        {
            qLearner.iterate();
            experience = qLearner.getEnvironment().mExperience;
            experience.reward = qLearner.getEnvironment().getRewardForStateAndAction(experience.state, experience.action);
            experience.newState =  experience.action;
            qLearner.updateFromExperience(experience);
        }

        rewardFile << qLearner.getAccumulatedReward().getValue() << std::endl;

        if (decisionPoint > 0)
        {
            decisionPoint -= 1;
        }
    }

    // print out q table
    for(auto state = 0;state < NUMBER_OF_STATES;++state)
    {
        qTableFile << state << ",";
        for(auto action = 0;action < NUMBER_OF_ACTIONS;++action)
        {
            qTableFile << qLearner.getQTableValue(state, action).getValue();
            if (action != (NUMBER_OF_ACTIONS - 1))
            {
                qTableFile << ",";
            }
        }

        qTableFile << std::endl;
    }

    qTableFile.close();
}
BOOST_AUTO_TEST_SUITE_END()


#ifndef __MAPNIK_VECTOR_TILE_GEOMETRY_CLIPPER_H__
#define __MAPNIK_VECTOR_TILE_GEOMETRY_CLIPPER_H__

// mapnik-vector-tile
#include "vector_tile_config.hpp"
#include "vector_tile_processor.hpp"

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/geometry.hpp>

// angus clipper
// http://www.angusj.com/delphi/clipper.php
#include "clipper.hpp"

// boost
#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/unique.hpp>
#pragma GCC diagnostic pop

namespace mapnik
{

namespace vector_tile_impl
{

namespace detail
{

inline ClipperLib::PolyFillType get_angus_fill_type(polygon_fill_type type)
{
    switch (type)
    {
    case polygon_fill_type_max:
    case even_odd_fill:
        return ClipperLib::pftEvenOdd;
    case non_zero_fill:
        return ClipperLib::pftNonZero;
    case positive_fill:
        return ClipperLib::pftPositive;
    case negative_fill:
        return ClipperLib::pftNegative;
    }
}

inline void process_polynode_branch(ClipperLib::PolyNode* polynode,
                                    mapnik::geometry::multi_polygon<std::int64_t> & mp,
                                    double area_threshold)
{
    mapnik::geometry::polygon<std::int64_t> polygon;
    polygon.set_exterior_ring(std::move(polynode->Contour));
    if (polygon.exterior_ring.size() > 2) // Throw out invalid polygons
    {
        double outer_area = ClipperLib::Area(polygon.exterior_ring);
        if (std::abs(outer_area) >= area_threshold)
        {
            // The view transform inverts the y axis so this should be positive still despite now
            // being clockwise for the exterior ring. If it is not lets invert it.
            if (outer_area < 0)
            {
                std::reverse(polygon.exterior_ring.begin(), polygon.exterior_ring.end());
            }

            // children of exterior ring are always interior rings
            for (auto * ring : polynode->Childs)
            {
                if (ring->Contour.size() < 3)
                {
                    continue; // Throw out invalid holes
                }
                double inner_area = ClipperLib::Area(ring->Contour);
                if (std::abs(inner_area) < area_threshold)
                {
                    continue;
                }

                if (inner_area > 0)
                {
                    std::reverse(ring->Contour.begin(), ring->Contour.end());
                }
                polygon.add_hole(std::move(ring->Contour));
            }
            mp.push_back(std::move(polygon));
        }
    }
    for (auto * ring : polynode->Childs)
    {
        for (auto * sub_ring : ring->Childs)
        {
            process_polynode_branch(sub_ring, mp, area_threshold);
        }
    }
}

} // end ns detail

template <typename NextProcessor>
class geometry_clipper
{
private:
    NextProcessor & next_;
    mapnik::box2d<int> const& tile_clipping_extent_;
    double area_threshold_;
    bool strictly_simple_;
    bool multi_polygon_union_;
    polygon_fill_type fill_type_;
    bool process_all_rings_;
public:
    geometry_clipper(mapnik::box2d<int> const& tile_clipping_extent,
                     double area_threshold,
                     bool strictly_simple,
                     bool multi_polygon_union,
                     polygon_fill_type fill_type,
                     bool process_all_rings,
                     NextProcessor & next) :
              next_(next),
              tile_clipping_extent_(tile_clipping_extent),
              area_threshold_(area_threshold),
              strictly_simple_(strictly_simple),
              multi_polygon_union_(multi_polygon_union),
              fill_type_(fill_type),
              process_all_rings_(process_all_rings)
    {
    }

    void operator() (mapnik::geometry::geometry_empty &)
    {
        return;
    }

    void operator() (mapnik::geometry::point<std::int64_t> & geom)
    {
        next_(geom);
    }

    void operator() (mapnik::geometry::multi_point<std::int64_t> & geom)
    {
        // Here we remove repeated points from multi_point
        auto last = std::unique(geom.begin(), geom.end());
        geom.erase(last, geom.end());
        next_(geom);
    }

    void operator() (mapnik::geometry::geometry_collection<std::int64_t> & geom)
    {
        for (auto & g : geom)
        {
            mapnik::util::apply_visitor((*this), g);
        }
    }

    void operator() (mapnik::geometry::line_string<std::int64_t> & geom)
    {
        boost::geometry::unique(geom);
        if (geom.size() < 2)
        {
            return;
        }
        //std::deque<mapnik::geometry::line_string<int64_t>> result;
        mapnik::geometry::multi_line_string<int64_t> result;
        mapnik::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        boost::geometry::intersection(clip_box, geom, result);
        if (result.empty())
        {
            return;
        }
        next_(result);
    }

    void operator() (mapnik::geometry::multi_line_string<std::int64_t> & geom)
    {
        if (geom.empty())
        {
            return;
        }

        mapnik::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        boost::geometry::unique(geom);
        mapnik::geometry::multi_line_string<int64_t> results;
        for (auto const& line : geom)
        {
            if (line.size() < 2)
            {
               continue;
            }
            boost::geometry::intersection(clip_box, line, results);
        }
        if (results.empty())
        {
            return;
        }
        next_(results);
    }

    void operator() (mapnik::geometry::polygon<std::int64_t> & geom)
    {
        if ((geom.exterior_ring.size() < 3) && !process_all_rings_)
        {
            return;
        }

        double clean_distance = 1.415;

        // Prepare the clipper object
        ClipperLib::Clipper clipper;

        if (strictly_simple_)
        {
            clipper.StrictlySimple(true);
        }

        // Start processing on exterior ring
        // if proces_all_rings is true even if the exterior
        // ring is invalid we will continue to insert all polygon
        // rings into the clipper
        ClipperLib::CleanPolygon(geom.exterior_ring, clean_distance);
        double outer_area = ClipperLib::Area(geom.exterior_ring);
        if ((std::abs(outer_area) < area_threshold_)  && !process_all_rings_)
        {
            return;
        }

        // The view transform inverts the y axis so this should be positive still despite now
        // being clockwise for the exterior ring. If it is not lets invert it.
        if (outer_area < 0)
        {
            std::reverse(geom.exterior_ring.begin(), geom.exterior_ring.end());
        }

        if (!clipper.AddPath(geom.exterior_ring, ClipperLib::ptSubject, true) && !process_all_rings_)
        {
            return;
        }

        for (auto & ring : geom.interior_rings)
        {
            if (ring.size() < 3)
            {
                continue;
            }
            ClipperLib::CleanPolygon(ring, clean_distance);
            double inner_area = ClipperLib::Area(ring);
            if (std::abs(inner_area) < area_threshold_)
            {
                continue;
            }
            // This should be a negative area, the y axis is down, so the ring will be "CCW" rather
            // then "CW" after the view transform, but if it is not lets reverse it
            if (inner_area > 0)
            {
                std::reverse(ring.begin(), ring.end());
            }
            if (!clipper.AddPath(ring, ClipperLib::ptSubject, true))
            {
                continue;
            }
        }

        // Setup the box for clipping
        mapnik::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(), tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(), tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(), tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(), tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(), tile_clipping_extent_.miny());

        // Finally add the box we will be using for clipping
        if (!clipper.AddPath( clip_box, ClipperLib::ptClip, true ))
        {
            return;
        }

        ClipperLib::PolyTree polygons;
        ClipperLib::PolyFillType fill_type = detail::get_angus_fill_type(fill_type_);
        clipper.Execute(ClipperLib::ctIntersection, polygons, fill_type, ClipperLib::pftEvenOdd);
        clipper.Clear();

        mapnik::geometry::multi_polygon<std::int64_t> mp;

        for (auto * polynode : polygons.Childs)
        {
            detail::process_polynode_branch(polynode, mp, area_threshold_);
        }

        if (mp.empty())
        {
            return;
        }
        next_(mp);
    }

    void operator() (mapnik::geometry::multi_polygon<std::int64_t> & geom)
    {
        if (geom.empty())
        {
            return;
        }

        double clean_distance = 1.415;
        mapnik::geometry::linear_ring<std::int64_t> clip_box;
        clip_box.reserve(5);
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.miny());
        clip_box.emplace_back(tile_clipping_extent_.maxx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.maxy());
        clip_box.emplace_back(tile_clipping_extent_.minx(),tile_clipping_extent_.miny());

        mapnik::geometry::multi_polygon<std::int64_t> mp;

        ClipperLib::Clipper clipper;

        if (strictly_simple_)
        {
            clipper.StrictlySimple(true);
        }

        ClipperLib::PolyFillType fill_type = detail::get_angus_fill_type(fill_type_);

        if (multi_polygon_union_)
        {
            for (auto & poly : geom)
            {
                // Below we attempt to skip processing of all interior rings if the exterior
                // ring fails a variety of sanity checks for size and validity for AddPath
                // When `process_all_rings_=true` this optimization is disabled. This is needed when
                // the ring order of input polygons is potentially incorrect and where the
                // "exterior_ring" might actually be an incorrectly classified exterior ring.
                if (poly.exterior_ring.size() < 3 && !process_all_rings_)
                {
                    continue;
                }
                ClipperLib::CleanPolygon(poly.exterior_ring, clean_distance);
                double outer_area = ClipperLib::Area(poly.exterior_ring);
                if ((std::abs(outer_area) < area_threshold_) && !process_all_rings_)
                {
                    continue;
                }
                // The view transform inverts the y axis so this should be positive still despite now
                // being clockwise for the exterior ring. If it is not lets invert it.
                if (outer_area < 0)
                {
                    std::reverse(poly.exterior_ring.begin(), poly.exterior_ring.end());
                }
                if (!clipper.AddPath(poly.exterior_ring, ClipperLib::ptSubject, true) && !process_all_rings_)
                {
                    continue;
                }

                for (auto & ring : poly.interior_rings)
                {
                    if (ring.size() < 3)
                    {
                        continue;
                    }
                    ClipperLib::CleanPolygon(ring, clean_distance);
                    double inner_area = ClipperLib::Area(ring);
                    if (std::abs(inner_area) < area_threshold_)
                    {
                        continue;
                    }
                    // This should be a negative area, the y axis is down, so the ring will be "CCW" rather
                    // then "CW" after the view transform, but if it is not lets reverse it
                    if (inner_area > 0)
                    {
                        std::reverse(ring.begin(), ring.end());
                    }
                    clipper.AddPath(ring, ClipperLib::ptSubject, true);
                }
            }
            if (!clipper.AddPath( clip_box, ClipperLib::ptClip, true ))
            {
                return;
            }
            ClipperLib::PolyTree polygons;
            clipper.Execute(ClipperLib::ctIntersection, polygons, fill_type, ClipperLib::pftEvenOdd);
            clipper.Clear();

            for (auto * polynode : polygons.Childs)
            {
                detail::process_polynode_branch(polynode, mp, area_threshold_);
            }
        }
        else
        {
            for (auto & poly : geom)
            {
                // Below we attempt to skip processing of all interior rings if the exterior
                // ring fails a variety of sanity checks for size and validity for AddPath
                // When `process_all_rings_=true` this optimization is disabled. This is needed when
                // the ring order of input polygons is potentially incorrect and where the
                // "exterior_ring" might actually be an incorrectly classified exterior ring.
                if (poly.exterior_ring.size() < 3 && !process_all_rings_)
                {
                    continue;
                }
                ClipperLib::CleanPolygon(poly.exterior_ring, clean_distance);
                double outer_area = ClipperLib::Area(poly.exterior_ring);
                if ((std::abs(outer_area) < area_threshold_) && !process_all_rings_)
                {
                    continue;
                }
                // The view transform inverts the y axis so this should be positive still despite now
                // being clockwise for the exterior ring. If it is not lets invert it.
                if (outer_area < 0)
                {
                    std::reverse(poly.exterior_ring.begin(), poly.exterior_ring.end());
                }
                if (!clipper.AddPath(poly.exterior_ring, ClipperLib::ptSubject, true) && !process_all_rings_)
                {
                    continue;
                }
                for (auto & ring : poly.interior_rings)
                {
                    if (ring.size() < 3)
                    {
                        continue;
                    }
                    ClipperLib::CleanPolygon(ring, clean_distance);
                    double inner_area = ClipperLib::Area(ring);
                    if (std::abs(inner_area) < area_threshold_)
                    {
                        continue;
                    }
                    // This should be a negative area, the y axis is down, so the ring will be "CCW" rather
                    // then "CW" after the view transform, but if it is not lets reverse it
                    if (inner_area > 0)
                    {
                        std::reverse(ring.begin(), ring.end());
                    }
                    if (!clipper.AddPath(ring, ClipperLib::ptSubject, true))
                    {
                        continue;
                    }
                }
                if (!clipper.AddPath( clip_box, ClipperLib::ptClip, true ))
                {
                    return;
                }
                ClipperLib::PolyTree polygons;
                clipper.Execute(ClipperLib::ctIntersection, polygons, fill_type, ClipperLib::pftEvenOdd);
                clipper.Clear();

                for (auto * polynode : polygons.Childs)
                {
                    detail::process_polynode_branch(polynode, mp, area_threshold_);
                }
            }
        }

        if (mp.empty())
        {
            return;
        }
        next_(mp);
    }
};

} // end ns vector_tile_impl

} // end ns mapnik

#endif // __MAPNIK_VECTOR_GEOMETRY_CLIPPER_H__


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include "Path.h"

#include <string>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>

#include <cmath>
#include <algorithm>

#include <Eigen/SVD>

/* For visualization/plotting only */
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#define PATH_DEBUG  0

namespace MPC
{

    Polynomial::Polynomial()
    {

    }

    // Copy constructor
    Polynomial::Polynomial(const Polynomial& poly) : coeffs_(poly.coeffs_)
    {

    }

    Polynomial::~Polynomial()
    {

    }

    Polynomial::Polynomial(unsigned int order) : coeffs_(order+1, 0.0)
    {

    }

    /* Coefficients are stored such that c[0] is the coefficient for the lowest order element, such that:
     * y = c[n]*x^n + c[n-1]*x^(n-1) + ... c[2]*x^2 + c[1]*x + c[0]
     */
    Polynomial::Polynomial(std::vector<double> coeffs)
    {
        coeffs_ = coeffs;
    }

    Polynomial::Polynomial(const double * coeffs, int num_coeffs)
    {
        coeffs_.insert(coeffs_.end(), coeffs, coeffs + num_coeffs);
    }

    // Assignment operator
    Polynomial& Polynomial::operator=(const Polynomial& other)
    {
        coeffs_ = other.coeffs_;
        return *this;
    }

    Polynomial Polynomial::operator+(const Polynomial& other) const
    {
        Polynomial out;

        if (order() >= other.order()) {
            out.coeffs_ = coeffs_;
            for (size_t i = 0; i < other.coeffs_.size(); i++)
                out.coeffs_.at(i) += other.coeffs_.at(i);
        }
        else {
            out.coeffs_ = other.coeffs_;
            for (size_t i = 0; i < coeffs_.size(); i++)
                out.coeffs_.at(i) += coeffs_.at(i);
        }

        return out;
    }

    Polynomial Polynomial::operator-(const Polynomial& other) const
    {
        Polynomial out;

        if (order() >= other.order()) {
            out.coeffs_ = coeffs_;
            for (size_t i = 0; i < other.coeffs_.size(); i++)
                out.coeffs_.at(i) -= other.coeffs_.at(i);
        }
        else {
            out.coeffs_ = other.coeffs_;
            for (size_t i = 0; i < coeffs_.size(); i++)
                out.coeffs_.at(i) -= coeffs_.at(i);
        }

        return out;
    }

    Polynomial Polynomial::operator+(const double& offset) const
    {
        Polynomial out;
        if (coeffs_.size() == 0) return out;

        out.coeffs_ = coeffs_;
        out.coeffs_[0] += offset;
        return out;
    }

    Polynomial Polynomial::operator-(const double& offset) const
    {
        Polynomial out;
        if (coeffs_.size() == 0) return out;

        out.coeffs_ = coeffs_;
        out.coeffs_[0] -= offset;
        return out;
    }

    void Polynomial::print()
    {
        std::cout << "Polynomial:" << std::endl;
        std::cout << "   y = ";
        if (order() < 0) {
            std::cout << "0" << std::endl << std::endl;
            return;
        }

        for (int i = order(); i >= 1; i--) {
            if (i > 1)
                std::cout << coeffs_.at(i) << "*t^" << i << " + ";
            else
                std::cout << coeffs_.at(i) << "*t + ";
        }
        std::cout << coeffs_.at(0) << std::endl;
        std::cout << std::endl;
    }

    /* Coefficients are stored such that c[0] is the coefficient for the lowest order element, such that:
     * out = c[n]*t^n + c[n-1]*t^(n-1) + ... c[2]*t^2 + c[1]*t + c[0]
     */
    double Polynomial::evaluate(double t)
    {
        // Polynomial evaluation
        double pow_t = 1;
        double out = 0;
        for (unsigned int i = 0; i < coeffs_.size(); i++) {
            out += coeffs_[i] * pow_t;
            pow_t *= t;
        }

        return out;
    }

    double Polynomial::operator()(double t)
    {
        return evaluate(t);
    }

    std::vector<double> Polynomial::evaluate(std::vector<double> tVec)
    {
        std::vector<double> evaluatedPoints;

        // Polynomial evaluation
        for (auto& t : tVec) {
            double pow_t = 1;
            double out = 0;
            for (unsigned int i = 0; i < coeffs_.size(); i++) {
                pow_t *= t;
                out += coeffs_[i] * pow_t;
            }

            evaluatedPoints.push_back(out);
        }

        return evaluatedPoints;
    }

    std::vector<double> Polynomial::operator()(std::vector<double> tVec)
    {
        return evaluate(tVec);
    }

    int Polynomial::order() const
    {
        return (int)coeffs_.size() - 1;
    }

    Polynomial Polynomial::squared() const
    {
        // Given polynomial coefficients for:
        // f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0
        // This function computes the coefficients of the polynomial f^2(x)
        // Coefficients are ordered such that coeffs[0] = c_0  (lowest order)

        // f^2(x) = sum(sum(c_i * c_j * x^(i+j))
        // The resulting coefficients, coeff_k = c_i*c_j | i+j=k
        int n = order(); // input polynomial order
        int m = 2*n; // squared (output) polynomial order

        if (m < 0) {
            Polynomial empty;
            return empty;
        }

        Polynomial squared(m);
        for (int k = 0; k <= m; k++) {
            for (int i = 0; i <= n; i++) {
                for (int j = 0; j <= n; j++) {
                    if (i + j == k) {
                        squared.coeffs_.at(k) = squared.coeffs_.at(k) + coeffs_.at(i) * coeffs_.at(j);
                    }
                }
            }
        }
        return squared;
    }

    Polynomial Polynomial::derivative() const
    {
        // Given polynomial coefficients for:
        // f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0
        // This function computes the coefficients of the polynomial f'(x)
        // f'(x) = df/dx = n*c_n*x^(n-1) + (n-1)*c_n-1*x^(n-2) + ... +
        // f'(x) = df/dx = n*c_n*x^(n-1) + (n-1)*c_n-1*x^(n-2) + ... + 2*c_2*x + 1*c_1
        // Coefficients are ordered such that coeff[0] = c_0  (lowest order)
        int n = order();

        if (n < 0) {
            Polynomial empty;
            return empty;
        }

        Polynomial derivative(n-1);
        for (int i = 0; i <= (n-1); i++) {
            derivative.coeffs_.at(i) = (i+1) * coeffs_.at(i+1);
        }
        return derivative;
    }

    double Polynomial::getCoefficient(unsigned int index)
    {
        if (int(index) > order())
            return 0;
        else
            return coeffs_.at(index);
    }

    double Polynomial::findMinimum(double s_init, double s_lower, double s_upper, double stoppingCriteria, unsigned int maxIterations)
    {
        // Find the minimum by Newton minimization
        if (order() < 0) return 0;

        Polynomial dPoly = derivative();
        Polynomial ddPoly = dPoly.derivative();

        // Newton's method - https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization
        bool converged = false;
        unsigned int iterations = 0;
        double s = s_init;
        double delta_s;

        while (!converged && iterations < maxIterations) {
            double FirstDerivative = dPoly.evaluate(s); // first derivative, for higher dimensions it would be the Gradient
            double SecondDerivative = ddPoly.evaluate(s); // second derivative, for higher dimensions it would be the Hessian

            delta_s = -FirstDerivative / SecondDerivative;
            s = s + delta_s;

            if (s < s_lower)
                s = s_lower;

            if (s > s_upper)
                s = s_upper;

            if (std::abs(delta_s) < stoppingCriteria)
                converged = true;

            iterations = iterations + 1;
        }

        return s;
    }

    void Polynomial::FitPoints(unsigned int order, std::vector<double>& tVec, std::vector<double>& values, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint)
    {
        if (order < 1) {
            std::cout << "Polynomial order needs to be at least 1" << std::endl; throw;
        }
        else if (order < 3 && EnforceBeginEndConstraint && EnforceBeginEndAngleConstraint) {
            std::cout << "Polynomial order too low to enforce both type of constraints" << std::endl; throw;
        }
        if (tVec.size() != values.size()) {
            std::cout << "Number of evaluation points (tVec) and corresponding values needs to be consistent" << std::endl; throw;
        }

        unsigned int n = tVec.size(); // should be the same length as values

        Eigen::VectorXd t = Eigen::VectorXd::Map(tVec.data(), tVec.size());
        Eigen::VectorXd tPow = Eigen::VectorXd::Ones(n, 1);

        Eigen::MatrixXd A(n, order+1);
        // Fill the matrix using the evaluation points, t
        for (unsigned int i = 0; i <= order; i++) {
            A.block(0, i, n, 1) = tPow;
            tPow = tPow.cwiseProduct(t); // tPow = tPow .* tVec
        }

        Eigen::VectorXd b = Eigen::VectorXd::Map(values.data(), values.size());

        // Create matrix and vector of equality constraints
        Eigen::MatrixXd Aeq;
        Eigen::VectorXd beq;
        if (EnforceBeginEndConstraint || EnforceBeginEndAngleConstraint) { // compute the constraint matrices
            Aeq.resize(4, order+1); // OBS. Note that resizing does not initialize values!
            beq.resize(4, 1);

            // First two rows enforces begin and end constraints
            Aeq(0,0) = 1;
            Aeq(1,0) = 1;
            // Last two rows enforces begin and end angle (derivative) constraints
            Aeq(2,0) = 0;
            Aeq(3,0) = 0;

            for (unsigned int j = 1; j <= order; j++) {
                Aeq(0, j) = Aeq(0, j-1) * t(0);
                Aeq(1, j) = Aeq(1, j-1) * t(n-1);
                Aeq(2, j) = Aeq(0, j-1) * j;
                Aeq(3, j) = Aeq(1, j-1) * j;
            }

            beq(0) = b(0); // first value
            beq(1) = b(n-1); // last value
            beq(2) = (b(1)-b(0)) / (t(1)-t(0)); // begin angle
            beq(3) = (b(n-1)-b(n-2)) / (t(n-1)-t(n-2)); // end angle
        }

#if PATH_DEBUG
        std::cout << "Aeq = " << std::endl << Aeq << std::endl;
        std::cout << "beq = " << std::endl << beq << std::endl;
#endif

        if (order < 3) { // can only enforce one type of the constraints
            // limit constraints due to reduced order such that we only require start and end point to be fulfilled
            if (EnforceBeginEndConstraint) {
                // Extract the first two rows
                Aeq = Aeq.block(0, 0, 2, order+1);
                beq = beq.block(0, 0, 2, 2);
            } else if (EnforceBeginEndAngleConstraint) {
                // Extract the last two rows
                Aeq = Aeq.block(2, 0, 2, order+1);
                beq = beq.block(2, 0, 2, 2);
            }
        }

        Eigen::VectorXd polyCoeffs;
        if (EnforceBeginEndConstraint || EnforceBeginEndAngleConstraint)
            polyCoeffs = ConstrainedLeastSquares(A, b, Aeq, beq, n*10000);
        else
            polyCoeffs = ConstrainedLeastSquares(A, b, Eigen::MatrixXd(), Eigen::VectorXd(), 1);

#if PATH_DEBUG
        std::cout << "polyCoeffs = " << std::endl << polyCoeffs << std::endl;
#endif

        coeffs_.clear();
        coeffs_.insert(coeffs_.begin(), polyCoeffs.data(), polyCoeffs.data() + polyCoeffs.rows());
    }

    Eigen::VectorXd Polynomial::ConstrainedLeastSquares(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::MatrixXd& Aeq, const Eigen::VectorXd& beq, double lambda)
    {
        Eigen::MatrixXd A_;
        Eigen::VectorXd b_;

        if (Aeq.rows() == beq.rows() && Aeq.cols() == A.cols()) {
            A_.resize(A.rows() + Aeq.rows(), A.cols()); // OBS. Note that resizing does not initialize values!
            b_.resize(b.rows() + beq.rows());
            A_ << A,
                  Aeq*lambda;
            b_ << b,
                  beq*lambda;
        } else {
            A_ = A;
            b_ = b;
        }

#if PATH_DEBUG
        std::cout << "A_ = " << std::endl << A_ << std::endl;
#endif

        // Pseudo-inverse through numerically unstable way
        //   A_invpseudo = inv(A_'*A_) * A_'
        // Pseudo-inverse through MATLAB
        //   A_invpseudo2 = pinv(A_)  % Moore-Penrose Pseudoinverse of matrix of A
        // Pseudo-inverse through SVD
        //   [U,S,V] = svd(A_);
        //   Sinv = [diag(1./diag(S)), zeros(size(S,2), size(S,1)-size(S,2))];
        //   A_invpseudo3 = V * Sinv * U';
        //   x = A_invpseudo3 * b_;

        Eigen::JacobiSVD<Eigen::MatrixXd> svd( A_, Eigen::ComputeFullV | Eigen::ComputeFullU );

        // create a matrix of just zeros and fill top part with inverse singular values in a diagonal matrix
        Eigen::MatrixXd Sinv = Eigen::MatrixXd::Zero(svd.singularValues().rows(), A_.rows());
        Sinv.block(0, 0, svd.singularValues().rows(), svd.singularValues().rows()) = svd.singularValues().asDiagonal().inverse().toDenseMatrix();
#if PATH_DEBUG
        std::cout << "Sinv = " << std::endl << Sinv << std::endl;
#endif

        Eigen::MatrixXd A_PseudoInverse = svd.matrixV() * Sinv * svd.matrixU().transpose();
#if PATH_DEBUG
        std::cout << "A_PseudoInverse = " << std::endl << A_PseudoInverse << std::endl;
#endif

        // Now get the solution (least squares minimization) by using the pseudo-inverse
        Eigen::VectorXd x = A_PseudoInverse * b_;
#if PATH_DEBUG
        std::cout << "x = " << std::endl << x << std::endl;
#endif

        return x;
    }

    Path::Path() : computed_dsquared_(false), s_end_(0)
    {

    }

    Path::Path(Polynomial& poly_x, Polynomial& poly_y, double s_end) : poly_x_(poly_x), poly_y_(poly_y), s_end_(s_end), computed_dsquared_(false)
    {

    }

    Path::Path(Trajectory& trajectory, unsigned int approximationOrder, bool StopAtEnd, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint) : computed_dsquared_(false)
    {
        FitTrajectory(trajectory, approximationOrder, StopAtEnd, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
    }

    Path::~Path()
    {

    }

    // Assignment operator
    Path& Path::operator=(const Path& other)
    {
        poly_x_ = other.poly_x_;
        poly_y_ = other.poly_y_;
        s_end_ = other.s_end_;
        computed_dsquared_ = other.computed_dsquared_;
        dsquared_ = other.dsquared_;
        return *this;
    }

    Eigen::Vector2d Path::get(double s)
    {
        return Eigen::Vector2d(poly_x_.evaluate(s), poly_y_.evaluate(s));
    }

    Eigen::Vector2d Path::operator()(double s)
    {
        return get(s);
    }

    std::vector<Eigen::Vector2d> Path::get(std::vector<double> sVec)
    {
        std::vector<Eigen::Vector2d> evaluatedPoints;

        // Polynomial evaluation
        for (auto& s : sVec) {
            evaluatedPoints.push_back(Eigen::Vector2d(poly_x_.evaluate(s), poly_y_.evaluate(s)));
        }

        return evaluatedPoints;
    }

    std::vector<Eigen::Vector2d> Path::operator()(std::vector<double> sVec)
    {
        return get(sVec);
    }

    int Path::order()
    {
        if (poly_x_.order() >= poly_y_.order())
            return poly_x_.order();
        else
            return poly_y_.order();
    }

    double Path::getXcoefficient(unsigned int index)
    {
        return poly_x_.getCoefficient(index);
    }

    double Path::getYcoefficient(unsigned int index)
    {
        return poly_y_.getCoefficient(index);
    }

    double Path::ArcCurveLength(double t)
    {
        // Arc curve length of a polynomial path whose position is defined as:
        //   p(t) = [x(t), y(t)]
        // Is given by the integral:
        // integral( sqrt( dxdt^2 + dydt^2 ) ) dt
        // This can however be approximated by using the velocity polynomial
        //   Q = sqrt( (df_x/dx)^2 + (df_dy)^2 );

        // First we compute the coefficients of the inner polynomial, f
        //   f = (df_x/dx)^2 + (df_dy)^2
        if (!computed_dsquared_) {
            Polynomial dx = poly_x_.derivative(); // taking the difference of a polynomial, moves the coefficients
            Polynomial dy = poly_y_.derivative();
            Polynomial dx_squared = dx.squared();
            Polynomial dy_squared = dy.squared();
            dsquared_ = dx_squared + dy_squared;
            computed_dsquared_ = true;
        }
        // Such that
        // Q = sqrt(EvaluatePolynomial(f_coeff, t))

        // Return the approximated arc curve length at point t - hence curve length from f(0) to f(t)
        // Based on http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.5.2912&rep=rep1&type=pdf
        // The approximation (see paper above) is given by
        // s(t) = t/2 * (5/9 * Q(1.774597*t/2) + 8/9 * Q(t/2) + 5/9 * Q(0.225403*t/2))
        double s = t/2.0 * (
                            5.0/9.0 * sqrt(dsquared_.evaluate(1.774597*t/2.0)) +
                            8.0/9.0 * sqrt(dsquared_.evaluate(t/2.0)) +
                            5.0/9.0 * sqrt(dsquared_.evaluate(0.225403*t/2.0))
                           );
        return s;
    }

    double Path::ApproximateArcCurveLength(double t, double discretizationStepSize)
    {
        double distance = 0;
        double t_current = 0;

        double x_prev = poly_x_.evaluate(t_current);
        double y_prev = poly_y_.evaluate(t_current);
        double x, y;
        double x_diff, y_diff;

        while (t_current < t) {
            t_current += discretizationStepSize;
            x = poly_x_.evaluate(t_current);
            y = poly_y_.evaluate(t_current);

            x_diff = x - x_prev;
            y_diff = y - y_prev;

            distance += sqrtf(x_diff*x_diff + y_diff*y_diff); // Euclidean distance

            x_prev = x;
            y_prev = y;
        }

        return distance;
    }

    void Path::plot(bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double aspect_ratio = (x_max - x_min) / (y_max - y_min);
        double xres, yres;
        if (aspect_ratio >= 1) {
            yres = 500;
            xres = yres * aspect_ratio;
        } else {
            xres = 500;
            yres = xres / aspect_ratio;
        }

        // Create black empty images
        cv::Mat image;
        if (drawXup)
            image = cv::Mat( xres, yres, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
        else
            image = cv::Mat( yres, xres, CV_8UC3, cv::Scalar( 255, 255, 255 ) );

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        if (s_end_ <= 0) return; // can not plot if length is unknown
        double s_spacing = s_end_ / 99.0;

        Eigen::Vector2d p_prev = get(0);
        Eigen::Vector2d p;

        for (unsigned int s_idx = 0; s_idx < 100; s_idx++) {
            double s = s_spacing * s_idx;
            p = get(s);

            float x = (p[0]-x_min) * scale_x;
            float y = (p[1]-y_min) * scale_y;

            if (x >= 0 && x < xres && y >= 0 && y < yres) {
                cv::Point point;
                if (drawXup) // draw with robot x-axis pointing up in plot
                    point = cv::Point(yres-y,xres-x);
                else
                    point = cv::Point(x,yres-y);

                // Draw a line
                //cv::line(image, cv::Point(p_prev[0], p_prev[1]), point, cv::Scalar( 0, 0, 0 ), 1, 8 );
                cv::drawMarker(image, point, cv::Scalar( 255, 0, 0 ), cv::MARKER_CROSS, 3, 1, 8 );

                p_prev = p;
            }
        }

        cv::imshow("Path", image);

        cv::waitKey( 5 );
    }

    void Path::plot(cv::Mat& image, cv::Scalar color, bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double xres = image.cols;
        double yres = image.rows;

        cv::line(image, cv::Point(0, yres/2), cv::Point(xres-1, yres/2), cv::Scalar(128,128,128), 1, 8, 0);
        cv::line(image, cv::Point(xres/2, 0), cv::Point(xres/2, yres-1), cv::Scalar(128,128,128), 1, 8, 0);

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        if (s_end_ <= 0) return; // can not plot if length is unknown
        double s_spacing = s_end_ / 99.0;

        Eigen::Vector2d p_prev = get(0);
        Eigen::Vector2d p;

        for (unsigned int s_idx = 0; s_idx < 100; s_idx++) {
            double s = s_spacing * s_idx;
            p = get(s);

            float x = (p[0]-x_min) * scale_x;
            float y = (p[1]-y_min) * scale_y;

            if (x >= 0 && x < xres && y >= 0 && y < yres) {
                cv::Point point;
                if (drawXup) // draw with robot x-axis pointing up in plot
                    point = cv::Point(yres-y,xres-x);
                else
                    point = cv::Point(x,yres-y);

                // Draw a line
                //cv::line(image, cv::Point(p_prev[0], p_prev[1]), point, cv::Scalar( 0, 0, 0 ), 1, 8 );
                cv::drawMarker(image, point, color, cv::MARKER_CROSS, 3, 1, 8 );

                p_prev = p;
            }
        }
    }

    void Path::PlotPoint(double sValue, cv::Mat& image, cv::Scalar color, bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double xres = image.cols;
        double yres = image.rows;
        Eigen::Vector2d p = get(sValue);

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        float x = (p[0]-x_min) * scale_x;
        float y = (p[1]-y_min) * scale_y;

        if (x >= 0 && x < xres && y >= 0 && y < yres) {
            cv::Point point;
            if (drawXup) // draw with robot x-axis pointing up in plot
                point = cv::Point(yres-y,xres-x);
            else
                point = cv::Point(x,yres-y);

            cv::drawMarker(image, point, color, cv::MARKER_STAR, 6, 2, 8);
        }
    }

    void Path::FitTrajectory(Trajectory& trajectory, unsigned int approximationOrder, bool StopAtEnd, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint)
    {
        // approximationOrder is for x(t) and y(t) fitting
        unsigned order_t2s = approximationOrder + 1; // order_t2s is for t(s) fitting
        unsigned order_f2s = approximationOrder + 1; // order_f2s is the order for the final fitted path: x(s) and y(s)

        std::vector<double> tVec = trajectory.GetDistanceList(); // get approximated distance vector for the individual points in the trajectory
        std::vector<double> xValues = trajectory.GetX();
        std::vector<double> yValues = trajectory.GetY();

        if (tVec.size() <= order_t2s) { // we do not have sufficient points for fitting - therefore just do a path that corresponds to holding a static position (end point)
            s_end_ = 99;
            poly_x_ = Polynomial({trajectory.back().point[0], 0.001});
            poly_y_ = Polynomial({trajectory.back().point[1], 0.001});
            return;
        }

        /* Fit window points to two polynomials, x(t) and y(t), using parameters t starting with t0=0 and spaced with the distance between each point (chordal parameterization) */
        Polynomial xt, yt;
        xt.FitPoints(approximationOrder, tVec, xValues, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        yt.FitPoints(approximationOrder, tVec, yValues, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);

        // Create temporary path holding the x(t) and y(t) approximation
        Path tmpPath(xt, yt);

        /* Compute numerical approximation of arc length at the points, t, using the fitted polynomial */
        // Given the now approximated x(t), y(t) path, we approximate the arc curve length for the different points
        // Hence to be able to create a mapping between s to t : t(s)
        std::vector<double> sVec;
        for (double& t : tVec) {
            sVec.push_back(tmpPath.ArcCurveLength(t));
        }

        double sTotal = sVec.back();

        /* Fit t(s) polynomial */
        Polynomial ts;
        ts.FitPoints(order_t2s, sVec, tVec, true, false);

        /* Create vector of evenly spaced distances, s, and get corresponding t values */
        // Make 100 linearly (evenly) seperated distance points
        double spacing = sTotal / 99;
        std::vector<double> sEven;
        std::vector<double> tEven;
        for (int i = 0; i < 100; i++) {
            double s = i * spacing;
            sEven.push_back(s);
            tEven.push_back(ts.evaluate(s));
        }

        /* Use vector of the corresponding t values (matching the evenly spaced s distances) to get corresponding x-y value pairs from the initial approximation */
        std::vector<double> xEven;
        std::vector<double> yEven;
        for (double& t : tEven) {
            xEven.push_back(xt.evaluate(t));
            yEven.push_back(yt.evaluate(t));
        }

        /* Hack to make the fitted trajectory keep the same position after reaching the distance value */
        /*if (StopAtEnd) {
            for (int i = 1; i < 100; i++) {
                double s = sTotal + i * spacing;
                sEven.push_back(s);
                xEven.push_back(xEven.back() + spacing/1000);
                yEven.push_back(yEven.back());
            }
        }*/

        /* Use the s to x-y pairs to create final approximation: x(s) and y(s) */
        // Fit two new polynomials on the new generated points, x_0,...,x_n  and y_0,...,y_n  using the evenly spaced distance parameters, s_0,...,s_n, as the parameter
        poly_x_.FitPoints(order_f2s, sEven, xEven, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        poly_y_.FitPoints(order_f2s, sEven, yEven, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        s_end_ = sEven.back(); // sTotal
    }

    void Path::print()
    {
        std::cout << "x(s) ";
        poly_x_.print();
        std::cout << "y(s) ";
        poly_y_.print();
    }

    double Path::length()
    {
        return s_end_;
    }

    double Path::FindClosestPoint(const Eigen::Vector2d& position)
    {
        // Find point on path being closest to the input position and return the corresponding path parameter (s-value) at this point
        // First we create a centered path around the input position, by taking the two polynomial and subtracting the position value
        Polynomial xs_centered = poly_x_ - position[0];
        Polynomial ys_centered = poly_y_ - position[1];
        // Given this centered polynomial, the distance to any point along the original path is defined by the function:
        //   dist(s) = sqrt( f_x(s)^2 + f_y(s)^2 )
        // Since this is the function we want to minimize we create this function as a distance polynomial such that we can take the derivative and find the minimum point
        Polynomial xs_squared = xs_centered.squared();
        Polynomial ys_squared = ys_centered.squared();
        // And since we want to find the closest distance, we can also just
        // minimize the squared distance: dist(s)^2
        //   dist(s)^2 = f_x(s)^2 + f_y(s)^2
        Polynomial dist_squared = xs_squared + ys_squared;

        // Finally we find the minimum distance, s, through a Newton-based minimization
        return dist_squared.findMinimum(0, 0, s_end_, 0.001, 100);
    }

}


#ifndef MM_GEOMTYPES_HPP
#define MM_GEOMTYPES_HPP

#ifdef USE_BG_GEOMETRY
#include <ogrsf_frmts.h> // C++ API for GDAL
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/extensions/gis/io/wkb/read_wkb.hpp>
#include <iterator>
#include <vector>
#else
#include <ogrsf_frmts.h> // C++ API for GDAL
#endif

namespace MM {

#ifdef USE_BG_GEOMETRY

namespace bg = boost::geometry;
typedef bg::model::point<double, 2, bg::cs::cartesian> bg_point_t;
typedef bg::model::linestring<bg_point_t> linestring_t;
/**
 *  Boost Geometry Linestring, compatible with OGRGeometry
 */
class BGLineString {
public:
inline double getX(int i){
    return bg::get<0>(line.at(i));
};
inline double getY(int i){
    return bg::get<1>(line.at(i));
};
inline void addPoint(double x,double y){
    bg::append(line, bg_point_t(x,y));
};
inline int getNumPoints(){
    return bg::num_points(line);
};
inline bool IsEmpty(){
    return bg::num_points(line)==0;
};
bg::wkt_manipulator<linestring_t> exportToWkt(){
    return bg::wkt(line);
};
linestring_t *get_geometry(){
    return &line;
};
inline double get_Length(){
    return bg::length(line);
};
private:
linestring_t line;
}; // BGLineString

typedef BGLineString LineString;

/**
 *  Convert an OGRLineString to Boost geometry, the caller is responsible to
 *  freeing the memory.
 *
 */
BGLineString *ogr2bg(OGRLineString *line){
    int binary_size = line->WkbSize();
    std::vector<unsigned char> wkb(binary_size);
    // http://www.gdal.org/ogr__core_8h.html#a36cc1f4d807ba8f6fb8951f3adf251e2
    line->exportToWkb(wkbNDR,&wkb[0]);
    BGLineString *l = new BGLineString();
    bg::read_wkb(wkb.begin(),wkb.end(),*(l->get_geometry()));
    return l;
};

#else

typedef OGRLineString LineString;

#endif //USE_BG_GEOMETRY

}; // MM

#endif // MM_GEOMTYPES_HPP