#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <random>
#include <math.h>
#include <armadillo>
#include "src/m_bp_neural_network.cpp"

using namespace std;

struct IdxImageInfo
{
  int count;
  int width;
  int height;
};

int ReverseInt(int i)
{
  unsigned char ch1, ch2, ch3, ch4;
  ch1 = i & 255;
  ch2 = (i >> 8) & 255;
  ch3 = (i >> 16) & 255;
  ch4 = (i >> 24) & 255;
  return ((int)ch1 << 24) + ((int)ch2 << 16) + ((int)ch3 << 8) + ch4;
}

mat build_mat_by_image(vector<double> &image_data)
{
  mat re = mat(1,image_data.size());
  for(int i=0;i<image_data.size();i++)
  {
    re(0,i)=image_data[i];
  }
  return re;
}

void read_Mnist_Label(string filename, vector<double> &labels)
{
  ifstream file(filename, ios::binary);
  if (file.is_open())
  {
    int magic_number = 0;
    int number_of_images = 0;
    file.read((char *)&magic_number, sizeof(magic_number));
    file.read((char *)&number_of_images, sizeof(number_of_images));
    magic_number = ReverseInt(magic_number);
    number_of_images = ReverseInt(number_of_images);
    cout << "magic number = " << magic_number << endl;
    cout << "number of images = " << number_of_images << endl;
    for (int i = 0; i < number_of_images; i++)
    {
      unsigned char label = 0;
      file.read((char *)&label, sizeof(label));
      labels.push_back((double)label);
    }
  }
}

void read_Mnist_Images(string filename, IdxImageInfo &info, vector<vector<double>> &images)
{
  ifstream file(filename, ios::binary);
  if (file.is_open())
  {
    int magic_number = 0;
    int number_of_images = 0;
    int n_rows = 0;
    int n_cols = 0;

    file.read((char *)&magic_number, sizeof(magic_number));
    file.read((char *)&number_of_images, sizeof(number_of_images));
    file.read((char *)&n_rows, sizeof(n_rows));
    file.read((char *)&n_cols, sizeof(n_cols));
    magic_number = ReverseInt(magic_number);
    number_of_images = ReverseInt(number_of_images);
    n_rows = ReverseInt(n_rows);
    n_cols = ReverseInt(n_cols);
    info.height = n_rows;
    info.width = n_cols;
    info.count = number_of_images;
    cout << "magic number = " << magic_number << endl;
    cout << "number of images = " << number_of_images << endl;
    cout << "rows = " << n_rows << endl;
    cout << "cols = " << n_cols << endl;
    for (int i = 0; i < number_of_images; i++)
    {
      vector<double> tp;
      for (int r = 0; r < n_rows; r++)
      {
        for (int c = 0; c < n_cols; c++)
        {
          unsigned char image = 0;
          file.read((char *)&image, sizeof(image));
          double v = (double)((double)((int)image) / 256.0f);
          tp.push_back(v);
        }
      }
      images.push_back(tp);
    }
  }
}

void sin_fitting()
{
  mat inputData(1, 1);
  mat resultData(1, 2);
  m_bp_neural_network bp_network(1, {5, 5, 2}, {"logsig", "logsig", "logsig"});
  bp_network.init();
  vector<mat> trainInput;
  vector<mat> trainOutput;
  vector<mat> testInput;
  vector<mat> testOutput;
  int sample_num = 20;
  int test_num = 20;
  for (int i = 0; i <= sample_num - 1; i++)
  {
    inputData(0, 0) = (((double)(i) / (sample_num - 1)) - 0.5) * 2;
    resultData(0, 0) = (1 + sinf(inputData(0, 0) * M_PI)) / 2;
    resultData(0, 1) = resultData(0, 0);
    trainInput.push_back(inputData);
    trainOutput.push_back(resultData);
  }
  for (int i = 0; i <= test_num - 1; i++)
  {
    inputData(0, 0) = (((double)(i) / (test_num - 1)) - 0.5) * 2;
    resultData(0, 0) = (1 + sinf(inputData(0, 0) * M_PI)) / 2;
    resultData(0, 1) = resultData(0, 0);

    testInput.push_back(inputData);
    testOutput.push_back(resultData);
  }
  bp_network.train("traingd", trainInput, trainOutput, 80000, 0.5);
  for (int i = 0; i < testInput.size(); i++)
  {
    mat re = bp_network.sim(testInput[i]);
    re.print("Sim");
    testOutput[i].print();
  }
  //bp_network.print();
}

void image_fitting()
{
  vector<double>labels;
  read_Mnist_Label("data/train-labels.idx1-ubyte", labels);
  vector<vector<double>> images;
  IdxImageInfo imageInfo;
  read_Mnist_Images("data/train-images.idx3-ubyte",imageInfo, images);

  m_bp_neural_network bp_network(784,{32,32,10}, {"logsig", "logsig", "logsig"});
  mat inputData(1, 784);
  mat resultData(1, 10);
  bp_network.init();
  vector<mat> trainInput;
  vector<mat> trainOutput;
  vector<mat> testInput;
  vector<mat> testOutput;
  vector<int> testLabel;
  int sample_num = 5000;
  int test_num = 1000;
  for (int i = 0; i <= sample_num - 1; i++)
  {
    int index = labels[i];
    inputData = build_mat_by_image(images[i]);
    resultData.fill(0);
    resultData(0,index) = 1;
    trainInput.push_back(inputData);
    trainOutput.push_back(resultData);
  }
  for (int i = 20000; i <= 20000 + test_num - 1; i++)
  {
    int index = labels[i];
    inputData = build_mat_by_image(images[i]);
    resultData.fill(0);
    resultData(0,index) = 1;
    testInput.push_back(inputData);
    testOutput.push_back(resultData);
    testLabel.push_back(index);
  }
  bp_network.train("traingd", trainInput, trainOutput, 10000, 0.5);
  int correct = 0;
  for (int i = 0; i < testInput.size(); i++)
  {
    mat re = bp_network.sim(testInput[i]);
    double max1 = re(0,testLabel[i]);
    double max2 = max(max(re));
    //cout<<max1<<"/"<<max2<<endl;
    //re.print("Sim");
    //testOutput[i].print();
    if(max1==max2)correct++;
  }
  cout<<"Correct:"<<(double)correct/(double)testInput.size()*100<<"%"<<endl;
}

int main()
{
  //sin_fitting();
  image_fitting();
  return 0;
}


/******************************************************************************

  Multidimensional Vector (Point) class.

  Copyright (c) 2010 - 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef VECTOR_HPP_4545D406_43E3_4444_8E4B_9B5A10E7AB16_
#define VECTOR_HPP_4545D406_43E3_4444_8E4B_9B5A10E7AB16_

#include <cstddef>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <limits>
#include <functional>
#include <stdexcept>
#include <boost/array.hpp>
#include <boost/operators.hpp>
#include <boost/format.hpp>
#include <boost/static_assert.hpp>

namespace bo {

// Basic n-vector. Some methods, such as x(), y(), cross_product() are available
// only for certain specializations. A compiler error will be thrown if an
// inappropriate call attempt is made. See notes on accessor functions below.
template <typename T, std::size_t N>
class Vector
    : boost::additive2< Vector<T, N>, T
    , boost::multiplicative2< Vector<T, N>, T
    , boost::equality_comparable1< Vector<T, N>
    , boost::additive1< Vector<T, N>
    > > > >
{
public:
    typedef Vector<T, N> this_type;

public:
    // Each component is initialized either to 0 or to a given value.
    // If a c-array of type S is given, copy its elements.
    Vector();
    Vector(const T& value);
    template <typename S> explicit Vector(const S& data, std::size_t length);

    // These constructors add support for specializations with N = 2, 3, 4. See
    // notes on accessor functions below.
    Vector(const T& x, const T& y);
    Vector(const T& x, const T& y, const T& z);
    Vector(const T& x, const T& y, const T& z, const T& w);

    // Some of standard operators. boost::operators library adds more.
    const this_type& operator+=(const T& scalar);
    const this_type& operator-=(const T& scalar);
    const this_type& operator*=(const T& scalar);
    const this_type& operator/=(const T& scalar);

    // In general won't work for floats. This is because not every real number can
    // be represented by float/double/long double and therefore theoretically equal
    // numbers can differ, i.e. f^{-1}(f(x)) can differ from x. Fore more information
    // on this topic see
    //     http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
    bool operator==(const this_type& other) const;
    // Lexicographical comparison operator.
    bool operator<(const this_type& other) const;

    const this_type& operator+=(const this_type& other);
    const this_type& operator-=(const this_type& other);

    // Unary negation operator. Creates a new Vector<T, N> and copies every element,
    // taken with the minus sign. Uses std::transform and std::negate<> in current
    // implementation. Another possible way to do this is multiplication by -1. In
    // this case type T should provide a c-tor taking -1 as single argument. This
    // approach seems to be less rational as the current one.
    this_type operator-() const;

    // Assignment and access operators. Range-check is done by boost::array via
    // debug-only assertions. Use at() method for safer but less efficient version
    // with exceptions.
    const T& operator[](std::size_t index) const;
    T& operator[](std::size_t index);

    // Assignment and access methods. Throw an exception in case of bad index.
    // Safer, but less efficient alternative of opeartor[].
    const T& at(std::size_t index) const;
    T& at(std::size_t index);

    // These special accessor functions are available only where appropriate.
    // That means, if you try to call x() on Vector<T, 10> or z() on Vector<T, 2>
    // you'll get an error. Unfortunately, not "... is not a member of ..." error,
    // but still clear enough to understand the reason.
    //
    // The other solution is to have a basic, say, VectorImpl<T, N> class and
    // derived classes Vector<T, N> and its specializations Vector<T, 2> and so on.
    // But this approach leads to a lot of copy-paste methods, e.g. all operators,
    // constructors and some other. A possible solution is to write a macros for
    // automatic creation of these duplicated methods. An open question is performance
    // in presence of inheritance and possible redundant object copying. That
    // should be checked in case of this approach.
    //
    // Finally, it was decided to favor the first approach because of its simplicity
    // and at least not worse performance.

    const T& x() const;
    const T& y() const;
    const T& z() const;
    const T& w() const;

    T& x();
    T& y();
    T& z();
    T& w();

    // Cross product makes sense only in 3D and therefore is available only for
    // Vector<T, 3>.
    this_type cross_product(const this_type& other) const;

    // See below for operator* (dot product), defined outside the class.
    // See below for operator<<, defined outside the class.

    // Simple aggregation functions.
    T min_element() const;
    T max_element() const;
    T sum() const;
    T product() const;
    T avg() const;

    // The same as min_element() and max_element(), but available only if min/max
    // macros are not defined (crappy "windows.h" defines these macors by default).
#ifndef min
    T min() const;
#endif // min

#ifndef max
    T max() const;
#endif // max

    // These functions use std::min_element() and therefore return an index of the
    // first found element (in case there are several equal min/max elements).
    std::size_t min_index() const;
    std::size_t max_index() const;

    // Computes euclidean (L2) norm of the vector. Double version converts self product
    // to double and calls std::sqrt(T) afterwards. Other overloads uses the double
    // version. Templated version converts norm in double to type T. If the return
    // variable is given, norm is converted to the retvar's type.
    double euclidean_norm_d() const;
    template <typename RetType> void euclidean_norm(RetType& retvar) const;
    T euclidean_norm() const;

    // Computes taxicab (L1) and maximum (L inf) norms of the vector. Uses std::abs(T),
    // which implies T is a built-in type or custom type, for which abs() is provided
    // in std namespace.
    T taxicab_norm() const;
    T maximum_norm() const;

    // Note that for integral types normalize won't work. For this reason this
    // function is designed const and it returns a normalized double vector.
    // If the vector is a null-vector, normalization is meaningless. However, some
    // software (e.g. Wolfram Mathematica 7.0) returns the null-vector as a result
    // of a normalization of the null-vector. In order not to inflate the code,
    // the function called on the null-vector will make 0/0 division and return
    // a vector made of NaNs.
    Vector<double, N> normalized() const;

    // Size is always the same: N.
    static std::size_t size();

    // Swap method (linear complexity).
    void swap(this_type& other);

    // Fill each component with a given value.
    void fill(const T& value);

    // Set components' values from anything, that can be accessed by [].
    template <typename S> void assign(const S& data, std::size_t length);

protected:
    // Provide range check for a given index. Throws a std::out_of_range exception
    // in case of bad index.
    static void check_range(std::size_t index);

protected:
    boost::array<T, N> components_;
};

// Dot product operator for two Vector<T, N>.
template <typename T, std::size_t N> inline
T operator*(Vector<T, N> lhs, const Vector<T, N>& rhs)
{
    T retvalue(0);
    for (std::size_t i = 0; i < N; ++i)
        retvalue += lhs[i] * rhs[i];

    return retvalue;
}

// Stream operator<< for printing Vector<T, N> contents.
template <typename T, std::size_t N>
std::ostream& operator<<(std::ostream& os, const Vector<T, N>& obj)
{
    os << boost::format("%1%-Vector, object %2$#x, %3% bytes: ")
        % N % &obj % sizeof(obj) << std::endl << "    (";

    for (std::size_t i = 0; i < N-1; ++i)
        os << boost::format("%1%, %|4t|") % obj[i];

    // Print last element separately in order to avoid last comma and spaces.
    os << boost::format("%1%)") % obj[N-1] << std::endl
       << boost::format("end of object %1$#x.") % &obj << std::endl;

    return os;
}

template <typename T, std::size_t N>
Vector<T, N>::Vector()
{
    fill(static_cast<T>(0));
}

template <typename T, std::size_t N>
Vector<T, N>::Vector(const T& value)
{
    fill(value);
}

template <typename T, std::size_t N> template <typename S>
Vector<T, N>::Vector(const S& data, std::size_t length)
{
    assign(data, length);
}

template <typename T, std::size_t N>
Vector<T, N>::Vector(const T& x, const T& y)
{
    BOOST_STATIC_ASSERT(N == 2);

    components_[0] = x;
    components_[1] = y;
}

template <typename T, std::size_t N>
Vector<T, N>::Vector(const T& x, const T& y, const T& z)
{
    BOOST_STATIC_ASSERT(N == 3);

    components_[0] = x;
    components_[1] = y;
    components_[2] = z;
}

template <typename T, std::size_t N>
Vector<T, N>::Vector(const T& x, const T& y, const T& z, const T& w)
{
    BOOST_STATIC_ASSERT(N == 4);

    components_[0] = x;
    components_[1] = y;
    components_[2] = z;
    components_[3] = w;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator+=(const T& scalar)
{
    std::transform(components_.begin(), components_.end(), components_.begin(),
                   std::bind2nd(std::plus<T>(), scalar));
    return *this;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator-=(const T& scalar)
{
    std::transform(components_.begin(), components_.end(), components_.begin(),
                   std::bind2nd(std::minus<T>(), scalar));
    return *this;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator*=(const T& scalar)
{
    std::transform(components_.begin(), components_.end(), components_.begin(),
                   std::bind2nd(std::multiplies<T>(), scalar));
    return *this;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator/=(const T& scalar)
{
    std::transform(components_.begin(), components_.end(), components_.begin(),
                   std::bind2nd(std::divides<T>(), scalar));
    return *this;
}

template <typename T, std::size_t N>
bool Vector<T, N>::operator==(const Vector<T, N>& other) const
{
    for (std::size_t i = 0; i < N; ++i)
        if (components_[i] != other.components_[i])
            return false;

    return true;
}

template <typename T, std::size_t N>
bool Vector<T, N>::operator<(const Vector<T, N>& other) const
{
    for (std::size_t i = 0; i < N; ++i)
    {
        if (components_[i] < other.components_[i])
            return true;
        if (components_[i] > other.components_[i])
            return false;
    }

    return false;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator+=(const Vector<T, N>& other)
{
    std::transform(components_.begin(), components_.end(), other.components_.begin(),
                   components_.begin(), std::plus<T>());
    return *this;
}

template <typename T, std::size_t N>
const Vector<T, N>& Vector<T, N>::operator-=(const Vector<T, N>& other)
{
    std::transform(components_.begin(), components_.end(), other.components_.begin(),
                   components_.begin(), std::minus<T>());
    return *this;
}

template <typename T, std::size_t N>
Vector<T, N> Vector<T, N>::operator-() const
{
    Vector<T, N> retvalue;
    std::transform(components_.begin(), components_.end(), retvalue.components_.begin(),
                   std::negate<T>());
    return retvalue;
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::operator[](std::size_t index) const
{
    return components_[index];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::operator[](std::size_t index)
{
    return components_[index];
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::at(std::size_t index) const
{
    check_range(index);
    return components_[index];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::at(std::size_t index)
{
    check_range(index);
    return components_[index];
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::x() const
{
    BOOST_STATIC_ASSERT(N >= 1 && N <= 4);
    return components_[0];
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::y() const
{
    BOOST_STATIC_ASSERT(N >= 2 && N <= 4);
    return components_[1];
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::z() const
{
    BOOST_STATIC_ASSERT(N >= 3 && N <= 4);
    return components_[2];
}

template <typename T, std::size_t N> inline
const T& Vector<T, N>::w() const
{
    BOOST_STATIC_ASSERT(N == 4);
    return components_[3];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::x()
{
    BOOST_STATIC_ASSERT(N >= 1 && N <= 4);
    return components_[0];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::y()
{
    BOOST_STATIC_ASSERT(N >= 2 && N <= 4);
    return components_[1];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::z()
{
    BOOST_STATIC_ASSERT(N >= 3 && N <= 4);
    return components_[2];
}

template <typename T, std::size_t N> inline
T& Vector<T, N>::w()
{
    BOOST_STATIC_ASSERT(N == 4);
    return components_[3];
}

template <typename T, std::size_t N>
Vector<T, N> Vector<T, N>::cross_product(const Vector<T, N>& other) const
{
    BOOST_STATIC_ASSERT(N == 3);

    return Vector<T, N>
        (y() * other.z() - z() * other.y(),
         z() * other.x() - x() * other.z(),
         x() * other.y() - y() * other.x());
}

template <typename T, std::size_t N>
T Vector<T, N>::min_element() const
{
    return
        (*std::min_element(components_.begin(), components_.end()));
}

// Available only if not interferes with "windows.h" macros (or other named "min").
#ifndef min
template <typename T, std::size_t N> inline
T Vector<T, N>::min() const
{
    return min_element();
}
#endif // min

template <typename T, std::size_t N>
std::size_t Vector<T, N>::min_index() const
{
    return
        std::distance(components_.begin(),
            std::min_element(components_.begin(), components_.end()));
}

template <typename T, std::size_t N>
T Vector<T, N>::max_element() const
{
    return
        (*std::max_element(components_.begin(), components_.end()));
}

// Available only if not interferes with "windows.h" macros (or other named "max").
#ifndef max
template <typename T, std::size_t N> inline
T Vector<T, N>::max() const
{
    return max_element();
}
#endif // max

template <typename T, std::size_t N>
std::size_t Vector<T, N>::max_index() const
{
    return
        std::distance(components_.begin(),
            std::max_element(components_.begin(), components_.end()));
}

template <typename T, std::size_t N>
T Vector<T, N>::sum() const
{
    // A small optimization here: start with the second elem and pass first elem
    // as an initial value.
    return
        std::accumulate(components_.begin() + 1, components_.end(), components_[0]);
}

template <typename T, std::size_t N>
T Vector<T, N>::product() const
{
    // A small optimization here: start with the second elem and pass first elem
    // as an initial value.
    return
        std::accumulate(components_.begin() + 1, components_.end(), components_[0],
            std::multiplies<T>());
}

template <typename T, std::size_t N>
T Vector<T, N>::avg() const
{
    return
        (sum() / N);
}

template <typename T, std::size_t N>
double Vector<T, N>::euclidean_norm_d() const
{
    return
        std::sqrt(static_cast<double>((*this) * (*this)));
}

template <typename T, std::size_t N> template <typename RetType>
void Vector<T, N>::euclidean_norm(RetType& retvar) const
{
    retvar = static_cast<RetType>(euclidean_norm_d());
}

template <typename T, std::size_t N>
T Vector<T, N>::euclidean_norm() const
{
    return T(euclidean_norm_d());
}

template <typename T, std::size_t N>
T Vector<T, N>::taxicab_norm() const
{
    T retvalue(0);

    for (std::size_t i = 0; i < N; ++i)
        retvalue += std::abs(components_[i]);

    return retvalue;
}

template <typename T, std::size_t N>
T Vector<T, N>::maximum_norm() const
{
    Vector<T, N> nonnegative;
    std::transform(components_.begin(), components_.end(),
                   nonnegative.components_.begin(), (T (*)(T))std::abs);

    return
        nonnegative.max_element();
}

template <typename T, std::size_t N>
Vector<double, N> Vector<T, N>::normalized() const
{
    // Normalization of the null vector is meaningess. In this case vector's norm
    // is zero and factor should be Infinity (according to IEC559/IEEE754). A simple
    // check for this is using numeric_limits<> class.
    double factor = 1.0 / euclidean_norm_d();
    if (std::numeric_limits<double>::infinity() == factor)
        throw std::logic_error("Normalization of the null vector is meaningless.");

    Vector<double, N> retvalue;

    for (std::size_t i = 0; i < N; ++i)
        retvalue[i] = static_cast<double>(components_[i]) * factor;

    return retvalue;
}

template <typename T, std::size_t N> inline
std::size_t Vector<T, N>::size()
{
    return N;
}

template <typename T, std::size_t N>
void Vector<T, N>::swap(Vector<T, N>& other)
{
    components_.swap(other.components_);
}

template <typename T, std::size_t N>
void Vector<T, N>::fill(const T& value)
{
    components_.fill(value);
}

template <typename T, std::size_t N> template <typename S>
void Vector<T, N>::assign(const S& data, std::size_t length)
{
    // If a given array is smaller than N, copy everything and set 0 for other
    // components. If a given array is bigger than N, copy first N elements.
    std::size_t num = (length < N) ? length : N;
    for (std::size_t i = 0; i < num; ++i)
        components_[i] = static_cast<T>(data[i]);

    for (std::size_t i = num; i < N; ++i)
        components_[i] = static_cast<T>(0);
}

template <typename T, std::size_t N>
void Vector<T, N>::check_range(std::size_t index)
{
    if (index >= size())
        throw std::out_of_range((boost::format(
            "%1%-Vector's index \"%2%\" is out of range.") % N % index).str());
}

} // namespace bo

#endif // VECTOR_HPP_4545D406_43E3_4444_8E4B_9B5A10E7AB16_


#include <operational_space_controllers/force_controller.hpp>
#include <pluginlib/class_list_macros.h>
#include <string>
#include <operational_space_controllers_msgs/Move.h>
#include <boost/math/constants/constants.hpp>

using namespace std;
using namespace operational_space_controllers;

/// Register controller to pluginlib
PLUGINLIB_DECLARE_CLASS(operational_space_controllers,ForceControllerPlugin,operational_space_controllers::ForceController,pr2_controller_interface::Controller)

static const double PI = boost::math::constants::pi<double>();
static const double BETA = 20.0 / PI;
static const double GAMMA = 1e4;

void ForceController::commandCB(const operational_space_controllers_msgs::MoveConstPtr &command)
{
    ROS_DEBUG("Force controller with kinematic chain ending at %s received a new move command", tip_name.c_str());

    if (command->header.frame_id != "/" + root_name)
    {
        ROS_ERROR("Move commands must be in the %s frame. Command was in the /%s frame.",
                  root_name.c_str(), command->header.frame_id.c_str());
        return;
    }

    if (command->stop) {
        move_command.set(operational_space_controllers_msgs::MoveConstPtr());
    }
    else {
        move_command.set(command);
    }
}

bool ForceController::init(pr2_mechanism_model::RobotState *robot,
                           ros::NodeHandle &n)
{

    if (!n.getParam("root_name", root_name))
    {
        ROS_ERROR("No root name given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    if (!n.getParam("tip_name", tip_name))
    {
        ROS_ERROR("No tip name given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    double kp;
    if (!n.getParam("proportional_gain", kp))
    {
        ROS_ERROR("No proportional gain given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    else {
        ROS_INFO("Proportional gain set to %f", kp);
    }

    double kd;
    if (!n.getParam("derivative_gain", kd))
    {
        ROS_ERROR("No derivative gain given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    else {
        ROS_INFO("Derivative gain set to %f", kd);
    }

    double kpRot;
    if (!n.getParam("proportional_gain_rotational", kpRot))
    {
        ROS_ERROR("No rotational proportional gain given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    else {
        ROS_INFO("Rotational proportional gain set to %f", kpRot);
    }

    double kdRot;
    if (!n.getParam("derivative_gain_rotational", kdRot))
    {
        ROS_ERROR("No rotational derivative gain given in namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    else {
        ROS_INFO("Rotational derivative gain set to %f", kdRot);
    }

    // Construct a chain from the root to the tip and prepare the kinematics
    // Note the joints must be calibrated
    if (!chain.init(robot, root_name, tip_name))
    {
        ROS_ERROR("ForceController could not use the chain from '%s' to '%s'", root_name.c_str(), tip_name.c_str());
        return false;
    }

    if (!chain.allCalibrated())
    {
        ROS_ERROR("Kinematics chain is not calibrated");
        return false;
    }

    // Store the robot handle for later use (to get time)
    robot_state = robot;

    // Construct the kdl solvers in non-realtime
    chain.toKDL(kdl_chain);
    jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain));
    jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(kdl_chain));

    // Resize (pre-allocate) the variables in non-realtime
    q.resize(kdl_chain.getNrOfJoints());
    qdot.resize(kdl_chain.getNrOfJoints());
    tau.resize(kdl_chain.getNrOfJoints());
    tau_act.resize(kdl_chain.getNrOfJoints());
    J.resize(kdl_chain.getNrOfJoints());
    qdotdot.resize(kdl_chain.getNrOfJoints());

    Kp.vel(0) = kp;
    Kp.vel(1) = kp;
    Kp.vel(2) = kp;
    Kp.rot(0) = kpRot;
    Kp.rot(1) = kpRot;
    Kp.rot(2) = kpRot;

    Kd.vel(0) = kd;
    Kd.vel(1) = kd;
    Kd.vel(2) = kd;
    Kd.rot(0) = kdRot;
    Kd.rot(1) = kdRot;
    Kd.rot(2) = kdRot;

    subscriber = n.subscribe("command", 1, &ForceController::commandCB, this);
    updates = 0;

    controller_state_publisher.reset(new realtime_tools::RealtimePublisher<operational_space_controllers_msgs::ForceControllerFeedback>(n, "state", 1));
    controller_state_publisher->msg_.requested_joint_efforts.resize(kdl_chain.getNrOfJoints());
    controller_state_publisher->msg_.actual_joint_efforts.resize(kdl_chain.getNrOfJoints());
    controller_state_publisher->msg_.x_error.resize(6);
    controller_state_publisher->msg_.force_desired.resize(6);
    controller_state_publisher->msg_.pose.header.frame_id = root_name;
    return true;
}

void ForceController::starting()
{
    ROS_INFO("Starting the ForceController");
}

static KDL::Vector dot(const KDL::Rotation& R, const KDL::Vector& V)
{
    KDL::Vector r = KDL::Vector::Zero();
    for (unsigned int i = 0; i < 3; ++i)
    {
        for (unsigned int j = 0; j < 3; ++j)
        {
            r(i) += R(i, j) * V(j);
        }
    }
    return r;
}

void ForceController::update()
{
    // Check if there is a current goal
    boost::shared_ptr<const operational_space_controllers_msgs::Move> move_command_ptr;
    move_command.get(move_command_ptr);
    updates++;

    // Get the current joint positions and velocities
    chain.getPositions(q);
    chain.getVelocities(qdot);

    // Compute the forward kinematics and Jacobian (at this location)
    jnt_to_pose_solver->JntToCart(q, x);

    if (move_command_ptr.get() != NULL)
    {
        // Determine if this is a torque only command
        if (move_command_ptr->has_torques)
        {
            for (unsigned int i = 0; i < move_command_ptr->torques.size(); ++i) {
                tau(i) = move_command_ptr->torques[i];
            }
        }
        else
        {
            // Calculate velocity in operational space from joint space.
            jnt_to_jac_solver->JntToJac(q, J);
            for (unsigned int i = 0; i < 6; i++)
            {
                xdot(i) = 0;
                for (unsigned int j = 0 ; j < kdl_chain.getNrOfJoints(); j++)
                {
                    xdot(i) += J(i, j) * qdot.qdot(j);
                }
            }

            // Convert the desired pose from ROS to KDL
            xd.p(0) = move_command_ptr->target.position.x;
            xd.p(1) = move_command_ptr->target.position.y;
            xd.p(2) = move_command_ptr->target.position.z;

            if (move_command_ptr->point_at_target)
            {
                double w = KDL::dot(x.p, xd.p);
                KDL::Vector d = x.p * xd.p;
                double q = sqrt(1.0 - w * w);
                xd.M = KDL::Rotation::Quaternion(
                           d.x() * q,
                           d.y() * q,
                           d.z() * q,
                           w);
            }
            else
            {
                xd.M = KDL::Rotation::Quaternion(move_command_ptr->target.orientation.x,
                                                 move_command_ptr->target.orientation.y,
                                                 move_command_ptr->target.orientation.z,
                                                 move_command_ptr->target.orientation.w);
            }

            // Calculate a Cartesian restoring force.
            xerr.vel = x.p - xd.p;
            xerr.rot = 0.5 * (xd.M.UnitX() * x.M.UnitX() + xd.M.UnitY() * x.M.UnitY() + xd.M.UnitZ() * x.M.UnitZ());

            // F is a vector of forces/wrenches corresponding to x, y, z, tx,ty,tz,tw
            // Reduce velocity as the position approaches the desired position.
            for(unsigned int i = 0; i < 6; ++i)
            {
                F(i) = -Kp(i) * xerr(i) - Kd(i) * xdot(i);
            }

            // Factor in obstacle avoidance
            if (move_command_ptr->has_obstacle)
            {
                obstacle.p(0) = move_command_ptr->obstacle.position.x;
                obstacle.p(1) = move_command_ptr->obstacle.position.y;
                obstacle.p(2) = move_command_ptr->obstacle.position.z;

                // Based on (Hoffmann, 2009)
                KDL::Rotation R = KDL::Rotation::Rot((obstacle.p - x.p) * xdot.vel, PI / 2.0);

                double phi = acos(KDL::dot(obstacle.p - x.p, xdot.vel) / ((obstacle.p - x.p).Norm() * xdot.vel.Norm()));

                KDL::Vector P = GAMMA * dot(R, xdot.vel) * phi * exp(-BETA * phi);

                for (unsigned int i = 0; i < 3; ++i)
                {
                    F(i) += P(i);
                }
            }

            // Convert the force into a set of joint torques
            // tau_ is a vector of joint torques q1...qn
            for (unsigned int i = 0 ; i < kdl_chain.getNrOfJoints(); i++)
            {
                // Iterate through the vector. Every joint torque is contributed to
                // by the Jacobian Transpose (note the index switching in J access) times
                // the desired force (from impedance OR explicit force)
                tau(i) = 0;
                for (unsigned int j = 0 ; j < 6 ; j++)
                {
                    tau(i) += J(j, i) * F(j);
                }
            }
        }

        // Send the torques to the robot
        chain.setEfforts(tau);
    }

    // Send controller feedback
    if (updates % 10 == 0 && controller_state_publisher && controller_state_publisher->trylock())
    {
        chain.getEfforts(tau_act);
        double eff_err = 0;
        for (unsigned int i = 0; i < kdl_chain.getNrOfJoints(); i++)
        {
            eff_err += (tau(i) - tau_act(i)) * (tau(i) - tau_act(i));
            controller_state_publisher->msg_.requested_joint_efforts[i] = tau(i);
            controller_state_publisher->msg_.actual_joint_efforts[i] = tau_act(i);
        }

        double pose_sq_err = 0;
        for (unsigned int i = 0; i < 6; ++i)
        {
            pose_sq_err += xerr[i] * xerr[i];
        }

        double force_desired_sq = 0;
        for (unsigned int i = 0; i < 6; ++i)
        {
            force_desired_sq += F[i] * F[i];
        }

        controller_state_publisher->msg_.header.stamp = robot_state->getTime();
        controller_state_publisher->msg_.effort_sq_error = eff_err;
        controller_state_publisher->msg_.pose_sq_error = pose_sq_err;
        controller_state_publisher->msg_.force_desired_sq = force_desired_sq;

        if(move_command_ptr != NULL)
        {
            controller_state_publisher->msg_.goal.header = move_command_ptr->header;
            controller_state_publisher->msg_.goal.pose = move_command_ptr->target;
        }

        controller_state_publisher->msg_.header.stamp = robot_state->getTime();
        controller_state_publisher->msg_.pose.pose.position.x = x.p(0);
        controller_state_publisher->msg_.pose.pose.position.y = x.p(1);
        controller_state_publisher->msg_.pose.pose.position.z = x.p(2);

        double qx, qy, qz, qw;
        x.M.GetQuaternion(qx, qy, qz, qw);
        controller_state_publisher->msg_.pose.pose.orientation.x = qx;
        controller_state_publisher->msg_.pose.pose.orientation.y = qy;
        controller_state_publisher->msg_.pose.pose.orientation.z = qz;
        controller_state_publisher->msg_.pose.pose.orientation.w = qw;
        for (int i = 0; i < 6; ++i)
        {
            controller_state_publisher->msg_.x_error[i] = xerr(i);
        }
        for (int i = 0; i < 6; ++i)
        {
            controller_state_publisher->msg_.force_desired[i] = F(i);
        }
        controller_state_publisher->unlockAndPublish();
    }
}

void ForceController::stopping()
{
    ROS_INFO("Stopping the ForceController");
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE bls_api_test

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <filcrypto.h>

BOOST_AUTO_TEST_SUITE(bls_api_test_suite)

BOOST_AUTO_TEST_CASE(key_verification) {
      let private_key = (*fil_private_key_generate()).private_key.inner;
      let public_key =
      (*fil_private_key_public_key(&private_key[0])).public_key.inner;
      let message = b "hello world";
      let digest = (*fil_hash(&message[0], message.size())).digest.inner;
      let signature =
      (*fil_private_key_sign(&private_key[0], &message[0], message.size()))
      .signature.inner;
      let verified = fil_verify(&signature[0], &digest[0], digest.size(),
      &public_key[0], public_key.size(), );

      BOOST_CHECK_EQUAL(1, verified);

      let flattened_messages = message;
      let message_sizes = [message.size()];
      let verified = fil_hash_verify(
      signature.as_ptr(), flattened_messages.as_ptr(),
      flattened_messages.size(), message_sizes.as_ptr(), message_sizes.size(),
      public_key.as_ptr(), public_key.size(), );

      BOOST_CHECK_EQUAL(1, verified);

      let different_message = b "bye world";
      let different_digest =
      (*fil_hash(&different_message[0], different_message.size()))
      .digest.inner;
      let not_verified = fil_verify(&signature[0], &different_digest[0],
      different_digest.size(), &public_key[0],
      public_key.size(), );

      BOOST_CHECK_EQUAL(0, not_verified);

      // garbage verification
      let different_digest = vec ![ 0, 1, 2, 3, 4 ];
      let not_verified = fil_verify(&signature[0], &different_digest[0],
      different_digest.size(), &public_key[0],
      public_key.size(), );

      BOOST_CHECK_EQUAL(0, not_verified);
}

BOOST_AUTO_TEST_CASE(private_key_with_seed) {
      let seed = fil_32ByteArray {
        inner:
        [5u8; 32]
      };
      let private_key =
      (*fil_private_key_generate_with_seed(seed)).private_key.inner;
      BOOST_CHECK_EQUAL(
      [
      115, 245, 77,  209, 4,   57,  40,  107, 10,  153, 141,
      16,  153, 172, 85,  197, 125, 163, 35,  217, 108, 241,
      64,  235, 231, 220, 131, 1,   77,  253, 176, 19
      ],
      private_key);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef _SITE_HPP
#define _SITE_HPP

#include <Eigen/Core>
#include <unordered_map>
#include <vector>
#include <iostream>
#include <boost/mem_fn.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include "myindex.hpp"

class Lattice;
class Site;
class PairIndex;
class TripletIndex;

Eigen::Vector3d validCoordinate(const Eigen::Vector3d& lhs, const Eigen::Vector3d& rhs);

Eigen::Vector3d validCoordinate(Eigen::Vector3d lhs);

std::vector<std::vector<double>> getAllTriplets(const std::vector<Eigen::Vector3d>& points, const Eigen::Matrix3d& lattice, double maxd, const std::vector<double>& distances);

std::vector<TripletIndex> getAllTripletIndex(
	const std::vector<std::shared_ptr<Site>>& sites,
	const Eigen::Matrix3d& lattice,
	double maxd,
	const std::vector<PairIndex>& vec_pair_index
);

struct RelativeSite{
	double distance;
	std::shared_ptr<Site> site;
	double x,y,z;

	RelativeSite(std::shared_ptr<Site> _s, double _d, const Eigen::Vector3d& _r)
		: site(_s), distance(_d) {
			x = _r(0);
			y = _r(1);
			z = _r(2);
		}

	bool operator==(const RelativeSite &r) const {
		if( std::fabs(this->x-r.x) < 0.00001
				&& std::fabs(this->y-r.y) < 0.00001
				&& std::fabs(this->z-r.z) < 0.00001
			){
			return true;
		} else {
			return false;
		}
	}

	friend size_t hash_value(const RelativeSite &r){
		const int C = 997;	// prime_number
		double result = 0;
		result = result*C + r.x;
		result = result*C + r.y;
		result = result*C + r.z;
		return result;
	}
};

using namespace std;
using namespace boost::multi_index;

struct tag_distance {};
struct tag_relative_coord {};

typedef composite_key <
    RelativeSite,
		BOOST_MULTI_INDEX_MEMBER(RelativeSite, double, x),
		BOOST_MULTI_INDEX_MEMBER(RelativeSite, double, y),
		BOOST_MULTI_INDEX_MEMBER(RelativeSite, double, z)
> composite_key_site;

typedef composite_key_hash <
	std::hash<double>,
	std::hash<double>,
	std::hash<double>
> composite_key_hash_coord;

typedef composite_key_equal_to <
	std::equal_to<double>,
	std::equal_to<double>,
	std::equal_to<double>
> composite_key_equal_coord;

typedef multi_index_container<
    RelativeSite,
    indexed_by<
        hashed_non_unique<tag<tag_distance>, member<RelativeSite, double, &RelativeSite::distance> >,
				hashed_unique<composite_key_site, composite_key_hash_coord, composite_key_equal_coord>
    >
> LinkedSite;

class Site {
private :

	class hash_vec3d {
	public:
	  double operator()(const Eigen::Vector3d &x) const {
	    const int C = 997;      // 素数
	    double t = 0;
	    for (int i = 0; i != x.size(); ++i) {
	        t = t * C + x[i];
	    }
	    return t;
	  }
	};

	const int site_num;
	const Eigen::Vector3d coordinate;
	std::shared_ptr<Lattice> lattice;

	LinkedSite linked_site;

	const static std::shared_ptr<Site> not_found;

public :
	Site(int _n, const Eigen::Vector3d& _c):site_num(_n), coordinate(_c) {};
	Site(int _n, const Eigen::Vector3d& _c, std::shared_ptr<Lattice> _l):site_num(_n), coordinate(_c), lattice(_l){};

	bool operator <(const Site& rhs) {
		return this->site_num < rhs.site_num;
	}

	void setLinkedSite(const std::vector<std::shared_ptr<Site>>& all_sites, double pair_truncation);
	void setLinkedSite(const std::shared_ptr<Site> all_sites, double distance, const Eigen::Vector3d& relative_coord);

	std::pair<LinkedSite::iterator, LinkedSite::iterator> getLinkedSiteIterator(double distance){
		std::pair<LinkedSite::iterator, LinkedSite::iterator> pair_itr  = linked_site.equal_range(distance);
		return pair_itr;
	}

	std::shared_ptr<Site> getLinkedSite(const Eigen::Vector3d& coord){
		LinkedSite::nth_index<1>::type& a = linked_site.get<1>();
		LinkedSite::nth_index<1>::type::iterator itr = a.find(boost::make_tuple(coord(0), coord(1), coord(2)));
		if( itr != a.end() ) return itr->site;
		else {
			for( auto it=a.begin(); it!=a.end(); ++it){
				if( (validCoordinate(it->site->getCoordinate(), this->getCoordinate())-coord).norm() < 0.0001 ) return it->site;
			}
		}
		std::cerr << "ERROR : site not found" << std::endl;
		exit(1);

		return not_found;
	}

	// std::shared_ptr<LinkedSite> getLinkedSite(){return linked_site;}

	const int getSiteNum() const { return this->site_num; };
	const int getLatticeNum();
	const Eigen::Vector3d& getCoordinate() const { return this->coordinate; };

	bool isCoordinate( const Eigen::Vector3d& obj ){
		for( int i=0; i<3; ++i ){
			if( std::abs( this->coordinate[i]-obj[i] ) > 0.00001 ) {
				return false;
			}
		}
		return true;
	}
};

#endif


/*
 * Copyright (C) 2018-2021 LEIDOS.
 *
 * 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.
 */

#include "accel_limiter.hpp"
#include <limits>
#include <boost/math/special_functions/sign.hpp>

namespace twist_filter {

autoware_msgs::ControlCommandStamped
LongitudinalAccelLimiter::longitudinalAccelLimitCtrl(const autoware_msgs::ControlCommandStamped& msg) {
    if (!_initialized) {
        // Record current command and then return unmodified
        _prev_v = msg.cmd.linear_velocity;
        _prev_t = msg.header.stamp;
        _initialized = true;

        return msg;
    }

    autoware_msgs::ControlCommandStamped out{msg};
    // Else we've initialized previously, let's compare.
    double delta_v = msg.cmd.linear_velocity - _prev_v;
    double delta_t = (msg.header.stamp - _prev_t).toSec();
    double accel = delta_v / delta_t;

    if (std::abs(accel) > _accel_limit) {
        out.cmd.linear_acceleration = _accel_limit * boost::math::sign<double>(accel);
        out.cmd.linear_velocity = _prev_v + (_accel_limit * delta_t * boost::math::sign<double>(accel));
    }

    // Update filter for next timestep
    _prev_v = out.cmd.linear_velocity;
    _prev_t = out.header.stamp;

    return out;
}

geometry_msgs::TwistStamped
LongitudinalAccelLimiter::longitudinalAccelLimitTwist(const geometry_msgs::TwistStamped& msg) {
    if (!_initialized) {
        // Record current command and then return unmodified
        _prev_v = msg.twist.linear.x;
        _prev_t = msg.header.stamp;
        _initialized = true;

        return msg;
    }

    geometry_msgs::TwistStamped out{msg};
    // Else we've initialized previously, let's compare.
    double delta_v = msg.twist.linear.x - _prev_v;
    double delta_t = (msg.header.stamp - _prev_t).toSec();
    double accel = delta_v / delta_t;

    if (std::abs(accel) > _accel_limit) {
        out.twist.linear.x = _prev_v + (_accel_limit * delta_t * boost::math::sign<double>(accel));
    }

    // Update filter for next timestep
    _prev_v = out.twist.linear.x;
    _prev_t = out.header.stamp;

    return out;
}

}


#include <iostream>
#include <vector>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <pybind11/stl_bind.h>
#include <boost/polygon/polygon.hpp>

namespace py = pybind11;
namespace gtl = boost::polygon;
using namespace boost::polygon::operators;

typedef gtl::polygon_data<int> Polygon;
typedef gtl::polygon_traits<Polygon>::point_type Point;
typedef std::vector<gtl::polygon_data<int> > PolygonSet;

struct PolyGroup {
    void addPolyData(const std::vector<int> &data) {
        size_t npts = (data.size())/2;
        Point *pts = new Point[npts];
        for(unsigned int i = 0; i< npts; i++) {
            pts[i]=gtl::construct<Point>(data.at(2*i),data.at(2*i+1));
        }
        Polygon poly;
        gtl::set_points(poly,pts,pts+npts);

        ps_.push_back(poly);
        delete [] pts;
    }
    unsigned int getPolyCount(void) {return static_cast<unsigned int>(ps_.size());}
    const std::vector<int> getPoly(unsigned int n) const {
        std::vector<int> pseq;
        if(n<ps_.size()) {
            Polygon poly = ps_[n];
            for(auto v = poly.begin(); v!=poly.end(); v++) {
                pseq.push_back(v->x());
                pseq.push_back(v->y());
            }
        }
        return pseq;
    }
    double area() {
        return gtl::area(ps_);
    }
    void clear() {
        gtl::clear(ps_);
    }
    bool empty() {
        return gtl::empty(ps_);
    }
    void difference(PolyGroup &pg2) {
        ps_-=pg2.ps_;
    }
    void intersection(PolyGroup &pg2) {
        ps_&=pg2.ps_;
    }
    void merge(PolyGroup &pg2) {
        ps_+=pg2.ps_;
    }
    void assign() {
        gtl::assign(ps_,ps_);
    }
    void exor(PolyGroup &pg2) {
        ps_^=pg2.ps_;
    }
    void trapezoids() {
        PolygonSet psin = ps_;
        clear();
        gtl::get_trapezoids(ps_,psin);
    }
    void resize(double value, bool corner_fill_arc, unsigned int num_circle_segments) {
        ps_=gtl::resize(ps_,value,corner_fill_arc,num_circle_segments);
    }

    PolygonSet ps_;
};

PYBIND11_MODULE(boopy, m) {
    py::class_<PolyGroup>(m, "PolyGroup")
        .def(py::init<>())
        .def("addPolyData",&PolyGroup::addPolyData)
        .def("getPolyCount", &PolyGroup::getPolyCount)
        .def("getPoly", &PolyGroup::getPoly)
        .def("area", &PolyGroup::area)
        .def("clear", &PolyGroup::clear)
        .def("empty", &PolyGroup::empty)
        .def("assign", &PolyGroup::assign)
        .def("difference", &PolyGroup::difference)
        .def("intersection", &PolyGroup::intersection)
        .def("merge", &PolyGroup::merge)
        .def("exor", &PolyGroup::exor)
        .def("trapezoids", &PolyGroup::trapezoids)
        .def("resize", &PolyGroup::resize);
}


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

#include <boost/simd/ieee/functions/exponent.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/exponentbits.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>

#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/attributes.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#endif

#if defined(__INTEL_COMPILER) && defined(BOOST_SIMD_HAS_AVX_SUPPORT) && !defined(BOOST_SIMD_HAS_AVX2_SUPPORT)
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( exponent_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return Zero<result_type>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( exponent_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {

    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type             s_type;
      const int nmb= int(Nbmantissabits<s_type>());
      result_type x = shri(exponentbits(a0), nmb);

      // workaround for ICC 14.0.3, tested on AVX
      #if defined(__INTEL_COMPILER) && defined(BOOST_SIMD_HAS_AVX_SUPPORT) && !defined(BOOST_SIMD_HAS_AVX2_SUPPORT)
      x = x - if_else(a0, Maxexponent<A0>(), Zero<result_type>());
      #else
      x = selsub(a0, x, Maxexponent<A0>());
      #endif

      #ifndef BOOST_SIMD_NO_INVALIDS
      return if_zero_else( is_invalid(a0), x );
      #else
      return x;
      #endif
    }
  };
} } }

#endif


/*
 * nav_graph.cpp
 *
 * Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Arnaud Degroote <arnaud.degroote@laas.fr>
 * created: 2013-09-10
 * license: BSD
 */
#include "gladys/dstar.hpp"
#include <boost/graph/graphviz.hpp>
#include <limits>
#include <iostream>

namespace gladys {

    dstar_search::dstar_search(const graph_t& g, const vertex_t& start,
                                                 const vertex_t& goal):
        g(g), start(start), goal(goal), last(start), t(std::time(0)),
        costs(boost::num_vertices(g),
              {std::numeric_limits<float>::infinity(),
               std::numeric_limits<float>::infinity()}
              ), km(0)
    {
        costs[goal].rhs = 0;
        pq.insert(bm_value(goal, calc_key(goal)));
        compute_shortest_path();
    }

    dstar_search::key_t
    dstar_search::calc_key(const vertex_t& v) const
    {
        const cost& c = costs[v];
        float min = std::min(c.rhs, c.g);
        return std::make_pair(min + h(start, v) + km, min);
    }

    void
    dstar_search::update_vertex(const vertex_t& v)
    {
        cost& c = costs[v];
        if (v != goal) {
            float min = std::numeric_limits<float>::infinity();
            boost::graph_traits<graph_t>::out_edge_iterator ei,ei_end;
            for (boost::tie(ei,ei_end) = boost::out_edges(v, g); ei != ei_end; ++ei)
                min = std::min(min, g[*ei].weight + costs[target(*ei, g)].g);
            c.rhs = min;
        }

        auto it = pq.left.find(v);
        if (it != pq.left.end()) pq.left.erase(it);

        if (c.g != c.rhs)
            pq.insert(bm_value(v, calc_key(v)));
    }

    void
    dstar_search::compute_shortest_path()
    {
        int i = 0;
        while (pq.right.begin()->first < calc_key(start) or
               costs[start].rhs != costs[start].g)
        {
            auto it = pq.right.begin();
            key_t kold = it->first;
            vertex_t v = it->second;
            pq.right.erase(it);

            key_t k = calc_key(v);
            cost& cv = costs[v];

            if (kold < k) {
                pq.insert(bm_value(v, k));
            } else if (cv.g > cv.rhs) {
                cv.g = cv.rhs;
                boost::graph_traits<graph_t>::adjacency_iterator vi,vi_end;
                for (boost::tie(vi, vi_end) = boost::adjacent_vertices(v, g);
                    vi != vi_end; ++vi) {
                    update_vertex(*vi);
                }
            } else {
                cv.g = std::numeric_limits<float>::infinity();
                update_vertex(v);
                boost::graph_traits<graph_t>::adjacency_iterator vi,vi_end;
                for (boost::tie(vi, vi_end) = boost::adjacent_vertices(v, g);
                    vi != vi_end; ++vi)
                    update_vertex(*vi);
            }

            ++i;
        }

        if (pq.empty()) throw no_path();
    }

    float
    dstar_search::h(const vertex_t& v1, const vertex_t& v2) const
    {
        return distance(g[v1].pt, g[v2].pt);
    }

    path_t
    dstar_search::get_path() const
    {
        path_t res;
        if (costs[last].g == std::numeric_limits<float>::infinity())
            throw no_path();

        res.push_back(g[last].pt);

        vertex_t v = last;
        while (v != goal) {
            boost::graph_traits<graph_t>::out_edge_iterator ei,ei_end;
            boost::tie(ei, ei_end) = boost::out_edges(v, g);
            vertex_t best_v = boost::target(*ei, g);
            float min = g[*ei].weight + costs[best_v].g;
            ++ ei;
            for (; ei != ei_end; ++ei) {
                vertex_t v1 = boost::target(*ei, g);
                float c = g[*ei].weight + costs[v1].g;
                if (min > c) {
                    min = c;
                    best_v = v1;
                }
            }
            v = best_v;
            res.push_back(g[v].pt);
        }

        return res;
    }

    void
    dstar_search::print_pq() const
    {
        for( bm_type::right_const_iterator
            i = pq.right.begin(), iend = pq.right.end();
            i != iend ; ++i )
        {
            std::cerr << i->second << " with [" << i->first.first;
            std::cerr << ", " << i->first.second << "]" << std::endl;
        }
    }

    struct graph_property_writer
    {
        void operator()(std::ostream& out) const
        {
            out << "node [shape=box]" << std::endl;
        }
    };

    void
    dstar_search::write_graphviz(std::ostream& os) const
    {
        std::vector<std::string> vert_label(costs.size());
        boost::graph_traits<graph_t>::vertex_iterator vi,vi_end;
        boost::tie(vi, vi_end) = boost::vertices(g);
        for (; vi != vi_end; ++vi) {
            std::ostringstream oss;
            const cost& c = costs[*vi];
            oss << "pt " << g[*vi].pt << " g: " << c.g << " rhs: " << c.rhs;
            vert_label[*vi] = oss.str();
        }

        boost::write_graphviz(os, g, boost::make_label_writer(vert_label.data()),
                                     boost::default_writer(),
                                     graph_property_writer());
    }

    void
    dstar_search::replan(const vertex_t& v)
    {
        time_t current_time = std::time(0);
        km = km + h(v, last);
        last = v;

        boost::graph_traits<graph_t>::edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = boost::edges(g) ; ei != ei_end; ++ei)
            if (g[*ei].t > t) {
                vertex_t source = boost::source(*ei, g);
                vertex_t target = boost::target(*ei, g);
                update_vertex(source);
                update_vertex(target);
            }

        t = current_time;

        compute_shortest_path();
    }
} // namespace gladys


#pragma once
#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/solver_interface.hpp>

namespace sco
{
#if 0
Matrix3d leftCrossProdMat(const Vector3d& x);
Matrix3d rightCrossProdMat(const Vector3d& x);

ExprVector exprMatMult(const MatrixXd& A, const VarVector& x);
ExprVector exprMatMult(const MatrixXd& A, const ExprVector& x);

ExprVector exprCross(const VectorXd& x, const VarVector& y);
ExprVector exprCross(const VarVector& x, const VectorXd& y);
ExprVector exprCross(const VectorXd& x, const ExprVector& y);
ExprVector exprCross(const ExprVector& x, const VectorXd& y);

#endif
AffExpr varDot(const Eigen::VectorXd& x, const VarVector& v);
AffExpr exprDot(const Eigen::VectorXd& x, const AffExprVector& v);
#if 0
QuadExpr varNorm2(const VarVector& v);
QuadExpr exprNorm2(const ExprVector& v);
#endif
}


#define BOOST_TEST_MODULE "test_3SPN2BaseBaseInteraction_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/3SPN2/ThreeSPN2BaseBaseInteractionPotential.hpp>

using parameters_to_test_double = std::tuple<
    mjolnir::ThreeSPN2BaseBaseGlobalPotentialParameter<double>,
    mjolnir::ThreeSPN2CBaseBaseGlobalPotentialParameter<double>
>;

using parameters_to_test_float = std::tuple<
    mjolnir::ThreeSPN2BaseBaseGlobalPotentialParameter<float>,
    mjolnir::ThreeSPN2CBaseBaseGlobalPotentialParameter<float>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(f_3SPN2_BaseBaseInteraction_double,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type    = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type      = traits_type::real_type;
    using potential_type = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind = mjolnir::parameter_3SPN2::base_pair_kind  ;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type  pi = mjolnir::math::constants<real_type>::pi();

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    const real_type theta_0   = 0.5  * pi;
    const real_type theta_min = 0.01 * pi;
    const real_type theta_max = 0.99 * pi;
    const real_type dtheta    = (theta_max - theta_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type theta = theta_min + i * dtheta;
        const real_type f1  = potential.f(base_pair_kind::AT, theta + h, theta_0);
        const real_type f2  = potential.f(base_pair_kind::AT, theta - h, theta_0);
        const real_type df_numeric  = (f1 - f2) / (2 * h);
        const real_type df_analytic = potential.df(base_pair_kind::AT, theta, theta_0);

        if((f1 == 0.0 || f2 == 0.0) && !(f1 == 0.0 && f2 == 0.0))
        {
            // the numeric differentiation becomes unstable.
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(f_3SPN2_BaseBaseInteraction_float,
        ParameterSet, parameters_to_test_float)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type    = mjolnir::SimulatorTraits<float, mjolnir::UnlimitedBoundary>;
    using real_type      = traits_type::real_type;
    using potential_type = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind = mjolnir::parameter_3SPN2::base_pair_kind  ;

    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;
    constexpr real_type  pi = mjolnir::math::constants<real_type>::pi();

    potential_type potential(
        ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    const real_type theta_0  = 0.5  * pi;
    const real_type theta_min = 0.01 * pi;
    const real_type theta_max = 0.99 * pi;
    const real_type dtheta    = (theta_max - theta_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type theta = theta_min + i * dtheta;
        const real_type f1  = potential.f(base_pair_kind::AT, theta + h, theta_0);
        const real_type f2  = potential.f(base_pair_kind::AT, theta - h, theta_0);
        const real_type df_numeric  = (f1 - f2) / (2 * h);
        const real_type df_analytic = potential.df(base_pair_kind::AT, theta, theta_0);

        if((f1 == 0.0f || f2 == 0.0f) && !(f1 == 0.0f && f2 == 0.0f))
        {
            // the numeric differentiation becomes unstable.
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_rep_3SPN2_BaseBaseInteraction_double,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type    = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type      = traits_type::real_type;
    using potential_type = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind = mjolnir::parameter_3SPN2::base_pair_kind  ;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    const real_type r_min =  3.0;
    const real_type r_max = 12.0;
    const real_type dr    = (r_max - r_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const auto r         = r_min + i * dr;
        const real_type pot1 = potential.U_rep(base_pair_kind::AT, r + h);
        const real_type pot2 = potential.U_rep(base_pair_kind::AT, r - h);
        const real_type df_numeric  = (pot1 - pot2) / (2 * h);
        const real_type df_analytic = potential.dU_rep(base_pair_kind::AT, r);

        if(pot1 != 0.0 && pot2 == 0.0)
        {
            // the numeric differentiation becomes unstable.
        }
        else if(df_analytic == 0.0)
        {
            BOOST_TEST(potential.U_rep(base_pair_kind::AT, r) == 0.0,
                       boost::test_tools::tolerance(h));
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(h));
        }
    }
}
BOOST_AUTO_TEST_CASE_TEMPLATE(U_attr_3SPN2_BaseBaseInteraction_double,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type    = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type      = traits_type::real_type;
    using potential_type = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind = mjolnir::parameter_3SPN2::base_pair_kind  ;

    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    const real_type r_min =  3.0; // avoiding the long tail around cutoff;
    const real_type r_max = 12.0; // not take numerical error around zero into account
    const real_type dr    = (r_max - r_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const auto r         = r_min + i * dr;
        const real_type pot1 = potential.U_attr(base_pair_kind::AT, r + h);
        const real_type pot2 = potential.U_attr(base_pair_kind::AT, r - h);
        const real_type df_numeric  = (pot1 - pot2) / (2 * h);
        const real_type df_analytic = potential.dU_attr(base_pair_kind::AT, r);

        if(pot1 != 0.0 && pot2 == 0.0)
        {
            // the numeric differentiation becomes unstable.
        }
        else if(df_analytic == 0.0)
        {
            // dU == 0 means that r is in `U == -epsilon` part.
            BOOST_TEST(potential.U_attr(base_pair_kind::AT, r) ==
                       -potential.epsilon(base_pair_kind::AT),
                       boost::test_tools::tolerance(h));
        }
        else
        {
            BOOST_TEST(df_numeric == df_analytic, boost::test_tools::tolerance(1e-5));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_dU_attr_3SPN2_BaseBaseInteraction_double,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type    = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type      = traits_type::real_type;
    using potential_type = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind   = mjolnir::parameter_3SPN2::base_pair_kind  ;
    using cross_stack_kind = mjolnir::parameter_3SPN2::cross_stack_kind;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-8;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );
    for(const auto bp : {base_pair_kind::AT, base_pair_kind::TA,
                         base_pair_kind::GC, base_pair_kind::CG})
    {
        const real_type r_min =  3.0;
        const real_type r_max = 18.0;
        const real_type dr    = (r_max - r_min) / N;

        for(std::size_t i=0; i<N; ++i)
        {
            const auto r         = r_min + i * dr;
            const auto U_attr    = potential.U_attr(bp, r);
            const auto dU_attr   = potential.dU_attr(bp, r);
            const auto U_dU_attr = potential.U_dU_attr(bp, r);

            BOOST_TEST(U_dU_attr.first  == U_attr,  boost::test_tools::tolerance(h));
            BOOST_TEST(U_dU_attr.second == dU_attr, boost::test_tools::tolerance(h));
        }
    }
    for(const auto cs : {cross_stack_kind::AA5, cross_stack_kind::AA3,
                         cross_stack_kind::AT5, cross_stack_kind::AT3,
                         cross_stack_kind::AG5, cross_stack_kind::AG3,
                         cross_stack_kind::AC5, cross_stack_kind::AC3,
                         cross_stack_kind::TA5, cross_stack_kind::TA3,
                         cross_stack_kind::TT5, cross_stack_kind::TT3,
                         cross_stack_kind::TG5, cross_stack_kind::TG3,
                         cross_stack_kind::TC5, cross_stack_kind::TC3,
                         cross_stack_kind::GA5, cross_stack_kind::GA3,
                         cross_stack_kind::GT5, cross_stack_kind::GT3,
                         cross_stack_kind::GG5, cross_stack_kind::GG3,
                         cross_stack_kind::GC5, cross_stack_kind::GC3,
                         cross_stack_kind::CA5, cross_stack_kind::CA3,
                         cross_stack_kind::CT5, cross_stack_kind::CT3,
                         cross_stack_kind::CG5, cross_stack_kind::CG3,
                         cross_stack_kind::CC5, cross_stack_kind::CC3})
    {
        const real_type r_min =  3.0;
        const real_type r_max = 18.0;
        const real_type dr    = (r_max - r_min) / N;

        for(std::size_t i=0; i<N; ++i)
        {
            const auto r         = r_min + i * dr;
            const auto U_attr    = potential.U_attr(cs, r);
            const auto dU_attr   = potential.dU_attr(cs, r);
            const auto U_dU_attr = potential.U_dU_attr(cs, r);

            BOOST_TEST(U_dU_attr.first  == U_attr,  boost::test_tools::tolerance(h));
            BOOST_TEST(U_dU_attr.second == dU_attr, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(U_dU_attr_3SPN2_BaseBaseInteraction_float,
        ParameterSet, parameters_to_test_float)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type      = mjolnir::SimulatorTraits<float, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using potential_type   = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_pair_kind   = mjolnir::parameter_3SPN2::base_pair_kind;
    using cross_stack_kind = mjolnir::parameter_3SPN2::cross_stack_kind;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-4;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    for(const auto bp : {base_pair_kind::AT, base_pair_kind::TA,
                         base_pair_kind::GC, base_pair_kind::CG})
    {
        const real_type r_min =  3.0;
        const real_type r_max = 18.0;
        const real_type dr    = (r_max - r_min) / N;

        for(std::size_t i=0; i<N; ++i)
        {
            const auto r         = r_min + i * dr;
            const auto U_attr    = potential.U_attr(bp, r);
            const auto dU_attr   = potential.dU_attr(bp, r);
            const auto U_dU_attr = potential.U_dU_attr(bp, r);

            BOOST_TEST(U_dU_attr.first  == U_attr,  boost::test_tools::tolerance(h));
            BOOST_TEST(U_dU_attr.second == dU_attr, boost::test_tools::tolerance(h));
        }
    }
    for(const auto cs : {cross_stack_kind::AA5, cross_stack_kind::AA3,
                         cross_stack_kind::AT5, cross_stack_kind::AT3,
                         cross_stack_kind::AG5, cross_stack_kind::AG3,
                         cross_stack_kind::AC5, cross_stack_kind::AC3,
                         cross_stack_kind::TA5, cross_stack_kind::TA3,
                         cross_stack_kind::TT5, cross_stack_kind::TT3,
                         cross_stack_kind::TG5, cross_stack_kind::TG3,
                         cross_stack_kind::TC5, cross_stack_kind::TC3,
                         cross_stack_kind::GA5, cross_stack_kind::GA3,
                         cross_stack_kind::GT5, cross_stack_kind::GT3,
                         cross_stack_kind::GG5, cross_stack_kind::GG3,
                         cross_stack_kind::GC5, cross_stack_kind::GC3,
                         cross_stack_kind::CA5, cross_stack_kind::CA3,
                         cross_stack_kind::CT5, cross_stack_kind::CT3,
                         cross_stack_kind::CG5, cross_stack_kind::CG3,
                         cross_stack_kind::CC5, cross_stack_kind::CC3})
    {
        const real_type r_min =  3.0;
        const real_type r_max = 18.0;
        const real_type dr    = (r_max - r_min) / N;

        for(std::size_t i=0; i<N; ++i)
        {
            const auto r         = r_min + i * dr;
            const auto U_attr    = potential.U_attr(cs, r);
            const auto dU_attr   = potential.dU_attr(cs, r);
            const auto U_dU_attr = potential.U_dU_attr(cs, r);

            BOOST_TEST(U_dU_attr.first  == U_attr,  boost::test_tools::tolerance(h));
            BOOST_TEST(U_dU_attr.second == dU_attr, boost::test_tools::tolerance(h));
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(bp_kind_3SPN2_BaseBaseInteraction,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using potential_type   = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_kind        = mjolnir::parameter_3SPN2::base_kind;
    using base_pair_kind   = mjolnir::parameter_3SPN2::base_pair_kind;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    BOOST_TEST(base_pair_kind::AT == potential.bp_kind(base_kind::A, base_kind::T));
    BOOST_TEST(base_pair_kind::TA == potential.bp_kind(base_kind::T, base_kind::A));
    BOOST_TEST(base_pair_kind::GC == potential.bp_kind(base_kind::G, base_kind::C));
    BOOST_TEST(base_pair_kind::CG == potential.bp_kind(base_kind::C, base_kind::G));
}
BOOST_AUTO_TEST_CASE_TEMPLATE(cs_kind_3SPN2_BaseBaseInteraction,
        ParameterSet, parameters_to_test_double)
{
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_base_interaction_potential.log");
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using potential_type   = mjolnir::ThreeSPN2BaseBaseInteractionPotential<traits_type>;
    using base_kind = mjolnir::parameter_3SPN2::base_kind;
    using cross_stack_kind = mjolnir::parameter_3SPN2::cross_stack_kind;

    potential_type potential(ParameterSet{}, {}, {},
        typename potential_type::ignore_molecule_type("Nothing"),
        typename potential_type::ignore_group_type({})
        );

    BOOST_TEST(cross_stack_kind::AA5 == potential.cs5_kind(base_kind::A, base_kind::A));
    BOOST_TEST(cross_stack_kind::AT5 == potential.cs5_kind(base_kind::A, base_kind::T));
    BOOST_TEST(cross_stack_kind::AG5 == potential.cs5_kind(base_kind::A, base_kind::G));
    BOOST_TEST(cross_stack_kind::AC5 == potential.cs5_kind(base_kind::A, base_kind::C));
    BOOST_TEST(cross_stack_kind::TA5 == potential.cs5_kind(base_kind::T, base_kind::A));
    BOOST_TEST(cross_stack_kind::TT5 == potential.cs5_kind(base_kind::T, base_kind::T));
    BOOST_TEST(cross_stack_kind::TG5 == potential.cs5_kind(base_kind::T, base_kind::G));
    BOOST_TEST(cross_stack_kind::TC5 == potential.cs5_kind(base_kind::T, base_kind::C));
    BOOST_TEST(cross_stack_kind::GA5 == potential.cs5_kind(base_kind::G, base_kind::A));
    BOOST_TEST(cross_stack_kind::GT5 == potential.cs5_kind(base_kind::G, base_kind::T));
    BOOST_TEST(cross_stack_kind::GG5 == potential.cs5_kind(base_kind::G, base_kind::G));
    BOOST_TEST(cross_stack_kind::GC5 == potential.cs5_kind(base_kind::G, base_kind::C));
    BOOST_TEST(cross_stack_kind::CA5 == potential.cs5_kind(base_kind::C, base_kind::A));
    BOOST_TEST(cross_stack_kind::CT5 == potential.cs5_kind(base_kind::C, base_kind::T));
    BOOST_TEST(cross_stack_kind::CG5 == potential.cs5_kind(base_kind::C, base_kind::G));
    BOOST_TEST(cross_stack_kind::CC5 == potential.cs5_kind(base_kind::C, base_kind::C));

    BOOST_TEST(cross_stack_kind::AA3 == potential.cs3_kind(base_kind::A, base_kind::A));
    BOOST_TEST(cross_stack_kind::AT3 == potential.cs3_kind(base_kind::A, base_kind::T));
    BOOST_TEST(cross_stack_kind::AG3 == potential.cs3_kind(base_kind::A, base_kind::G));
    BOOST_TEST(cross_stack_kind::AC3 == potential.cs3_kind(base_kind::A, base_kind::C));
    BOOST_TEST(cross_stack_kind::TA3 == potential.cs3_kind(base_kind::T, base_kind::A));
    BOOST_TEST(cross_stack_kind::TT3 == potential.cs3_kind(base_kind::T, base_kind::T));
    BOOST_TEST(cross_stack_kind::TG3 == potential.cs3_kind(base_kind::T, base_kind::G));
    BOOST_TEST(cross_stack_kind::TC3 == potential.cs3_kind(base_kind::T, base_kind::C));
    BOOST_TEST(cross_stack_kind::GA3 == potential.cs3_kind(base_kind::G, base_kind::A));
    BOOST_TEST(cross_stack_kind::GT3 == potential.cs3_kind(base_kind::G, base_kind::T));
    BOOST_TEST(cross_stack_kind::GG3 == potential.cs3_kind(base_kind::G, base_kind::G));
    BOOST_TEST(cross_stack_kind::GC3 == potential.cs3_kind(base_kind::G, base_kind::C));
    BOOST_TEST(cross_stack_kind::CA3 == potential.cs3_kind(base_kind::C, base_kind::A));
    BOOST_TEST(cross_stack_kind::CT3 == potential.cs3_kind(base_kind::C, base_kind::T));
    BOOST_TEST(cross_stack_kind::CG3 == potential.cs3_kind(base_kind::C, base_kind::G));
    BOOST_TEST(cross_stack_kind::CC3 == potential.cs3_kind(base_kind::C, base_kind::C));

}


/**
 * tod_eci with velocity component
 *
 * Copyright 2013 Bruce Ide
 *
 *  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.

 */

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "xyz_velocity.hpp"

#ifndef _HPP_TOD_ECI_VEL
#define _HPP_TOD_ECI_VEL

namespace fr {

  namespace coordinates {

    class tod_eci_vel : public xyz_velocity {

    public:
      typedef xyz_velocity super;

      tod_eci_vel(const double &x, const double &y, const double &z, const double &dx, const double &dy, const double &dz) : super(x,y,z,dx,dy,dz)
      {
      }

      tod_eci_vel(const tod_eci_vel &copy) : super(copy.get_x(), copy.get_y(), copy.get_z(), copy.get_dx(), copy.get_dy(), copy.get_dz())
      {
      }

      tod_eci_vel() : super(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
      {
      }

      virtual ~tod_eci_vel()
      {
      }

      /**
       * Interpolates a point between two points. Takes a time observed
       * at the current point, another tod_eci_vel, time observed at that
       * point and the time in between for the point you want.
       */

      virtual tod_eci_vel interpolate(const double &time_now, const tod_eci_vel &next_point, const double &time_then, const double &time_between)
      {
	Eigen::Vector3d xyz_now = get_xyz();
	Eigen::Vector3d xyz_then = next_point.get_xyz();
	Eigen::Vector3d vel_now = get_deltas();
	Eigen::Vector3d vel_then = next_point.get_deltas();
	Eigen::Vector3d xyz_between = interpolate_3x3(xyz_now, time_now, xyz_then, time_then, time_between);
	Eigen::Vector3d vel_between = interpolate_3x3(vel_now, time_now, vel_then, time_then, time_between);
	tod_eci_vel retval(xyz_between(0), xyz_between(1), xyz_between(2), vel_between(0), vel_between(1), vel_between(2));
	return retval;
      }

    };

  }

}

#endif


#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <laser_geometry/laser_geometry.h>
#include <std_srvs/Empty.h>
#include <pcl_ros/point_cloud.h>

#include <boost/uuid/uuid_io.hpp>

#include <slam3d/core/Mapper.hpp>
#include <slam3d/graph/boost/BoostGraph.hpp>
#include <slam3d/sensor/pointmatcher/Scan2DSensor.hpp>
#include <slam3d/sensor/pcl/PointCloudSensor.hpp>
#include <slam3d/solver/g2o/G2oSolver.hpp>

#include <iostream>

#include "GraphPublisher.hpp"
#include "ros_common.hpp"
#include "ros_tf.hpp"
#include "helper_functions.hpp"

tf::TransformBroadcaster* gTransformBroadcaster;
tf::TransformListener* gTransformListener;

ros::Publisher* gMapPublisher;
ros::Publisher* gCloudPublisher;
ros::ServiceServer* gOptimizeService;
ros::ServiceServer* gShowMapService;
ros::ServiceServer* gWriteGraphService;
GraphPublisher* gGraphPublisher;

slam3d::Logger* gLogger;
slam3d::BoostGraph* gGraph;
slam3d::Mapper* gMapper;
slam3d::Scan2DSensor* gScan2DSensor;
slam3d::PointCloudSensor* gPclSensor;
slam3d::G2oSolver* gSolver;
slam3d::G2oSolver* gPatchSolver;

std::string gRobotName;
std::string gLaserName;
std::string gCameraName;

std::string gOdometryFrame;
std::string gRobotFrame;
std::string gMapFrame;
std::string gLaserFrame;
std::string gCameraFrame;

double gScanResolution;
double gMapOutlierRadius;
int gMapOutlierNeighbors;

slam3d::Transform gSensorPose;
Odometry* gOdometry;
tf::StampedTransform gOdomInMap;

typedef slam3d::PM::DataPoints::Label Label;
typedef slam3d::PM::DataPoints::Labels Labels;

using namespace slam3d;

bool optimize(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res)
{
	gSolver->saveGraph("input_graph.g2o");
	return gGraph->optimize();
}

typedef Eigen::Matrix<ScalarType,4,1> Point;

bool show_map(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res)
{
	// Show 2D map
	sensor_msgs::PointCloud pc_msg;
	pc_msg.header.stamp = ros::Time::now();
	pc_msg.header.frame_id = gMapFrame;

	VertexObjectList vertices = gGraph->getVerticesFromSensor(gLaserName);
	for(VertexObjectList::iterator v = vertices.begin(); v != vertices.end(); v++)
	{
		Point point;
		point(2) = 0; // z = 0
		point(3) = 1; // homogenous vector
		Scan2DMeasurement::Ptr scan = boost::dynamic_pointer_cast<Scan2DMeasurement>(v->measurement);
		assert(scan);

		Transform pose = v->corrected_pose * v->measurement->getSensorPose();

		// Transform to map and write into ROS-Msg
		unsigned numPoints = scan->getDataPoints().features.cols();
		for(unsigned p = 0; p < numPoints; p++)
		{
			point(0) = scan->getDataPoints().features(0,p);
			point(1) = scan->getDataPoints().features(1,p);
			Point point_tf = pose * point;

			geometry_msgs::Point32 p_msg;
			p_msg.x = point_tf(0);
			p_msg.y = point_tf(1);
			p_msg.z = point_tf(2);
			pc_msg.points.push_back(p_msg);
		}
	}
	gMapPublisher->publish(pc_msg);

	// Show 3D map
	PointCloud::Ptr downsampled = gPclSensor->buildMap(gGraph->getVerticesFromSensor(gPclSensor->getName()));
	gLogger->message(INFO, (boost::format("Map cloud has %1% points.") % downsampled->size()).str());
	downsampled->header.frame_id = gMapFrame;
	gCloudPublisher->publish(downsampled);

	return true;
}

bool write_graph(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res)
{
	gGraph->writeGraphToFile("pose_graph");
	return true;
}

void receiveScan(const sensor_msgs::LaserScan::ConstPtr& scan)
{
	Labels feat_labels;
	feat_labels.push_back(Label("x", 1));
	feat_labels.push_back(Label("y", 1));
	feat_labels.push_back(Label("w", 1));

	laser_geometry::LaserProjection projection;
	sensor_msgs::PointCloud cloud;
	projection.projectLaser(*scan, cloud);
	PM::Matrix feat_points(3, cloud.points.size());
	unsigned i = 0;
	for(auto it = cloud.points.begin(); it < cloud.points.end(); it++)
	{
		feat_points(0, i) = it->x;
		feat_points(1, i) = it->y;
		feat_points(2, i) = 1;
		i++;
	}

	PM::DataPoints dp(feat_points, feat_labels);
	ros::Time rostime = scan->header.stamp;
	timeval stamp;
	stamp.tv_sec  = rostime.sec;
	stamp.tv_usec = rostime.nsec / 1000;

	// Get the pose of the laser scanner
	tf::StampedTransform laser_pose;
	try
	{
		gTransformListener->waitForTransform(gRobotFrame, gLaserFrame, rostime, ros::Duration(0.1));
		gTransformListener->lookupTransform(gRobotFrame, gLaserFrame, rostime, laser_pose);
	}catch(tf2::TransformException &e)
	{
		ROS_WARN("Could not get transform: '%s' => '%s'!", gRobotFrame.c_str(), gLaserFrame.c_str());
		return;
	}

	Scan2DMeasurement::Ptr m(new Scan2DMeasurement(dp, stamp, gRobotName, gLaserName, tf2eigen(laser_pose)));

	bool added;
	try
	{
		added = gScan2DSensor->addMeasurement(m, gOdometry->getPose(stamp));
	}catch(std::exception &e)
	{
		ROS_ERROR("Could not add scan: %s", e.what());
		return;
	}

	slam3d::Transform current = gMapper->getCurrentPose();
	if(current.matrix().determinant() == 0)
	{
		ROS_ERROR("Current pose from mapper has 0 determinant!");
	}

	// Publish the transformations
	if(added)
	{
		tf::Stamped<tf::Pose> map_in_robot(eigen2tf(current.inverse()), rostime, gRobotFrame);
		tf::Stamped<tf::Pose> map_in_odom;
		try
		{
			gTransformListener->waitForTransform(gRobotFrame, gOdometryFrame, rostime, ros::Duration(0.1));
			gTransformListener->transformPose(gOdometryFrame, map_in_robot, map_in_odom);
			gOdomInMap = tf::StampedTransform(map_in_odom.inverse(), rostime, gMapFrame, gOdometryFrame);
		}catch(tf2::TransformException &e)
		{
			ROS_ERROR("%s", e.what());
		}
	}
	gOdomInMap.stamp_ = rostime;
	gTransformBroadcaster->sendTransform(gOdomInMap);

	// Publish the graph
	if(added)
	{
		gGraphPublisher->publishNodes(scan->header.stamp, gMapFrame);
		gGraphPublisher->publishEdges(gScan2DSensor->getName(), scan->header.stamp, gMapFrame);
		gGraphPublisher->publishEdges(gPclSensor->getName(), scan->header.stamp, gMapFrame);
	}
}

void receivePointCloud(const slam3d::PointCloud::ConstPtr& pcl)
{
	// Get the pose of the laser scanner
	ros::Time t;
	t.fromNSec(pcl->header.stamp * 1000);
	tf::StampedTransform laser_pose;
	try
	{
		gTransformListener->waitForTransform(gRobotFrame, gCameraFrame, t, ros::Duration(0.1));
		gTransformListener->lookupTransform(gRobotFrame, gCameraFrame, t, laser_pose);
	}catch(tf2::TransformException &e)
	{
		ROS_WARN("Could not get transform: '%s' => '%s'!", gRobotFrame.c_str(), gLaserFrame.c_str());
		return;
	}

	slam3d::PointCloud::Ptr cloud;
	if(gScanResolution > 0)
	{
		cloud = gPclSensor->downsample(pcl, gScanResolution);
	}else
	{
		cloud = slam3d::PointCloud::Ptr(new slam3d::PointCloud(*pcl));
	}

	bool added;
	slam3d::PointCloudMeasurement::Ptr m(
		new slam3d::PointCloudMeasurement(cloud, gRobotName, gCameraName, tf2eigen(laser_pose)));
	try
	{
		added = gPclSensor->addMeasurement(m, gOdometry->getPose(m->getTimestamp()));
	}catch(std::exception &e)
	{
		ROS_ERROR("Could not add new pointcloud measurement: %s", e.what());
		return;
	}

	if(added)
	{
		gPclSensor->linkLastToNeighbors(true);
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "mapper2d");
	ros::NodeHandle n;
	ros::NodeHandle pn("~/");

	gTransformBroadcaster = new tf::TransformBroadcaster;
	gTransformListener = new tf::TransformListener();

	gLogger = new RosLogger();
	gLogger->setLogLevel(DEBUG);

	gGraph = new slam3d::BoostGraph(gLogger);
	gMapper = new slam3d::Mapper(gGraph, gLogger);
	gSolver = new slam3d::G2oSolver(gLogger);
	gGraph->setSolver(gSolver);

	pn.param("robot_name", gRobotName, std::string("Robot"));
	pn.param("odometry_frame", gOdometryFrame, std::string("odometry"));
	pn.param("robot_frame", gRobotFrame, std::string("robot"));
	pn.param("map_frame", gMapFrame, std::string("map"));
	pn.param("laser_frame", gLaserFrame, std::string("laser"));
	pn.param("camera_frame", gCameraFrame, std::string("camera"));

	// Apply tf-prefix to all frames
	gRobotFrame = gTransformListener->resolve(gRobotFrame);
	gOdometryFrame = gTransformListener->resolve(gOdometryFrame);
	gMapFrame = gTransformListener->resolve(gMapFrame);
	gLaserFrame = gTransformListener->resolve(gLaserFrame);

	// Create the ScanSensor for the 2d laser
	std::string icp_config;
	pn.param("laser_name", gLaserName, std::string("LineScanner"));
	pn.param("icp_config_file", icp_config, std::string(""));
	gScan2DSensor = new slam3d::Scan2DSensor(gLaserName, gLogger, icp_config);
	readScanSensorParameters(pn, gScan2DSensor);
	gScan2DSensor->writeDebugData();
	gMapper->registerSensor(gScan2DSensor);

	// Create the PointcloudSensor
	pn.param("camera_name", gCameraName, std::string("DepthCam"));
	gPclSensor = new PointCloudSensor(gCameraName, gLogger);
	readPointcloudSensorParameters(pn, gPclSensor);

	pn.param("scan_resolution", gScanResolution, 0.5);

	gPatchSolver = new G2oSolver(gLogger);
	gPclSensor->setPatchSolver(gPatchSolver);

	gMapper->registerSensor(gPclSensor);
	gGraph->fixNext();

	// Create the Odometry
	gOdometry = new Odometry(gGraph, gLogger);
	gOdometry->setTF(gTransformListener, gOdometryFrame, gRobotFrame);
	gMapper->registerPoseSensor(gOdometry);

	// Subscribe to point cloud
	ros::Subscriber scanSub = n.subscribe<sensor_msgs::LaserScan>("scan", 10, &receiveScan);
	ros::Subscriber pclSub = n.subscribe<slam3d::PointCloud>("pointcloud", 10, &receivePointCloud);
	ros::ServiceServer optSrv = n.advertiseService("optimize", &optimize);
	ros::ServiceServer showSrv = n.advertiseService("show_map", &show_map);
	ros::ServiceServer writeSrv = n.advertiseService("write_graph", &write_graph);
	ros::Publisher pclPub = n.advertise<sensor_msgs::PointCloud>("map", 1);
	ros::Publisher cloudPub = n.advertise<slam3d::PointCloud>("map_cloud", 1);

	gOptimizeService = &optSrv;
	gShowMapService = &showSrv;
	gWriteGraphService = &writeSrv;
	gMapPublisher = &pclPub;
	gCloudPublisher = &cloudPub;
	gGraphPublisher = new GraphPublisher(gGraph);
	gGraphPublisher->addNodeSensor(gLaserName, 0,0.8,0);
	gGraphPublisher->addNodeSensor(gCameraName, 0, 0, 0.8);
	gGraphPublisher->addEdgeSensor(gOdometry->getName());
	gGraphPublisher->addEdgeSensor(gLaserName);
	gGraphPublisher->addEdgeSensor(gCameraName);

	gLogger->message(INFO, "Mapper2D is ready!");

	ros::spin();

	delete gMapper;
	delete gGraph;
	delete gGraphPublisher;
	delete gScan2DSensor;
	delete gPatchSolver;
	delete gSolver;
	delete gLogger;
	delete gTransformBroadcaster;
	delete gTransformListener;
	return 0;
}