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

#ifndef CRYPTO3_ACCUMULATORS_ZK_SPARSE_HPP
#define CRYPTO3_ACCUMULATORS_ZK_SPARSE_HPP

#include <boost/container/static_vector.hpp>

#include <boost/parameter/value_type.hpp>

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>

#include <nil/crypto3/zk/snark/accumulators/parameters/offset.hpp>

#include <nil/crypto3/algebra/multiexp/multiexp.hpp>

namespace nil {
    namespace crypto3 {
        namespace accumulators {
            namespace detail {
                template<typename T>
                struct sparse_impl : boost::accumulators::accumulator_base {
                    typedef typename T::value_type value_type;
                    typedef std::vector<std::size_t> indicies_type;

                    typedef std::pair<value_type, std::pair<indicies_type, std::vector<T>>> result_type;

                    template<typename Args>
                    sparse_impl(const Args &args) : in_block(false), accumulated_value(value_type::zero()) {
                    }

                    template<typename ArgumentPack>
                    inline void operator()(const ArgumentPack &args) {
                        resolve_type(args[boost::accumulators::sample], args[::nil::crypto3::accumulators::offset]);
                    }

                    inline result_type result(boost::accumulators::dont_care) const {
                    }

                protected:
                    template<typename SinglePassRange>
                    inline result_type resolve_type(const SinglePassRange r, std::size_t offset) {
                        const std::size_t chunks = 1;

                        std::pair<indicies_type, std::vector<T>> resulting_vector;
                        resulting_vector.domain_size_ = domain_size_;

                        const std::size_t range_len = r.size();
                        std::size_t first_pos = -1, last_pos = -1;
                        for (std::size_t i = 0; i < indices.size(); ++i) {
                            const bool matching_pos = (offset <= indices[i] && indices[i] < offset + range_len);
                            bool copy_over;

                            if (in_block) {
                                if (matching_pos && last_pos == i - 1) {
                                    // block can be extended, do it
                                    last_pos = i;
                                    copy_over = false;
                                } else {
                                    // block has ended here
                                    in_block = false;
                                    copy_over = true;

                                    accumulated_value =
                                        accumulated_value +
                                        algebra::multiexp<algebra::policies::multiexp_method_bos_coster>(
                                            values.begin() + first_pos, values.begin() + last_pos + 1,
                                            std::begin(r) + (indices[first_pos] - offset),
                                            std::begin(r) + (indices[last_pos] - offset) + 1, chunks);
                                }
                            } else {
                                if (matching_pos) {
                                    // block can be started
                                    first_pos = i;
                                    last_pos = i;
                                    in_block = true;
                                    copy_over = false;
                                } else {
                                    copy_over = true;
                                }
                            }

                            if (copy_over) {
                                resulting_vector.first.emplace_back(indices[i]);
                                resulting_vector.second.emplace_back(values[i]);
                            }
                        }

                        if (in_block) {
                            accumulated_value =
                                accumulated_value + algebra::multiexp<algebra::policies::multiexp_method_bos_coster>(
                                                        values.begin() + first_pos,
                                                        values.begin() + last_pos + 1,
                                                        std::begin(r) + (indices[first_pos] - offset),
                                                        std::begin(r) + (indices[last_pos] - offset) + 1,
                                                        chunks);
                        }

                        return std::make_pair(accumulated_value, resulting_vector);
                    }

                    bool in_block;

                    value_type accumulated_value;

                    indicies_type indices;
                    std::vector<value_type> values;
                    std::size_t domain_size_;
                };
            }    // namespace detail

            namespace tag {
                template<typename T>
                struct sparse : boost::accumulators::depends_on<> {
                    typedef T value_type;

                    /// INTERNAL ONLY
                    ///

                    typedef boost::mpl::always<accumulators::detail::sparse_impl<value_type>> impl;
                };
            }    // namespace tag

            namespace extract {
                template<typename Mode, typename AccumulatorSet>
                typename boost::mpl::apply<AccumulatorSet, tag::sparse<Mode>>::type::result_type
                    sparse(const AccumulatorSet &acc) {
                    return boost::accumulators::extract_result<tag::sparse<Mode>>(acc);
                }
            }    // namespace extract
        }        // namespace accumulators
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ACCUMULATORS_SNARK_HPP


/**
 * Copyright (c) 2018 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sstream>
#include <string>
#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/raw_function.hpp>
#include <boost/python/slice.hpp>
#include <boost/python/call.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <stdint.h>

#include "dbglog/dbglog.hpp"

#include "pysupport/package.hpp"
#include "pysupport/converters.hpp"

#include "../geometry_core.hpp"
#include "../math.hpp"
#include "../geometry.hpp"
#include "../transform.hpp"

namespace bp = boost::python;

namespace math { namespace py {

// point2

template <typename T>
bp::object repr_from_ostream(const T &t)
{
    std::ostringstream os;
    os << std::fixed << t;
    return bp::str(os.str());
}

template <typename T, typename Point>
T Point_getItem(const Point &p, int index)
{
    return p[index];
}

template <typename T, typename Point>
void Point_setItem(Point &p, int index, T value)
{
    p[index] = value;
}

template <typename T, typename Points, typename Extents>
Extents computeExtents(const Points &points)
{
    return math::computeExtents(points.begin(), points.end());
}

/** Constructor for creating Points* from any python sequence
 */
template <typename Point>
void Points_create(const boost::python::object &self
                   , const bp::object &iterable)
{
    // Create a constructor object.
    const auto constructor
        (boost::python::make_constructor
         (+[](const bp::object &iterable) {
             return std::make_shared<std::vector<Point>>
                 (bp::stl_input_iterator<Point>(iterable)
                  , bp::stl_input_iterator<Point>());
         }));

    // Invoke the constructor.
    constructor(self, iterable);
}

template <typename T, typename Point, typename Extents>
bp::class_<Point> point(const char *name, const char *listName)
{
    using namespace bp;

    typedef std::vector<Point> Points;

    auto cls = class_<Point>
        (name, init<const Point&>())
        .def(init<>())

        .def("__repr__", &py::repr_from_ostream<Point>)
        .def("__getitem__", &Point_getItem<T, Point>)
        .def("__setitem__", &Point_setItem<T, Point>)
        ;

        // wrap vector of point2
    class_<Points>(listName)
        .def(init<const Points&>())
        .def(vector_indexing_suite<Points>())
        .def("__init__", make_function(&py::Points_create<Point>))
        ;

    def("computeExtents", &computeExtents<T, Points, Extents>);

    return cls;
}

template <typename T>
bp::class_<Point2_<T>> point2(const char *name, const char *listName)
{
    using namespace bp;
    auto cls(point<T, math::Point2_<T>, math::Extents2_<T>>(name, listName));
    cls.def(init<T, T>());
    return cls;
}

// point3

template <typename T>
bp::class_<Point3_<T>> point3(const char *name, const char *listName)
{
    using namespace bp;
    auto cls(point<T, math::Point3_<T>, math::Extents3_<T>>(name, listName));
    cls.def(init<T, T, T>());
    return cls;
}

// size2

template <typename T>
bp::class_<Size2_<T>> size2(const char *name)
{
    using namespace bp;

    typedef math::Size2_<T> Size;

    auto cls = class_<Size>
        (name, init<const Size&>())
        .def(init<>())
        .def(init<T, T>())

        .def("__repr__", &py::repr_from_ostream<Size>)
        .def_readwrite("width", &Size::width)
        .def_readwrite("height", &Size::height)
        .template def<bool (*)(const Size&)
                      >("empty", &math::empty<T>)
        ;
    return cls;
}

// size3

template <typename T>
bp::class_<Size3_<T>> size3(const char *name)
{
    using namespace bp;

    typedef math::Size3_<T> Size;

    auto cls = class_<Size>
        (name, init<const Size&>())
        .def(init<>())
        .def(init<T, T, T>())

        .def("__repr__", &py::repr_from_ostream<Size>)
        .def_readwrite("width", &Size::width)
        .def_readwrite("height", &Size::height)
        .def_readwrite("depth", &Size::depth)
        ;
    return cls;
}

// extents2

template <typename Extents>
std::shared_ptr<Extents> invalidExtents(void*)
{
    return std::make_shared<Extents>(math::InvalidExtents{});
}

template <typename Extents>
auto Extents_center(const Extents &extents)
    -> decltype(math::center(extents))
{
    return math::center(extents);
}

template <typename Extents>
auto Extents_size(const Extents &extents)
    -> decltype(math::size(extents))
{
    return math::size(extents);
}

template <typename T>
bp::class_<Extents2_<T>> extents2(const char *name)
{
    using namespace bp;

    typedef math::Extents2_<T> Extents;
    typedef math::Point2_<T> Point;

    auto cls = class_<Extents>
        (name, init<const Extents&>())
        .def(init<>())
        .def(init<const Point&>())
        .def(init<const Point&, const Point&>())
        .def(init<T, T, T, T>())
        .def("__init__", make_constructor(&py::invalidExtents<Extents>))

        .def("__repr__", &py::repr_from_ostream<Extents>)
        .def_readwrite("ll", &Extents::ll)
        .def_readwrite("ur", &Extents::ur)
        .template def<math::Size2_<T> (*)(const Extents&)
                      >("size", &math::size<T>)
        .template def<bool (*)(const Extents&)
                      >("empty", &math::empty<T>)
        .def("center", &py::Extents_center<Extents>)
        .def("size", &py::Extents_size<Extents>)
        ;
    return cls;
}

// extents3

template <typename T>
bp::class_<Extents3_<T>> extents3(const char *name)
{
    using namespace bp;

    typedef math::Extents3_<T> Extents;
    typedef math::Point3_<T> Point;

    auto cls = class_<Extents>
        (name, init<const Extents&>())
        .def(init<>())
        .def(init<const Point&>())
        .def(init<const Point&, const Point&>())
        .def(init<T, T, T, T, T, T>())
        .def("__init__", make_constructor(&py::invalidExtents<Extents>))

        .def("__repr__", &py::repr_from_ostream<Extents>)
        .def_readwrite("ll", &Extents::ll)
        .def_readwrite("ur", &Extents::ur)
        .template def<math::Size3_<T> (*)(const Extents&)
                      >("size", &math::size<T>)
        .template def<bool (*)(const math::Extents2_<T>&)
                      >("empty", &math::empty<T>)

        .def("center", &py::Extents_center<Extents>)
        .def("size", &py::Extents_size<Extents>)
        ;
    return cls;
}

template <typename Matrix, int size>
std::shared_ptr<Matrix> Matrix_create()
{
    return std::make_shared<Matrix>
        (boost::numeric::ublas::zero_matrix<double>(size));
}

template <typename Matrix, int size>
Matrix Matrix_eye()
{
    return Matrix(boost::numeric::ublas::identity_matrix<double>(size));
}

template <typename Matrix>
double Matrix_get(const Matrix &m, int j, int i)
{
    return m(j, i);
}

template <typename Matrix>
double Matrix_set(Matrix &m, int j, int i, double value)
{
    return m(j, i) = value;
}

template <typename Matrix>
Matrix Matrix_mul(const Matrix &m1, const Matrix &m2)
{
    return ublas::prod(m1, m2);
}

template <typename Matrix>
void Matrix_imul(Matrix &m1, const Matrix &m2)
{
    m1 = ublas::prod(m1, m2);
}

template <typename Matrix>
Matrix Matrix_invert(const Matrix &m)
{
    return math::matrixInvert(m);
}

template <typename Matrix, int size>
bp::class_<Matrix> matrix(const char *name)
{
    using namespace bp;

    auto cls = class_<Matrix>
        (name, init<const Matrix&>())
        .def("__init__", make_constructor(&py::Matrix_create<Matrix, size>))
        .def("eye", &py::Matrix_eye<Matrix, size>)
        .staticmethod("eye")
        .def("__repr__", &py::repr_from_ostream<Matrix>)
        .def("__call__", &py::Matrix_get<Matrix>)
        .def("__call__", &py::Matrix_set<Matrix>)
        .def("__mul__",  &py::Matrix_mul<Matrix>)
        .def("__imul__",  &py::Matrix_imul<Matrix>)
        .def("invert",  &py::Matrix_invert<Matrix>)
        ;

    return cls;
}

} } // namespace math::py

BOOST_PYTHON_MODULE(melown_math)
{
    using namespace bp;
    namespace py = math::py;

    // geometry core: 2D stuff
    py::point2<int>("Point2i", "Points2i");
    py::point2<float>("Point2f", "Points2f");
    py::point2<double>("Point2", "Points2");

    py::size2<int>("Size2");
    py::size2<double>("Size2f");

    py::extents2<int>("Extents2i");
    py::extents2<double>("Extents2");

    // geometry core: 3D stuff
    py::point3<int>("Point3i", "Points3i");
    py::point3<float>("Point3f", "Points3f");
    py::point3<double>("Point3", "Points3");

    py::size3<int>("Size3");
    py::size3<double>("Size3f");

    py::extents3<int>("Extents3i");
    py::extents3<double>("Extents3");

    // matrices (only doubles)
    py::matrix<math::Matrix2, 2>("Matrix2");
    py::matrix<math::Matrix3, 3>("Matrix3");
    py::matrix<math::Matrix4, 4>("Matrix4");

    // transform.hpp
    def<math::Point3 (*)(const math::Matrix4&, const math::Point3&)>
        ("transform", &math::transform);
    def<math::Point2 (*)(const math::Matrix4&, const math::Point2&)>
        ("transform", &math::transform);
    def<math::Extents2 (*)(const math::Matrix4&, const math::Extents2&)>
        ("transform", &math::transform);
    def<void (*)(const math::Matrix4&, math::Points3&)>
        ("transform", &math::transform);
    def<void (*)(const math::Matrix4&, math::Points2&)>
        ("transform", &math::transform);

    PYSUPPORT_OPTIONAL(math::Size2_<int>);
    PYSUPPORT_OPTIONAL(math::Size2_<double>);
}

namespace math { namespace py {
PYSUPPORT_MODULE_IMPORT(math)
} } // namespace math::py


#pragma once

#include <Eigen/Dense>
#include <memory>
#include <variant>
#include <vector>

namespace kdtreepp {

// The EigenKdTreeNode represents one element of the k-d tree, which may be
// either a leaf (containing an iterable sequence), or a branch (containing a
// left and right child). All nodes have a bounding box.
//
// The node is constructed with an iterable range and two function objects.
// The first transforms the dereferenced iterator into a sortable point (i.e.
// the element centroid), and the second returns a point or aligned box that is
// used to extend a bounding box.
template <typename Iterator, typename T, int N>
class alignas(sizeof(T) * 4 * (1 + ((N - 1) / 4))) EigenKdTreeNode {
public:
  // Construct a EigenKdTreeNode from the iterable range.
  // The node will compute a surrounding bounds for the range of objects,
  // based on a bounding element returned from the "boundsGetter" functor. This
  // element can be either a point or a box. If the node has too many elements
  // (based on maxPerLeaf), and can be subdivided (based on maxSubDivs), it will
  // median-sort, in-place, the iterator range around a midpoint chosen by
  // comparing the "sort point" of the elements along the major axis dimension.
  //
  // This function *will* allocate memory for the child nodes, but does
  // not require any allocation for the contained objects.
  template <typename SortPointGetter, typename BoundsGetter>
  explicit EigenKdTreeNode(Iterator itemBegin, Iterator itemEnd, SortPointGetter&& sortPointGetter,
                           BoundsGetter&& boundsGetter, const int maxPerLeaf = 8,
                           const int maxSubDivs = 16) {
    const auto count = std::distance(itemBegin, itemEnd);

    // Compute the bounds
    _bounds.setEmpty();
    for (auto iter = itemBegin; iter != itemEnd; ++iter) {
      _bounds.extend(boundsGetter(*iter));
    }

    if (maxSubDivs > 0 && count > static_cast<decltype(count)>(maxPerLeaf)) {
      // Subdivide
      int majorAxis = 0;
      _bounds.sizes().maxCoeff(&majorAxis);
      Iterator itemMid = itemBegin;
      std::advance(itemMid, count / 2);

      // Median-sort items according to major axis of bounding box
      std::nth_element(itemBegin, itemMid, itemEnd,
                       [majorAxis, &sortPointGetter](auto& a, auto& b) -> bool {
                         return sortPointGetter(a)[majorAxis] < sortPointGetter(b)[majorAxis];
                       });

      // Create a branch
      _body = Branch{std::make_unique<this_type>(
                         itemBegin, itemMid, std::forward<SortPointGetter>(sortPointGetter),
                         std::forward<BoundsGetter>(boundsGetter), maxPerLeaf, maxSubDivs - 1),
                     std::make_unique<this_type>(
                         itemMid, itemEnd, std::forward<SortPointGetter>(sortPointGetter),
                         std::forward<BoundsGetter>(boundsGetter), maxPerLeaf, maxSubDivs - 1)};
    } else {
      _body = Leaf{itemBegin, itemEnd};
    }
  }

  [[nodiscard]] bool isLeaf() const { return _body.index() == 0; }

  [[nodiscard]] bool isBranch() const { return _body.index() == 1; }

  [[nodiscard]] const auto& bounds() const { return _bounds; }

  // Recursively visit every item in this node and its children,
  // provided the bounding box of each node passes the "BoundsTest" function
  // passed in here - BoundsTest should take a const bounds as an argument and
  // return a bool, based on whether we should consider the node.
  //
  // Once leaf nodes are reached, the iteration range with the leaf will
  // be iterated over, and every item therein will be passed to visitor
  template <typename BoundsTest, typename Visitor>
  void visit(BoundsTest&& boundsTest, Visitor&& visitor) {
    if (!boundsTest(_bounds)) {
      return;
    }

    if (isLeaf()) {
      auto& leaf = std::get<0>(_body);
      for (auto iter = leaf.begin; iter != leaf.end; ++iter) {
        visitor(*iter);
      }
    } else if (isBranch()) {
      auto& branch = std::get<1>(_body);
      branch.left->visit(std::forward<BoundsTest>(boundsTest), std::forward<Visitor>(visitor));
      branch.right->visit(std::forward<BoundsTest>(boundsTest), std::forward<Visitor>(visitor));
    }
  }

  // Recursively visit every item in this node and its children,
  // provided the bounding box of each node passes the "BoundsTest" function
  // passed in here - BoundsTest should take a const bounds as an argument and
  // return a bool, based on whether we should consider the node.
  //
  // Once leaf nodes are reached, the iteration range with the leaf will
  // be iterated over, and every item therein will be passed to visitor
  //
  // (const version)
  template <typename BoundsTest, typename Visitor>
  void visit(BoundsTest&& boundsTest, Visitor&& visitor) const {
    if (!boundsTest(_bounds)) {
      return;
    }

    if (isLeaf()) {
      const auto& leaf = std::get<0>(_body);
      for (auto iter = leaf.begin; iter != leaf.end; ++iter) {
        visitor(*iter);
      }
    } else if (isBranch()) {
      const auto& branch = std::get<1>(_body);
      branch.left->visit(std::forward<BoundsTest>(boundsTest), std::forward<Visitor>(visitor));
      branch.right->visit(std::forward<BoundsTest>(boundsTest), std::forward<Visitor>(visitor));
    }
  }

private:
  using this_type = EigenKdTreeNode<Iterator, T, N>;
  Eigen::AlignedBox<T, N> _bounds;

  struct Leaf {
    Iterator begin;
    Iterator end;
  };

  struct Branch {
    std::unique_ptr<this_type> left;
    std::unique_ptr<this_type> right;
  };

  std::variant<Leaf, Branch> _body;
};

template <typename T, int N, typename Iterator, typename SortPointGetter, typename BoundsGetter>
EigenKdTreeNode<Iterator, T, N> MakeEigenKdTreeNode(Iterator itemBegin, Iterator itemEnd,
                                                    SortPointGetter&& sortPointGetter,
                                                    BoundsGetter&& boundsGetter,
                                                    const int maxPerLeaf = 8,
                                                    const int maxSubDivs = 16) {
  return EigenKdTreeNode<Iterator, T, N>{itemBegin,
                                         itemEnd,
                                         std::forward<SortPointGetter>(sortPointGetter),
                                         std::forward<BoundsGetter>(boundsGetter),
                                         maxPerLeaf,
                                         maxSubDivs};
}

}  // namespace kdtreepp


// MIT License

// Copyright (c) 2021 Daniel Kowalczyk

// 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.

#include "PricerSolverSimpleDP.hpp"
#include <fmt/core.h>
#include <gurobi_c++.h>                            // for GRBLinExpr, GRBModel
#include <algorithm>                               // for fill, find, max
#include <boost/graph/adjacency_list.hpp>          // for adjacency_list
#include <boost/graph/detail/adjacency_list.hpp>   // for edges, get, num_edges
#include <boost/graph/detail/edge.hpp>             // for operator!=, operat...
#include <boost/graph/graph_selectors.hpp>         // for bidirectionalS
#include <boost/graph/graphviz.hpp>                // for write_graphviz
#include <boost/iterator/iterator_facade.hpp>      // for operator!=, operat...
#include <boost/multiprecision/cpp_int.hpp>        // for cpp_int
#include <boost/pending/property.hpp>              // for no_property
#include <cstddef>                                 // for size_t
#include <fstream>                                 // for operator<<, basic_...
#include <iostream>                                // for cerr
#include <limits>                                  // for numeric_limits
#include <list>                                    // for operator==
#include <range/v3/iterator/basic_iterator.hpp>    // for basic_iterator
#include <range/v3/iterator/reverse_iterator.hpp>  // for reverse_cursor
#include <range/v3/view/iota.hpp>                  // for iota_view, ints
#include <range/v3/view/reverse.hpp>               // for reverse_fn, revers...
#include <range/v3/view/view.hpp>                  // for operator|, view_cl...
#include <span>                                    // for span
#include <string>                                  // for char_traits, opera...
#include <utility>                                 // for move
#include <vector>                                  // for vector, vector<>::...
#include "Column.h"                                // for ScheduleSet
#include "Instance.h"                              // for Instance
#include "Job.h"                                   // for Job
#include "ModelInterface.hpp"                      // for ReformulationModel
#include "PricerSolverBase.hpp"                    // for PricerSolverBase
/**
 * Pricersolver for the TI index formulation
 */
PricerSolverSimpleDp::PricerSolverSimpleDp(const Instance& instance)
    : PricerSolverBase(instance),
      Hmax(instance.H_max),
      size_graph{},
      A(Hmax + 1),
      F(Hmax + 1),
      backward_F(Hmax + 1),
      TI_x(convex_constr_id * (Hmax + 1), GRBVar()),
      take(convex_constr_id * (Hmax + 1), 0),
      lp_x(convex_constr_id * (Hmax + 1), 0.0),
      solution_x(convex_constr_id * (Hmax + 1), 0.0) {
    init_table();
}

void PricerSolverSimpleDp::init_table() {
    backward_graph = std::vector<std::vector<Job*>>(Hmax + 1);
    forward_graph = std::vector<std::vector<Job*>>(Hmax + 1);

    for (auto t = 0UL; t < Hmax + 1; t++) {
        for (auto i = 1UL; i < convex_constr_id + 1; i++) {
            auto  j = i - 1;
            auto* job = jobs[j].get();

            if (t >= static_cast<size_t>(job->processing_time)) {
                forward_graph[t].push_back(job);
                size_graph++;
            }

            if (t + job->processing_time <= Hmax) {
                backward_graph[t].push_back(job);
            }
        }
    }

    fmt::print("Number of arcs in TI formulation = {}\n", size_graph);
}

bool PricerSolverSimpleDp::evaluate_nodes(
    [[maybe_unused]] std::span<const double>& pi) {
    forward_evaluator(pi);
    backward_evaluator(pi);

    auto counter = size_t{};
    auto x = size_t{};
    auto num_removed = 0;

    for (auto t = size_t{}; t < Hmax + 1; ++t) {
        auto it = forward_graph[t].begin();
        while (it != forward_graph[t].end()) {
            double result = F[t - (*it)->processing_time] +
                            (*it)->weighted_tardiness(t) - pi[(*it)->job] +
                            backward_F[t];

            if (constLB + result +
                    static_cast<double>(convex_rhs - 1) * F[Hmax] >
                UB - 1.0 + RC_FIXING) {
                --size_graph;
                it = forward_graph[t].erase(it);
                ++num_removed;
            } else {
                it++;
            }
        }

        x += forward_graph[t].size();

        auto iter = backward_graph[t].begin();
        while (iter != backward_graph[t].end()) {
            double result = F[t] + (*iter)->weighted_tardiness_start(t) -
                            pi[(*iter)->job] +
                            backward_F[t + (*iter)->processing_time];
            if (constLB + result +
                    static_cast<double>(convex_rhs - 1) * F[Hmax] >
                UB - 1.0 + RC_FIXING) {
                take[(*iter)->job * (Hmax + 1) + t] = false;
                iter = backward_graph[t].erase(iter);
            } else {
                take[(*iter)->job * (Hmax + 1) + t] = true;
                iter++;
            }
        }

        counter += backward_graph[t].size();
    }

    return (num_removed > 0);
}

bool PricerSolverSimpleDp::evaluate_nodes([[maybe_unused]] double* pi) {
    forward_evaluator(pi);
    backward_evaluator(pi);

    std::span aux_pi{pi, reformulation_model.size()};
    auto      counter = size_t{};
    auto      x = size_t{};
    auto      num_removed = 0;

    for (auto t = size_t{}; t < Hmax + 1; ++t) {
        auto it = forward_graph[t].begin();
        while (it != forward_graph[t].end()) {
            double result = F[t - (*it)->processing_time] +
                            (*it)->weighted_tardiness(t) - aux_pi[(*it)->job] +
                            backward_F[t];

            if (constLB + result +
                    static_cast<double>(convex_rhs - 1) * F[Hmax] >
                UB - 1.0 + RC_FIXING) {
                --size_graph;
                it = forward_graph[t].erase(it);
                ++num_removed;
            } else {
                it++;
            }
        }

        x += forward_graph[t].size();

        auto iter = backward_graph[t].begin();
        while (iter != backward_graph[t].end()) {
            double result = F[t] + (*iter)->weighted_tardiness_start(t) -
                            aux_pi[(*iter)->job] +
                            backward_F[t + (*iter)->processing_time];
            if (constLB + result +
                    static_cast<double>(convex_rhs - 1) * F[Hmax] >
                UB - 1.0 + RC_FIXING) {
                take[(*iter)->job * (Hmax + 1) + t] = false;
                iter = backward_graph[t].erase(iter);
            } else {
                take[(*iter)->job * (Hmax + 1) + t] = true;
                iter++;
            }
        }

        counter += backward_graph[t].size();
    }

    return (num_removed > 0);
}

void PricerSolverSimpleDp::build_mip() {
    try {
        fmt::print("Building Mip model for the TI formulation\n");

        /** Constructing variables */
        for (auto t = size_t{}; t < Hmax + 1; t++) {
            for (auto& it : backward_graph[t]) {
                double cost = it->weighted_tardiness_start(t);
                double ub = take[(it->job) * (Hmax + 1) + t] ? 1.0 : 0.0;
                TI_x[it->job * (Hmax + 1) + t] =
                    model.addVar(0.0, ub, cost, 'B');
            }
        }

        model.update();

        /** Assignment variables */
        std::vector<GRBLinExpr> assignment(convex_constr_id, GRBLinExpr());
        std::vector<char>       sense(convex_constr_id, '=');
        std::vector<double>     rhs(convex_constr_id, 1.0);

        for (auto t = 0UL; t <= Hmax; t++) {
            for (auto& it : backward_graph[t]) {
                assignment[it->job] += TI_x[it->job * (Hmax + 1) + t];
            }
        }

        std::unique_ptr<GRBConstr> assignment_constrs(
            model.addConstrs(assignment.data(), sense.data(), rhs.data(),
                             nullptr, static_cast<int>(convex_constr_id)));

        model.update();

        std::vector<GRBLinExpr> interval_constr(Hmax + 1, GRBLinExpr());
        std::vector<char>       interval_sense(Hmax + 1);
        std::vector<double>     interval_rhs(Hmax + 1);

        for (auto t = 0UL; t <= Hmax; t++) {
            auto add_constraint = false;
            for (auto& it : backward_graph[t]) {
                for (auto s = std::max(0UL, t - it->processing_time); s <= t;
                     s++) {
                    if (std::find(backward_graph[s].begin(),
                                  backward_graph[s].end(),
                                  it) != backward_graph[s].end()) {
                        interval_constr[t] += TI_x[(it->job) * (Hmax + 1) + s];
                        add_constraint = true;
                    }
                }
            }

            if (add_constraint) {
                interval_sense[t] = '<';
                interval_rhs[t] = static_cast<double>(convex_rhs);

                model.addConstr(interval_constr[t], interval_sense[t],
                                interval_rhs[t]);
            }
        }

        model.update();

    } catch (GRBException& e) {
        std::cerr << e.getMessage() << '\n';
    }

    for (auto t = 0UL; t <= Hmax; t++) {
        for (auto& it : backward_graph[t]) {
            TI_x[it->job * (Hmax + 1) + t].set(
                GRB_DoubleAttr_Start, solution_x[(it->job) * (Hmax + 1) + t]);
        }
    }

    model.write("ti_" + problem_name + "_" + std::to_string(convex_rhs) +
                "correct.lp");
    model.optimize();
}

void PricerSolverSimpleDp::forward_evaluator(std::span<const double>& _pi) {
    /** Initialisation */
    F[0] = 0.0;
    A[0] = nullptr;

    for (auto t = 1UL; t < Hmax + 1; t++) {
        F[t] = std::numeric_limits<double>::max() / 2;
        A[t] = nullptr;
    }

    /** Recursion */
    for (auto t = size_t{1}; t < Hmax + 1; t++) {
        for (auto& it : forward_graph[t]) {
            if (F[t - it->processing_time] +
                    static_cast<double>(it->weighted_tardiness(t)) -
                    _pi[it->job] <=
                F[t]) {
                F[t] = F[t - it->processing_time] + it->weighted_tardiness(t) -
                       _pi[it->job];
                A[t] = it;
            }
        }

        if (F[t - 1] <= F[t]) {
            F[t] = F[t - 1];
        }
    }
}

void PricerSolverSimpleDp::forward_evaluator(double* _pi) {
    /** Initialisation */
    std::span aux_pi{_pi, reformulation_model.size()};
    F[0] = 0.0;
    A[0] = nullptr;

    for (auto t = 1UL; t < Hmax + 1; t++) {
        F[t] = std::numeric_limits<double>::max() / 2;
        A[t] = nullptr;
    }

    /** Recursion */
    for (auto t = size_t{1}; t < Hmax + 1; t++) {
        for (auto& it : forward_graph[t]) {
            if (F[t - it->processing_time] +
                    static_cast<double>(it->weighted_tardiness(t)) -
                    aux_pi[it->job] <=
                F[t]) {
                F[t] = F[t - it->processing_time] + it->weighted_tardiness(t) -
                       aux_pi[it->job];
                A[t] = it;
            }
        }

        if (F[t - 1] <= F[t]) {
            F[t] = F[t - 1];
        }
    }
}

void PricerSolverSimpleDp::backward_evaluator(std::span<const double>& _pi) {
    backward_F[Hmax] = 0.0;

    for (auto t = 0UL; t < Hmax; t++) {
        backward_F[t] = std::numeric_limits<double>::max() / 2;
    }

    for (auto t :
         ranges::views::ints(size_t{}, Hmax) | ranges::views::reverse) {
        for (auto& it : backward_graph[t]) {
            auto tt = t + it->processing_time;
            if (backward_F[tt] +
                    static_cast<double>(it->weighted_tardiness(tt)) -
                    _pi[it->job] <=
                backward_F[t]) {
                backward_F[t] =
                    backward_F[tt] +
                    static_cast<double>(it->weighted_tardiness(tt)) -
                    _pi[it->job];
            }
        }

        if (backward_F[t + 1] <= backward_F[t]) {
            backward_F[t] = backward_F[t + 1];
        }
    }
}

void PricerSolverSimpleDp::backward_evaluator(double* _pi) {
    backward_F[Hmax] = 0.0;
    std::span aux_pi{_pi, reformulation_model.size()};

    for (auto t = 0UL; t < Hmax; t++) {
        backward_F[t] = std::numeric_limits<double>::max() / 2;
    }

    for (auto t :
         ranges::views::ints(size_t{}, Hmax) | ranges::views::reverse) {
        for (auto& it : backward_graph[t]) {
            auto tt = t + it->processing_time;
            if (backward_F[tt] +
                    static_cast<double>(it->weighted_tardiness(tt)) -
                    aux_pi[it->job] <=
                backward_F[t]) {
                backward_F[t] =
                    backward_F[tt] +
                    static_cast<double>(it->weighted_tardiness(tt)) -
                    aux_pi[it->job];
            }
        }

        if (backward_F[t + 1] <= backward_F[t]) {
            backward_F[t] = backward_F[t + 1];
        }
    }
}

PricingSolution PricerSolverSimpleDp::pricing_algorithm(
    std::span<const double>& _pi) {
    PricingSolution opt_sol;
    opt_sol.cost = 0;
    std::vector<Job*> v;

    forward_evaluator(_pi);

    /** Find optimal solution */
    opt_sol.obj = std::numeric_limits<double>::max();

    for (auto i = 0UL; i < Hmax + 1; i++) {
        if (F[i] < opt_sol.obj) {
            opt_sol.C_max = i;
            opt_sol.obj = F[i];
        }
    }

    auto t_min = opt_sol.C_max;

    /** Construct the solution */
    while (A[t_min] != nullptr) {
        Job* job = A[t_min];
        v.push_back(A[t_min]);
        opt_sol.cost += A[t_min]->weighted_tardiness(t_min);
        t_min -= job->processing_time;
    }

    auto it = v.rbegin();

    for (; it != v.rend(); ++it) {
        opt_sol.jobs.push_back(*it);
    }

    /** Free the memory */
    return opt_sol;
}

PricingSolution PricerSolverSimpleDp::pricing_algorithm(double* _pi) {
    PricingSolution opt_sol;
    opt_sol.cost = 0;
    std::vector<Job*> v;

    forward_evaluator(_pi);

    /** Find optimal solution */
    opt_sol.obj = std::numeric_limits<double>::max();

    for (auto i = 0UL; i < Hmax + 1; i++) {
        if (F[i] < opt_sol.obj) {
            opt_sol.C_max = i;
            opt_sol.obj = F[i];
        }
    }

    auto t_min = opt_sol.C_max;

    /** Construct the solution */
    while (A[t_min] != nullptr) {
        Job* job = A[t_min];
        v.push_back(A[t_min]);
        opt_sol.cost += A[t_min]->weighted_tardiness(t_min);
        t_min -= job->processing_time;
    }

    auto it = v.rbegin();

    for (; it != v.rend(); ++it) {
        opt_sol.jobs.push_back(*it);
    }

    /** Free the memory */
    return opt_sol;
}

PricingSolution PricerSolverSimpleDp::farkas_pricing(
    [[maybe_unused]] double* _pi) {
    PricingSolution opt_sol;

    return opt_sol;
}

PricingSolution PricerSolverSimpleDp::farkas_pricing(
    [[maybe_unused]] std::span<const double>& _pi) {
    PricingSolution opt_sol;

    return opt_sol;
}

void PricerSolverSimpleDp::construct_lp_sol_from_rmp(
    const double*                               lambda,
    const std::vector<std::shared_ptr<Column>>& columns) {
    std::span aux_cols{lambda, columns.size()};
    // std::span aux_columns{columns->pdata, columns->len};
    std::fill(lp_x.begin(), lp_x.end(), 0.0);
    for (auto k = 0UL; k < columns.size(); k++) {
        if (aux_cols[k] > EPS_SOLVER) {
            auto* tmp = columns[k].get();
            int   t = 0;
            // std::span aux_jobs{tmp->job_list->pdata, tmp->job_list->len};
            for (auto& it : tmp->job_list) {
                lp_x[(it->job) * (Hmax + 1) + t] += aux_cols[k];
                t += it->processing_time;
            }
        }
    }
}
void PricerSolverSimpleDp::construct_lp_sol_from_rmp(
    const std::span<const double>&              lambda,
    const std::vector<std::shared_ptr<Column>>& columns) {
    // std::span aux_columns{columns->pdata, columns->len};
    std::fill(lp_x.begin(), lp_x.end(), 0.0);
    for (auto k = 0UL; k < columns.size(); k++) {
        if (lambda[k] > EPS_SOLVER) {
            auto* tmp = columns[k].get();
            int   t = 0;
            // std::span aux_jobs{tmp->job_list->pdata, tmp->job_list->len};
            for (auto& it : tmp->job_list) {
                lp_x[(it->job) * (Hmax + 1) + t] += lambda[k];
                t += it->processing_time;
            }
        }
    }
}

// void PricerSolverSimpleDp::create_dot_zdd([[maybe_unused]] const char* name)
// {
//     boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS>
//         graph;
//     for (auto t = 0UL; t <= Hmax; t++) {
//         boost::add_vertex(graph);
//     }

//     for (auto t = 0UL; t < Hmax; t++) {
//         for (auto& it : backward_graph[t]) {
//             boost::add_edge(t, t + it->processing_time, graph);
//         }
//     }
//     auto file_name = "TI_representation_" + problem_name + "_" +
//                      std::to_string(convex_rhs) + ".gv";
//     auto otf = std::ofstream(file_name);
//     boost::write_graphviz(otf, graph);
//     otf.close();
// }

size_t PricerSolverSimpleDp::get_nb_edges() {
    size_t nb_edges = 0u;
    for (auto& it : forward_graph) {
        nb_edges += it.size();
    }
    return nb_edges;
}

size_t PricerSolverSimpleDp::get_nb_vertices() {
    size_t nb_vertices = 0u;
    for (auto& it : forward_graph) {
        if (!it.empty()) {
            nb_vertices++;
        }
    }
    return nb_vertices;
}

boost::multiprecision::cpp_int PricerSolverSimpleDp::print_num_paths() {
    return 0;
}

bool PricerSolverSimpleDp::check_column([[maybe_unused]] Column const* set) {
    // int t = 0;
    // for(unsigned int j = 0; j < set->len; j++) {
    //     Job* tmp_j = static_cast<Job*>() g_ptr_array_index()atic_cast<set,
    //     j>())); t += tmp_j->processing_time;

    //     if (t > Hmax) {
    //         return false;
    //     }

    //     auto it = std::find(forward_graph[t].begin(),
    //     forward_graph[t].end(),tmp_j); if (it == forward_graph[t].end()) {
    //         return false;
    //     }

    // }

    return true;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef MULTIVARIATIONALUTIL_HH
#define MULTIVARIATIONALUTIL_HH

#include <algorithm>
#include <array>
#include <cassert>
#include <iostream>

#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_const.hpp>

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/sequence.hpp>

#include "fem/fixdune.hh"
#include "fem/functionspace.hh"
#include "fem/linearspace.hh"
#include "linalg/crsutil.hh"

/**
 * @file
 * @brief  Variables and their descriptions
 * @author Martin Weiser
 *
 * This file contains a number of core classes of Kaskade, needed to define a problem. The important ones are:
 *
 * - VariableDescription: elementary information about a single variable
 * - VariableSetDescription: information about a set of variables and associated FEFunctionSpace s
 * - VariableSet: a set of variables with all the data stored in FunctionSpaceElement s
 *
 * The other classes are auxilliary
 */

//---------------------------------------------------------------------

namespace Kaskade
{
  /**
   * \brief A class storing elementary information about a single variable.
   * \ingroup variables
   * This includes the number of components, the id and name of the
   * variable, and the index of the associated FEFunctionSpace from
   * which the variable comes.
   *
   * \tparam spaceId the index of the FEFunctionSpace to which the variable belongs.
   *   This references an external container of FE spaces.
   * \tparam components the number of components that this variable has
   *   (1 indicates a scalar variable, values greater than 1 a vectorial
   *   variable). Has to be greater than 0.
   * \tparam Id the index of the variable by which it can be retrieved from a
   *   boost::fusion container of variables.
   *
   * Note that the order of template parameters is important. A more convenient interface
   * is provided by \ref Variable, where the order of template parameters can be arbitrary.
   */
  template <int spaceId, int components, int Id>
  struct VariableDescription
  {
    /// number of this variable
    static int const id = Id;
    /// number of component of this variable
    static int const m = components;
    /// number of the space, this variable is associated with
    static int const spaceIndex = spaceId;
  };

  /**
   * \brief Helper class for specifying the FE space index of a variable.
   *
   * This is intended to be used as template argument for \ref Variable.
   */
  template <int spaceIndex>
  struct SpaceIndex : public VariableDescription<spaceIndex,-1,-1> {};

  /**
   * \brief Helper class for specifying the number of components of a variable.
   *
   * This is intended to be used as template argument for \ref Variable.
   */
  template <int components>
  struct Components : public VariableDescription<-1,components,-1> {};

  /**
   * \brief Helper class for specifying the id of a variable.
   *
   * This is intended to be used as template argument for \ref Variable.
   */
  template <int id>
  struct VariableId : public VariableDescription<-1,-1,id> {};

  /**
   * \brief A class storing elementary information about a single variable.
   * \ingroup variables
   *
   * This includes the number of components, the id and name of the
   * variable, and the index of the associated FEFunctionSpace from
   * which the variable comes.
   *
   * This class differs from \ref VariableDescription only in how it is created. The template
   * parameters should be \ref SpaceIndex, \ref Components, and \ref VariableId, in arbitrary order (but one of each).
   *
   * Example (e.g. for a Stokes setting):
   * \code
   * typedef boost::fusion::vector<Variable<SpaceIndex<0>,Components<3>,VariableId<0> >,
   *                               Variable<Components<1>,VariableId<1>,SpaceIndex<1> > > VariableDescriptions;
   * \endcode
   */
  template <typename A, typename B, typename C>
  struct Variable {
    static int const spaceIndex = A::spaceIndex>=0? A::spaceIndex : B::spaceIndex>=0? B::spaceIndex : C::spaceIndex;
    static int const m  = A::m>=0? A::m : B::m>=0? B::m : C::m;
    static int const id = A::id>=0? A::id : B::id>=0? B::id : C::id;

    static_assert(spaceIndex>=0,"Space index has to be nonnegative (space indices cover a contiguous range from 0 to k-1).");
    static_assert(m>=0,"Number of components has to be nonnegative (in fact, 0 makes rarely sense as well).");
    static_assert(id>=0,"Variable Id has to be nonnegative (ids have to cover a contiguous range from 0 to n-1).");
  };

  //---------------------------------------------------------------------

  /**
   * \brief A boost::fusion functor for generating function space elements for given variables.
   *
   * \tparam Spaces: a boost::fusion container of pointers to FEFunctionSpace s.
   */
  template <class Spaces>
  struct ConstructElement
  {
    ConstructElement(Spaces const& spaces_):
      spaces(spaces_)
    {}

    /**
     * \brief For a given variable, defines the correct FunctionSpaceElement type.
     *
     * This depends on the spaceIndex and the number of components defined by the Variable (which is a VariableDescription).
     */
    template <typename Arg> struct result {};

    template <class Variable>
    struct result<ConstructElement<Spaces>(Variable)>
    {
      typedef typename boost::remove_reference<Variable>::type Var;
      typedef typename SpaceType<Spaces,Var::spaceIndex>::type Space;
      typedef typename Space::template Element<Var::m>::type type;
    };

    /**
     * \brief Returns a FunctionSpaceElement element from the FEFunctionSpace associated to the Variable.
     */
    template <class Variable>
    typename result<ConstructElement<Spaces>(Variable)>::type operator()(Variable const& variable) const
    {
      return typename result<ConstructElement<Spaces>(Variable)>::type(*boost::fusion::at_c<Variable::spaceIndex>(spaces));
    }

  private:
    Spaces const& spaces;
  };

  /**
   * \brief A boost::fusion functor for generating coefficient vectors for given variables.
   *
   * \tparam Spaces: a boost::fusion container of pointers to FEFunctionSpace s.
   */
  template <class Spaces>
  struct ConstructCoefficientVector
  {
    ConstructCoefficientVector(Spaces const& spaces_):
      spaces(spaces_)
    {}

    /**
     * For a given variable (VariableDescription), defines the correct
     * coefficient vector type. This depends on the space index and the
     * number of components defined by the variable.
     */
    template <typename Arg> struct result {};

    template <class Variable>
    struct result<ConstructCoefficientVector<Spaces>(Variable)>
    {
      typedef typename boost::remove_reference<Variable>::type Var;
      typedef typename SpaceType<Spaces,Var::spaceIndex>::type Space;
      typedef typename Space::template Element<Var::m>::type::StorageType StorageType;

      typedef Dune::BlockVector<typename StorageType::block_type> type;
    };

    /**
     * \brief Returns a FunctionSpaceElement from the FEFunctionSpace associated to the variable.
     *
     * This is currently not very efficient. This may change when Dune starts supporting
     * move-semantics(C++11). Alternatively it may make sense to return only the dof instead
     * of the full matrix and construct the latter later on.
     */
    template <class Variable>
    typename result<ConstructCoefficientVector<Spaces>(Variable)>::type operator()(Variable const& variable) const
    {
      return typename result<ConstructCoefficientVector<Spaces>(Variable)>::type(boost::fusion::at_c<Variable::spaceIndex>(spaces)->degreesOfFreedom());
    }

  private:
    Spaces const& spaces;
  };

  //---------------------------------------------------------------------

  /**
   * \cond internals
   */
  namespace Variables_Detail
  {

    // A functor constructing representations of consecutive subsets of variables
    template <class Variables, class RepresentationConstructor,
              int first=0, int last=boost::fusion::result_of::size<Variables>::type::value>
    class VariableRangeCreator
    {
      typedef typename boost::fusion::result_of::begin<Variables>::type       Begin;
      typedef typename boost::fusion::result_of::advance_c<Begin,first>::type First;
      typedef typename boost::fusion::result_of::advance_c<Begin,last>::type  Last;

    public:
      typedef typename boost::fusion::transform_view<boost::fusion::iterator_range<First,Last> const,
                                                     RepresentationConstructor> View;
      typedef typename boost::fusion::result_of::as_vector<View>::type type;

      static View apply(RepresentationConstructor const& c)
      {
        using namespace boost::fusion;
        Variables vars;
        return transform(iterator_range<First,Last>(advance_c<first>(begin(vars)),advance_c<last>(begin(vars))),c);
      }
    };
  }
  /**
   * \endcond
   */

  //---------------------------------------------------------------------

  /**
   * \ingroup variables
   * \brief A class for storing a heterogeneous collection of FunctionSpaceElement s.
   * \tparam Descriptions a VariableSetDescription class
   *
   * Basic vector operations and I/O are supported.
   *
   * The type of a VariableSet is defined via a VariableSetDescription, which contains information
   * about the variables in form of a boost::fusion::vector of FEFunctionSpace s and VariableDescription s.
   * The type of a VariableSet can be determined by VariableSetDescription::VariableSet.
   *
   * VariableSet s contains a collection of FunctionSpaceElement s (vars) and a reference to its VariableSetDescription (descriptions).
   * Simultaneous evaluation of all functions is supported by \ref evaluateVariables.
   *
   * Usually, solutions are stored in VariableSets. Access to data is via the public member
   * variable data, which is a boost::fusion::vector of \ref FunctionSpaceElement s
   */
  template <class VSDescriptions>
  class VariableSet: public LinearProductSpace<typename VSDescriptions::Scalar, typename VSDescriptions::RepresentationData>
  {
  public:
    /// Type of the VariableSetDescription
    typedef VSDescriptions Descriptions;

    /// boost::fusion::vector of data elements (of type FunctionSpaceElement)
    typedef typename VSDescriptions::RepresentationData Functions;

  private:
    typedef VariableSet<Descriptions> Self;
    typedef typename SpaceType<typename Descriptions::Spaces,0>::type::Scalar Scalar;
    typedef LinearProductSpace<Scalar,Functions> Base;

    /// Grid type
    typedef typename Descriptions::Grid Grid;

  public:
    typedef LinearProductSpace<Scalar,Functions> LinearSpace;

    /// Copy constructor
    VariableSet(Self const& vs):
      Base(vs.data), descriptions(vs.descriptions)
    {}

    /**
     * \brief Constructor
     *
     * \param d the VariableSetDescription object (has to exist during the lifetime of this VariableSet).
     */
    explicit VariableSet(Descriptions const& d):
        Base(Variables_Detail::VariableRangeCreator<typename Descriptions::Variables,ConstructElement<typename Descriptions::Spaces> >::apply(
          ConstructElement<typename Descriptions::Spaces>(d.spaces))),
        descriptions(d)
    {}

    /// Assignment
    Self& operator=(Self const& v) {
      if (this!=&v) this->data = v.data;
      return *this;
    }

    /// Assignment forwarded to the linear space base class
    template <class S>
    Self& operator=(S const& s)
    {
      static_cast<Base&>(*this) = s;
      return *this;
    }

    /// Descriptions of variable set, of type VariableSetDescription (lots of useful infos)
    Descriptions const& descriptions;
  };

  //---------------------------------------------------------------------

  /**
   * \ingroup variables
   * \brief A class that stores information about a set of variables.
   *
   * This includes in particular references to a list of variable
   * descriptions, a list of FEFunctionSpace s, a Grid, and an IndexSet.
   *
   *
   * \tparam SpaceList a boost::fusion container of FEFunctionSpace s
   * \tparam VariableList a boost::fusion container of VariableDescription s. Note that
   *   the variable id's have to be consecutive starting at 0, and shall be ordered.
   */
  template <class SpaceList, class VariableList>
  class VariableSetDescription
  {
  public:
    /// type of boost::fusion::vector of FEFunctionSpace s needed
    typedef SpaceList Spaces;
    /// type of boost::fusion vector of VariableDescription s
    typedef VariableList Variables;

    /**
     * \brief Type that contains a set of variables inside a boost vector, together with all the data
     */
    typedef typename Variables_Detail::VariableRangeCreator<Variables,ConstructElement<Spaces>>::type RepresentationData;

    /**
     * \brief Type that contains a set of variable values with some
     * functionality, such as simple vector arithmetic and so on
     * (cf. class \ref VariableSet)
     *
     * Construct an object r of this type from a given VariableSetDescription vsd by:
     * \code
     * VariableSetDescription::VariableSet r(vsd);
     * \endcode
     */
    typedef Kaskade::VariableSet<VariableSetDescription<Spaces,Variables>> VariableSet;

    /// scalar field type
    typedef typename SpaceType<Spaces,0>::type::Scalar   Scalar;
    /// Grid type
    typedef typename SpaceType<Spaces,0>::type::Grid     Grid;
    /// Grid view type
    typedef typename SpaceType<Spaces,0>::type::GridView GridView;
    /// IndexSet type
    typedef typename SpaceType<Spaces,0>::type::IndexSet IndexSet;

    /// Number of variables in this set
    static int const noOfVariables = boost::fusion::result_of::size<Variables>::type::value;

    /**
     * \brief The boost::fusion sequence of evaluators belonging to this variable set.
     */
    typedef typename boost::fusion::result_of::as_vector<
          typename boost::fusion::result_of::transform<Spaces, GetEvaluators<ShapeFunctionCache<Grid,Scalar>> >::type
          >::type Evaluators;

    /// Constructor, giving each variable a name (e.g. for output purposes)
    template <class RAIter>
    VariableSetDescription(Spaces const& spaces_, RAIter nameIt):
      VariableSetDescription(spaces_)
    {
      std::copy(nameIt,nameIt+noOfVariables,names.begin());
    }

    /**
     * \brief Constructor specifying for each variable a name.
     */
    VariableSetDescription(Spaces const& spaces_, std::vector<std::string> names_):
      VariableSetDescription(spaces_)
    {
      std::copy(names_.begin(),names_.end(),names.begin());
    }

    /**
     * \brief Constructor specifying for each variable both a name and a role.
     */
    template <class RAIter>
    VariableSetDescription(Spaces const& spaces_, RAIter nameIt, RAIter roleIt):
      VariableSetDescription(spaces_,nameIt)
    {
      std::copy(roleIt,roleIt+noOfVariables,roles.begin());
    }

    /**
     * \brief Constructor specifying for each variable both a name and a role.
     */
    VariableSetDescription(Spaces const& spaces_, std::vector<std::string> names_, std::vector<std::string> roles_)
      : VariableSetDescription(spaces_,names_)
    {
      std::copy(roles_.begin(),roles_.end(),roles.begin());
    }

    /// Constructor without naming the variables
    VariableSetDescription(Spaces const& spaces_):
      spaces(spaces_),
      gridView(boost::fusion::at_c<0>(spaces_)->gridView()),
      indexSet(boost::fusion::at_c<0>(spaces_)->indexSet())
    {}

    /**
     * \brief Computes the total number of scalar degrees of freedom collected in the variables [first,last).
     *
     * For each affected variable, the number of components is multiplied with the degrees of freedom of its FE space.
     */
    size_t degreesOfFreedom(int first=0, int last=noOfVariables) const
    {
      return degreesOfFreedom(spaces,first,last);
    }

    /**
     * \brief Computes the total number of scalar degrees of freedom collected in the variables [first,last).
     *
     * For each affected variable, the number of components is multiplied with the degrees of freedom of its FE space.
     */
    static size_t degreesOfFreedom(Spaces const& spaces, int first, int last)
    {
      auto dimensions = variableDimensions(spaces);
      assert(0<=first && first<=last && last<=dimensions.size());
      return std::accumulate(begin(dimensions)+first,begin(dimensions)+last,0);
    }

    /**
     * \brief Computes for each variable the number of scalar degrees of freedom.
     */
    static std::array<size_t,noOfVariables> variableDimensions(Spaces const& spaces, int first=0, int last=noOfVariables)
    {
      std::array<size_t,noOfVariables> dimensions;
      boost::fusion::for_each(VariableList(),ComputeDimension<noOfVariables>(dimensions,spaces));
      return dimensions;
    }

    /**
     * \brief number of components of idx'th variable
     */
    template <int idx>
    struct Components { static int const m = boost::fusion::result_of::value_at_c<VariableList,idx>::type::m; };

    /// space index of idx'th variable
    template <int idx>
    struct SpaceIndex { static int const spaceIndex = boost::fusion::result_of::value_at_c<VariableList,idx>::type::spaceIndex; };

    /**
     * \brief Typedefs for coefficient vector representations of contiguous subranges of the variables.
     *
     * Note that the representation defined by this class does not consist of FE functions.
     *
     * Since in contrast to FE functions the linear algebra coefficient
     * vectors are invalidated during mesh adaptation, the coefficient
     * vectors should be copied to FE functions before mesh refinement.
     */
    template <int first=0, int last=noOfVariables>
    class CoefficientVectorRepresentation
    {
      typedef Variables_Detail::VariableRangeCreator<Variables,ConstructCoefficientVector<Spaces>,first,last> Creator;

    public:
      /**
       * \brief A linear product space of FE coefficient vectors for the variables.
       * Note that this is not a linear space of
       * FE functions, only of coefficient vectors. In contrast to FE
       * functions, coefficient vectors are invalidated during mesh
       * adaptation.
       */
      typedef LinearProductSpace<Scalar,typename Creator::type> type;

      /**
       * \brief DEPRECATED. Use init(spaces) instead. Will be removed after 2014-05-01.
       */
      static typename Creator::View init(VariableSetDescription const& vardesc) __attribute__((deprecated)) {
        static bool called = false;
        if (called==false)
        {
          called = true;
          std::cerr << "Deprecated function CoefficientVectorRepresentation::init(vardesc) called. Use init(spaces) instead.\n"
                    << "This method will cease to work at the end of 2014.\n";
        }
        return init(vardesc.spaces);
      }

      /**
       * \brief Returns a (small) initializer object for coefficient vectors.
       *
       * The coeffcient vectors initialized with the returned initializer object are set to zero.
       */
      static typename Creator::View init(Spaces const& spaces) {
        return Creator::apply(ConstructCoefficientVector<Spaces>(spaces));
      }
    };

    /// spaces
    Spaces const&      spaces;

    /**
     * \brief The grid view on which the variables live.
     */
    GridView const&    gridView;

    /**
     * \brief The gridManager keeping the grid on which we operate.
     */
    GridManagerBase<Grid> const& gridManager() const { return boost::fusion::at_c<0>(spaces)->gridManager(); }

    /// index set
    IndexSet const&    indexSet;

    /// names
    std::array<std::string,noOfVariables>  names;

    std::array<std::string,noOfVariables>  roles;

  private:

    struct CheckHasEqualGridAndIndexSet
    {
      CheckHasEqualGridAndIndexSet(GridManagerBase<Grid> const* gp_, IndexSet const* isp_):
        gp(gp_), isp(isp_)
      {}

      template <class Space>
      void operator()(Space const* space) const {
        assert(&(space->gridManager())== gp);
        assert(&(space->indexSet()) == isp);
      }

      GridManagerBase<Grid> const* gp;
      IndexSet const* isp;
    };

    template <int nvars>
    struct ComputeDimension
    {
      ComputeDimension(std::array<size_t,nvars>& dims, Spaces const& spaces_):
        dims_(dims), spaces(spaces_)
      {}

      template <class Variable>
      void operator()(Variable const& /* v */) const
      {
        assert(dims_.size()>Variable::id);
        dims_[Variable::id] = Variable::m * boost::fusion::at_c<Variable::spaceIndex>(spaces)->degreesOfFreedom();
      }

      std::array<size_t,nvars>& dims_;
      Spaces const&             spaces;
    };

  };

  //---------------------------------------------------------------------
  //---------------------------------------------------------------------

  /**
   * \ingroup variables
   * \brief A function evaulating all functions in a variable set using the provided method.
   *
   * \tparam VariableSet a heterogeneous sequence of variable descriptions (defines the mapping of functions to evaluators)
   * \tparam Functions a heterogeneous sequence of FE functions (or function views)
   * \tparam Evaluators the heterogeneous container of evaluators for the variable set
   * \tparam Method a polymorphic Functor type with call operator (Function const&, Evaluator const&)
   *
   * \param vars the variable set to be evaluated.
   * \param eval the evaluators to be used for function evaluation
   * \param method the functor performing the actual evaluation
   *
   *  evaluateVariables valueMethod ValueMethod derivativeMethod DerivativeMethod
   */
  template <class VariableDescriptions, class Functions, class Evaluators, class Method>
  auto evaluateFunctions(Functions const& fs,
                         Evaluators const& eval,
                         Method const& method = Method())
  {
    using namespace boost::fusion;

    // Define the functor that evaluates the individual variables.
    auto doEval = [&] (auto const& pair)
    {
      // Extract the space index from the second component of the pair provided by zip.
      static int const sid = std::remove_reference_t<typename result_of::value_at_c<std::remove_reference_t<decltype(pair)>,1>::type>::spaceIndex;
      // Evaluate with evaluator corresponding to the function's space.
      return method(at_c<0>(pair),at_c<sid>(eval));
    };

    // We need this as vector type to default construct an object for transform. This way we can submit
    // views (such as joint_view) as template parameter.
    using VarDesc = typename result_of::as_vector<VariableDescriptions>::type;

    // For evaluation, the corresponding evaluator must be used based on the space index of the variables. To provide this,
    // we zip the functions with their variable descriptions to pairs from which we can extract both function and space index.
    return as_vector(transform(zip(fs,VarDesc()),doEval));
  }

  /**
   * \ingroup variables
   * \brief A function evaulating all functions in a variable set using the provided method.
   *
   * \tparam VariableSet the type of the variable set to be evaluated (i.e. VariableSet<...>)
   * \tparam Method a polymorphic Functor type with call operator (Function const&, Evaluator const&)
   *
   * \param vars the variable set to be evaluated.
   * \param eval a heterogeneous sequence of evaluators to be used for function evaluation
   * \param method the functor performing the actual evaluation
   *
   * \see EvaluateVariables evaluateFunctions valueMethod ValueMethod derivativeMethod DerivativeMethod
   */
  template <class VariableSet, class Method>
  auto evaluateVariables(VariableSet const& vars,
                         typename VariableSet::Descriptions::Evaluators const& eval,
                         Method const& method = Method())
  {
    return evaluateFunctions<typename VariableSet::Descriptions::Variables>(vars.data,eval,method);
  }

  /**
   * \ingroup variables
   * \brief The type of evaluated variables as returned by \ref evaluateVariables.
   *
   * This is a boost::fusion sequence of functions' values resulting from evaluating all functions in a given variable set.
   * \tparam VariableSet the type of the variable set (i.e. VariableSet<...>)
   * \tparam Method a polymorphic Functor type with call operator (Function const&, Evaluator const&) used for evaluating.
   *
   * \related evaluateVariables
   */
  template <class VariableSet, class Method>
  using EvaluateVariables = decltype(evaluateVariables(std::declval<VariableSet>(),
                                                       std::declval<typename VariableSet::Descriptions::Evaluators>(),
                                                       std::declval<Method>()));

  /**
   * \ingroup variables
   * \brief Helper method for evaluating whole variable sets.
   *
   * Use this in calling evaluateVariables when evaluating the functions' values.
   */
  auto valueMethod = [](auto const& f, auto const& eval) { return f.value(eval); };

  /**
   * \ingroup variables
   * \brief Helper method for evaluating whole variable sets.
   *
   * Use this in calling evaluateVariables when evaluating the functions' derivatives.
   */
  auto derivativeMethod = [](auto const& f, auto const& eval) { return f.derivative(eval); };

  /**
   * \ingroup variables
   * \brief The type of the valueMethod helper functor.
   */
  using ValueMethod = decltype(valueMethod);

  /**
   * \ingroup variables
   * \brief The type of the derivativeMethod helper functor.
   */
  using DerivativeMethod = decltype(derivativeMethod);

} // end of namespace Kaskade

//---------------------------------------------------------------------
//---------------------------------------------------------------------
//---------------------------------------------------------------------

#endif


#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <sepanta_msgs/upperbodymotors.h>
#include <sepanta_msgs/upperbodymotorsfeedback.h>
#include <sepanta_msgs/motorfeedback.h>
#include <sepanta_msgs/motor.h>
#include <sepanta_msgs/motorreset.h>
#include <sepanta_msgs/motorpid.h>
#include <sepanta_msgs/motortorque.h>
#include <sepanta_msgs/grip.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <sepanta_msgs/command.h>
#include <ik/simple_IK.h>
#include <ik/traj_IK.h>
#include <upperbody_core/sepanta_ik.h>

IKSystem::IKSystem(ros::NodeHandle n)
{
   node_handle = n;
   init();
}

void IKSystem::init_configs()
{
    motor_range m;

    m.max = 1023;
    m.min = 0;
    m.degree = 300;
    m.name = "RX-28";
    list_motor_configs.push_back(m);
    //===========================================

    m.max = 1023;
    m.min = 0;
    m.degree = 300;
    m.name = "RX-64";
    list_motor_configs.push_back(m);
    //===========================================

    m.max = 4095;
    m.min = 0;
    m.degree = 360;
    m.name = "MX-28";
    list_motor_configs.push_back(m);
    //===========================================

    m.max = 4095;
    m.min = 0;
    m.degree = 360;
    m.name = "MX-64";
    list_motor_configs.push_back(m);
    //===========================================

    m.max = 4095;
    m.min = 0;
    m.degree = 360;
    m.name = "MX-106";
    list_motor_configs.push_back(m);
}

float IKSystem::convert_motor_to_degree(string model_name,float value)
{
    if ( model_name == "MX-106")
    {
        int max_deg = list_motor_configs.at(4).degree;
        int min_pos = list_motor_configs.at(4).min;
        int max_pos = list_motor_configs.at(4).max;
        float result = roundf(((max_deg * float(value - min_pos)) / (max_pos - min_pos + 1)) - (max_deg / 2));
        return result;
    }
    else
    if ( model_name == "MX-64")
    {
        int max_deg = list_motor_configs.at(3).degree;
        int min_pos = list_motor_configs.at(3).min;
        int max_pos = list_motor_configs.at(3).max;
        float result = roundf(((max_deg * float(value - min_pos)) / (max_pos - min_pos + 1)) - (max_deg / 2));
        return result;
    }

    return -1;
}

float IKSystem::convert_degreeMotor_to_degreeIK(int index,float value)
{
    if ( index == 1) return (-1 * (value - 85) / 1.64); else
    if ( index == 2) return (-1 * (value - 27) / 1.64); else
    if ( index == 3) return (-1 * (value - 22) / 1.64); else
    return -1;
}

float IKSystem::convert_degreeIK_to_degreeMotor(int index,float value)
{
    if ( index == 1) return (value * -1 * 1.64 + 85); else
    if ( index == 2) return (value * -1 * 1.64 + 27); else
    if ( index == 3) return (value * -1 * 1.64 + 22); else

    return -1;
}

int IKSystem::convert_degree_to_motor(string model_name,float value)
{
    if ( model_name == "MX-106")
    {
        int max_deg = list_motor_configs.at(4).degree;
        int min_pos = list_motor_configs.at(4).min;
        int max_pos = list_motor_configs.at(4).max;
        int pos = 0;
        pos = (int)(roundf((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg)));
        pos = min(max(pos, 0), max_pos - 1);
        return pos;
    }
    else
    if ( model_name == "MX-64")
    {
        int max_deg = list_motor_configs.at(3).degree;
        int min_pos = list_motor_configs.at(3).min;
        int max_pos = list_motor_configs.at(3).max;
        int pos = 0;
        pos = (int)(roundf((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg)));
        pos = min(max(pos, 0), max_pos - 1);
        return pos;
    }

    return -1;
}

void IKSystem::scale_velocity(double * v1,double * v2 , double * v3,int size)
{
   double max = 0;
   for ( int i = 0 ; i < size ; i++)
   {
   	  if ( abs(v1[i]) > max ) max = abs(v1[i]);
   	  if ( abs(v2[i]) > max ) max = abs(v2[i]);
   	  if ( abs(v3[i]) > max ) max = abs(v3[i]);
   }

   for  ( int i = 0 ; i < size ; i++)
   {
   	   v1[i] = motor_max_speed * v1[i] / max;
   	   v2[i] = motor_max_speed * v2[i] / max;
   	   v3[i] = motor_max_speed * v3[i] / max;
   }
}

bool IKSystem::convert_all_positions(double * positions,int size)
{

   for ( int i = 0 ; i < size ; i++)
   {
       positions[i] =  convert_degree_to_motor("MX-64",convert_degreeIK_to_degreeMotor(1,Rad2Deg(positions[i])));
       if ( positions[i] > current_data[0].max || positions[i] < current_data[0].min ) return false;

       positions[i + size] =  convert_degree_to_motor("MX-64",convert_degreeIK_to_degreeMotor(2,Rad2Deg(positions[i + size])));
       if ( positions[i + size] > current_data[1].max || positions[i + size] < current_data[1].min ) return false;

       positions[i + 2 * size] =  convert_degree_to_motor("MX-64",convert_degreeIK_to_degreeMotor(3,Rad2Deg(positions[i + 2 * size])));
       if ( positions[i + size * 2] > current_data[2].max || positions[i + 2 * size] < current_data[2].min ) return false;
   }

   return true;
}

int IKSystem::convert_radPerSecond_to_speedMotor(double value)
{
    int direction = 0;

    if (value < 0)
         direction = 1024 ;

    double max_value = 1023 * 0.114 * 6;

    //value = min(max(value, -max_value), max_value);
    int speed = (int)(round(direction + abs(value) / (6 * 0.114)));

    return speed;
}

void IKSystem::motors_callback(const sepanta_msgs::upperbodymotorsfeedback::ConstPtr &msg)
{
   sepanta_msgs::motorfeedback m1 = msg->motorfeedbacks.at(0);
   sepanta_msgs::motorfeedback m2 = msg->motorfeedbacks.at(1);
   sepanta_msgs::motorfeedback m3 = msg->motorfeedbacks.at(2);

   ros::Time timeNow = ros::Time::now();

   current_data[0].position = m1.position;
   current_data[0].speed = m1.speed;
   current_data[0].min = m1.min;
   current_data[0].init = m1.init;
   current_data[0].max = m1.max;
   current_data[0].timestamp = timeNow;

   current_data[1].position = m2.position;
   current_data[1].speed = m2.speed;
   current_data[1].min = m2.min;
   current_data[1].init = m2.init;
   current_data[1].max = m2.max;
   current_data[1].timestamp = timeNow;

   current_data[2].position = m3.position;
   current_data[2].speed = m3.speed;
   current_data[2].min = m3.min;
   current_data[2].init = m3.init;
   current_data[2].max = m3.max;
   current_data[2].timestamp = timeNow;

   double p1  = Deg2Rad(convert_degreeMotor_to_degreeIK(1,convert_motor_to_degree("MX-64",current_data[0].position)));
   double p2  = Deg2Rad(convert_degreeMotor_to_degreeIK(2,convert_motor_to_degree("MX-64",current_data[1].position)));
   double p3  = Deg2Rad(convert_degreeMotor_to_degreeIK(3,convert_motor_to_degree("MX-64",current_data[2].position)));

   current_data[0].position_rad_ik = p1;
   current_data[1].position_rad_ik = p2;
   current_data[2].position_rad_ik = p3;

   double c1  = convert_degree_to_motor("MX-64", convert_degreeIK_to_degreeMotor(1,Rad2Deg(p1)));
   double c2  = convert_degree_to_motor("MX-64", convert_degreeIK_to_degreeMotor(2,Rad2Deg(p2)));
   double c3  = convert_degree_to_motor("MX-64", convert_degreeIK_to_degreeMotor(3,Rad2Deg(p3)));

   //std::cout<<"check 1 : "<<current_data[0].position<<" alireza 1 : "<<p1<<" motor 1 : "<<c1<<std::endl;
   //std::cout<<"check 2 : "<<current_data[1].position<<" alireza 2 : "<<p2<<" motor 2 : "<<c2<<std::endl;
   //std::cout<<"check 3 : "<<current_data[2].position<<" alireza 3 : "<<p3<<" motor 3 : "<<c3<<std::endl;
}

void IKSystem::update_arm_motors(int positions[3],int speed[3])
{
    sepanta_msgs::motor _msg;

    _msg.position = positions[0];
    _msg.speed = abs(speed[0]);
   // cout<<"1 :"<<_msg.position<<" "<<_msg.speed<<endl;
    motor_pub[0].publish(_msg);

    _msg.position = positions[1];
    _msg.speed = abs(speed[1]);
  //  cout<<"2 :"<<_msg.position<<" "<<_msg.speed<<endl;
    motor_pub[1].publish(_msg);

    _msg.position = positions[2];
    _msg.speed = abs(speed[2]);
  //  cout<<"3 :"<<_msg.position<<" "<<_msg.speed<<endl;
    motor_pub[2].publish(_msg);
}

void IKSystem::update_pen_motor(int position,int speed)
{
    sepanta_msgs::motor _msg;
    _msg.position = position;
    _msg.speed = speed;
    motor_pub[4].publish(_msg);
}

bool IKSystem::SepantaIK(double x0, double y0, double (&q_goal)[3])
{
    double l1 = 220;
    double l2 = 246;
    double c2 = ((x0*x0)+(y0*y0)-(l1*l1)-(l2*l2))/(2*l1*l2);
    double q21 = atan2(sqrt(1-(c2*c2)),c2);
    double q22 = atan2(-sqrt(1-(c2*c2)),c2);
    double q2 = 0;
    if(q21<3.313 && q21>0.217) q2 = q21;
    else if(q22<3.313 && q22>0.217) q2 = q22;
    else return false;

    double k1 = l1+l2*cos(q2);
    double k2 = l2*sin(q2);
    double q1 = atan2(y0,x0)-atan2(k2,k1);

    q_goal[0] = q1;
    q_goal[1] = q2;
    q_goal[2] = 1.5707963267948966 - (q1+q2);

    return true;
}

bool IKSystem::open_challange(double q0[3], double x0, double y0, double (&q_goal)[3])
{
  double T1[16] = {0};
  int i0;
  static const signed char iv0[4] = { 0, 0, -1, 0 };

  static const signed char iv1[4] = { 0, 0, 0, 1 };

  double E;
  double q2;
  double q3;
  double er1;
  double er2;
  double B;
  double x;

  //  --- get next point and give to IK ---
  //  -----------------------
  T1[12] = x0;
  T1[13] = y0;

  for (i0 = 0; i0 < 4; i0++)
  {
    T1[2 + (i0 << 2)] = iv0[i0];
    T1[3 + (i0 << 2)] = iv1[i0];
  }

  //  === newq = ik_sepanta(q0,T1,L) ===
  E = 100.0;

  //  ----------------------
  q2 = q0[0];
  q3 = q0[1];

  //  ----------------------
  //  ----------------------
  //  ----------------------
  int max_iteration = 100000;
  while (ros::ok())
  {
    //cout<<"Error :"<<E<<" "<<max_iteration<<endl;
    er1 = (22.0 * cos(q2) + 24.8 * cos(q2 + q3)) - T1[12] + 8.5;
    er2 = (22.0 * sin(q2) + 24.8 * sin(q2 + q3)) - T1[13] + 8;
    E = er1 * er1 + er2 * er2;
    B = q2 + q3;
    x = q2 + q3;
    q2 += -0.01 * (2.0 * er1 * (-22.0 * sin(q2) - 24.8 * sin(q2 + q3)) + 2.0 * er2 * (22.0 * cos(q2) + 24.8 * cos(q2 + q3)));
    q3 += -0.01 * (2.0 * er1 * (-24.8 * sin(B)) + 2.0 * er2 * (24.8 * cos(x)));

    if((int)E<20 || max_iteration < 0) break;
    max_iteration --;
  }

   // cout<<"Error :"<<E<<endl;

  if ( max_iteration <= 0 )
  {
    //problem
    return false;
  }

  //  ----------------------
  q_goal[0] = q2;
  q_goal[1] = q3;
  q_goal[2] = (-1.5707963267948966 - q2) - q3;

  return true;
}

string IKSystem::go_to_xy(int x,int y,int time)
{
     double q0[3] =  { current_data[0].position_rad_ik,
                       current_data[1].position_rad_ik,
                       current_data[2].position_rad_ik};

     cout<<"IK start "<<q0[0]<<" "<<q0[1]<<" "<<q0[2]<<endl;

     double f[2] = { x / 10 , y / 10};
     int frq=40;
     int TE=time;
     int samples = frq*TE;
     emxArray_real_T v1;
     v1.data = new double[samples];
     v1.size = new int[2];
     v1.size[0]=1;
     v1.size[1]=samples;
     v1.allocatedSize=samples;
     v1.numDimensions=2;
     v1.canFreeData=false;

     emxArray_real_T v2;
     v2.data = new double[samples];
     v2.size = new int[2];
     v2.size[0]=1;
     v2.size[1]=samples;
     v2.allocatedSize=samples;
     v2.numDimensions=2;
     v2.canFreeData=false;

     emxArray_real_T v3;
     v3.data = new double[samples];
     v3.size = new int[2];
     v3.size[0]=1;
     v3.size[1]=samples;
     v3.allocatedSize=samples;
     v3.numDimensions=2;
     v3.canFreeData=false;

     emxArray_real_T q1;
     q1.data = new double[samples * 3];
     q1.size = new int[2];
     q1.size[0]=3;
     q1.size[1]=samples;
     q1.allocatedSize=samples * 3;
     q1.numDimensions=2;
     q1.canFreeData=false;

     bool result = traj_IK(q0, f,TE,frq,&q1, &v1, &v2,&v3);

    if ( result )
    {
        bool result2 = convert_all_positions(q1.data,samples);
        if ( result2 == false )
        {
           //cout<<"Motor Limitation error - go to cancled"<<endl;
           return "ERROR MOTOR LIMITATION";
        }
        scale_velocity(v1.data,v2.data,v3.data,samples);

        int speeds[3];
        int positions[3];

        for ( int i = 1 ; i < samples ; i++ )
        {
            positions[0] = (int)q1.data[i];
            positions[1] = (int)q1.data[i + samples];
            positions[2] = (int)q1.data[i + 2 * samples];

            speeds[0] = (int)v1.data[i];
            speeds[1] = (int)v2.data[i];
            speeds[2] = (int)v3.data[i];

        	  update_arm_motors(positions,speeds);

            //cout<<"step :"<<i<<endl;
        	  boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / frq));
        }

        return "DONE";
    }
    else
    {
        return "ERROR IK";
    }

}

void IKSystem::init()
{
  init_configs();

  ros::NodeHandle n2;
  sub_handles = n2.subscribe("/upperbodycoreout_feedback", 10, &IKSystem::motors_callback,this);
  //============================================================================================
  motor_pub[0] = node_handle.advertise<sepanta_msgs::motor>("/upperbodycorein_right_1", 1);
  motor_pub[1] = node_handle.advertise<sepanta_msgs::motor>("/upperbodycorein_right_2", 1);
  motor_pub[2] = node_handle.advertise<sepanta_msgs::motor>("/upperbodycorein_right_3", 1);
  motor_pub[3] = node_handle.advertise<sepanta_msgs::motor>("/upperbodycorein_right_4", 1);
  motor_pub[4] = node_handle.advertise<sepanta_msgs::motor>("/upperbodycorein_right_5", 1);
  //============================================================================================
  cout<<"Init Done"<<endl;
}


/*
* Copyright (c) 2015 André Tupinambá (andrelrt@gmail.com)
*
* 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.
*/

#include <map>
#include <iostream>

#include <boost/timer/timer.hpp>
#include <boost/container/flat_map.hpp>

#include "soa_map.h"

template< class MapType >
void forwardFill( MapType& ret_map, size_t size )
{
    for( size_t i = 0; i < size; ++i )
    {
        ret_map.insert( std::make_pair( i, i ) );
    }
}

template< class MapType >
void reverseFill( MapType& ret_map, size_t size )
{
    for( size_t i = 0; i < size; ++i )
    {
        ret_map.insert( std::make_pair( size - i, i ) );
    }
}

template< class MapType >
void forwardFind( MapType& ret_map, size_t size )
{
    auto itEnd = ret_map.end();
    for( size_t i = 0; i < size; ++i )
    {
        auto it = ret_map.lower_bound( i );
        if( itEnd == it )
        {
            std::cout << "ff Oops! " << i << std::endl;
        }
    }
}

template< class MapType >
void reverseFind( MapType& ret_map, size_t size )
{
    auto itEnd = ret_map.end();
    for( size_t i = 0; i < size; ++i )
    {
        auto it = ret_map.lower_bound( size - i );
        if( itEnd == it )
        {
            std::cout << "rew Oops! " << i << std::endl;
        }
    }
}

template< class T_XY >
void least_square( const std::string& name )
{
    T_XY xy;

    srand( 1 );

    size_t size = 512 * 1024 * 1024;
    xy.reserve( size );
    for( size_t i = 0; i < size; ++i )
    {
        xy.push_back( i + ( 2.f * rand() - RAND_MAX / 2.f ) / RAND_MAX,
            i + ( 4.f * rand() - RAND_MAX / 2.f ) / RAND_MAX );
    }

    boost::timer::cpu_timer timer;

    timer.start();
    float meanx = 0;
    float meany = 0;
    for( size_t i = 0; i < size; ++i )
    {
        meanx += xy.get_x( i );
        meany += xy.get_y( i );
    }

    meanx /= size;
    meany /= size;

    float numerator = 0;
    float denominator = 0;
    for( size_t i = 0; i < size; ++i )
    {
        float diffx = ( xy.get_x( i ) - meanx );
        numerator += diffx * ( xy.get_y( i ) - meany );
        denominator += diffx * diffx;
    }

    float b = numerator / denominator;
    float a = meany - b * meanx;
    timer.stop();

    std::cout << "Least Square linear regression " << name << ": y = " << a << " + " << b << "x + e : "
        << timer.format();
}

struct XYZ
{
    float x;
    float y;
    XYZ( float x_, float y_ ) : x( x_ ), y( y_ ) {}
};
void scale_aos()
{
    std::vector< XYZ > xyz_a;
    std::vector< XYZ > xyz_b;
    std::vector< XYZ > ret_xyz;

    srand( 1 );

    size_t size = 512 * 1024;
    xyz_a.reserve( size );
    xyz_b.reserve( size );
    for( size_t i = 0; i < size; ++i )
    {
        xyz_a.emplace_back(  static_cast<float>( i ),  static_cast<float>( i ) );
        xyz_b.emplace_back( -static_cast<float>( i ), -static_cast<float>( i ) );
    }
    ret_xyz.resize( size, { 0.f, 0.f } );

    boost::timer::cpu_timer timer;

    XYZ* pxyza = xyz_a.data();
    XYZ* pxyzb = xyz_b.data();
    XYZ* prxyz = ret_xyz.data();
    {
        XYZ* mypxyza = pxyza;
        XYZ* mypxyzb = pxyzb;
        XYZ* myprxyz = prxyz;
        for( size_t i = 0; i < size; ++i )
        {
            myprxyz->x = mypxyza->x + mypxyzb->x;
            myprxyz->y = mypxyza->y + mypxyzb->y;
            ++mypxyza; ++mypxyzb; ++myprxyz;
        }
    }

    timer.start();
    for( size_t j = 0; j < 20000; ++j )
    {
        XYZ* mypxyza = pxyza;
        XYZ* mypxyzb = pxyzb;
        XYZ* myprxyz = prxyz;
        for( size_t i = 0; i < size; ++i )
        {
            myprxyz->x = mypxyza->x + mypxyzb->x;
            myprxyz->y = mypxyza->y + mypxyzb->y;
            ++mypxyza; ++mypxyzb; ++myprxyz;
        }
    }
    timer.stop();

    std::cout << "scale " << size << " points aos: " << timer.format();
}

void scale_soa()
{
    std::vector< float > x_a;
    std::vector< float > y_a;
    std::vector< float > x_b;
    std::vector< float > y_b;
    std::vector< float > ret_x;
    std::vector< float > ret_y;

    srand( 1 );

    size_t size = 512 * 1024;
    x_a.reserve( size );
    y_a.reserve( size );
    x_b.reserve( size );
    y_b.reserve( size );
    for( size_t i = 0; i < size; ++i )
    {
        x_a.push_back(  static_cast<float>( i ) );
        y_a.push_back(  static_cast<float>( i ) );
        x_b.push_back( -static_cast<float>( i ) );
        y_b.push_back( -static_cast<float>( i ) );
    }
    ret_x.resize( size, 0.f );
    ret_y.resize( size, 0. );

    boost::timer::cpu_timer timer;

    float *pxa = x_a.data();
    float *pya = y_a.data();
    float *pxb = x_b.data();
    float *pyb = y_b.data();
    float *prx = ret_x.data();
    float *pry = ret_y.data();
    {
        float *mypxa = pxa;
        float *mypya = pya;
        float *mypxb = pxb;
        float *mypyb = pyb;
        float *myprx = prx;
        float *mypry = pry;
        for( size_t i = 0; i < size; ++i )
        {
            *myprx = *mypxa + *mypxb;
            *mypry = *mypya + *mypyb;
            ++mypxa; ++mypxb; ++myprx;
            ++mypya; ++mypyb; ++mypry;
        }
    }
    timer.start();
    for( size_t j = 0; j < 20000; ++j )
    {
        float *mypxa = pxa;
        float *mypya = pya;
        float *mypxb = pxb;
        float *mypyb = pyb;
        float *myprx = prx;
        float *mypry = pry;
        for( size_t i = 0; i < size; ++i )
        {
            *myprx = *mypxa + *mypxb;
            *mypry = *mypya + *mypyb;
            ++mypxa; ++mypxb; ++myprx;
            ++mypya; ++mypyb; ++mypry;
        }
    }
    timer.stop();

    std::cout << "scale " << size << " points soa: " << timer.format();
}

namespace ccppbrasil {

class XY_aos
{
public:
    void reserve( size_t size )
    {
        xy_.reserve( size );
    }

    void push_back( float x, float y )
    {
        xy_.push_back( std::make_pair( x, y ) );
    }

    float get_x( size_t idx )
    {
        return xy_[ idx ].first;
    }

    float get_y( size_t idx )
    {
        return xy_[ idx ].second;
    }

private:
    std::vector< std::pair< float, float > > xy_;
};

class XY_soa
{
public:
    void reserve( size_t size )
    {
        x_.reserve( size );
        y_.reserve( size );
    }

    void push_back( float x, float y )
    {
        x_.push_back( x );
        y_.push_back( y );
    }

    float get_x( size_t idx )
    {
        return x_[ idx ];
    }

    float get_y( size_t idx )
    {
        return y_[ idx ];
    }

private:
    std::vector< float > x_;
    std::vector< float > y_;
};

}

int main( int argc, char* argv[] )
{
    boost::timer::cpu_timer timer;

    size_t ffsize = 100000000;
    size_t rewsize = 100000;

    // std::map
    for( int i = 0; i < 10; ++i )
    {
        std::map<size_t, size_t> std_map1;
        timer.start();
        forwardFill( std_map1, ffsize );
        timer.stop();
        std::cout << "forward fill std::map: " << timer.format();

        timer.start();
        forwardFind( std_map1, ffsize );
        timer.stop();
        std::cout << "forward find std::map: " << timer.format();
    }

    for( int i = 0; i < 10; ++i )
    {
        std::map<size_t, size_t> std_map2;
        timer.start();
		reverseFill( std_map2, rewsize );
        timer.stop();
        std::cout << "reverse fill std::map: " << timer.format();

        timer.start();
		reverseFind( std_map2, rewsize );
        timer.stop();
        std::cout << "reserve find std::map: " << timer.format();
    }

    // boost::container::flat_map
	for( int i = 0; i < 10; ++i )
	{
        boost::container::flat_map<size_t, size_t> flat_map1;
        flat_map1.reserve( ffsize );
        timer.start();
        forwardFill( flat_map1, ffsize );
        timer.stop();
        std::cout << "forward fill boost::container::flat_map: " << timer.format();

        timer.start();
        forwardFind( flat_map1, ffsize );
        timer.stop();
        std::cout << "forward find boost::container::flat_map: " << timer.format();
    }

	for( int i = 0; i < 10; ++i )
	{
        boost::container::flat_map<size_t, size_t> flat_map2;
        flat_map2.reserve( rewsize );
        timer.start();
        reverseFill( flat_map2, rewsize );
        timer.stop();
        std::cout << "reverse fill boost::container::flat_map: " << timer.format();

        timer.start();
        reverseFind( flat_map2, rewsize );
        timer.stop();
        std::cout << "reverse find boost::container::flat_map: " << timer.format();
    }

    // ccppbrasil::soa_map
	for( int i = 0; i < 10; ++i )
	{
        ccppbrasil::soa_map<size_t, size_t> soa_map1;
        soa_map1.reserve( ffsize );
        timer.start();
        forwardFill( soa_map1, ffsize );
        timer.stop();
        std::cout << "forward fill ccppbrasil::soa_map: " << timer.format();

        timer.start();
        forwardFind( soa_map1, ffsize );
        timer.stop();
        std::cout << "forward find ccppbrasil::soa_map: " << timer.format();
    }

	for( int i = 0; i < 10; ++i )
	{
        ccppbrasil::soa_map<size_t, size_t> soa_map2;
        soa_map2.reserve( rewsize );
        timer.start();
        reverseFill( soa_map2, rewsize );
        timer.stop();
        std::cout << "reverse fill ccppbrasil::soa_map: " << timer.format();

        timer.start();
        reverseFind( soa_map2, rewsize );
        timer.stop();
        std::cout << "reverse find ccppbrasil::soa_map: " << timer.format();
    }

	for( int i = 0; i < 10; ++i )
		least_square<ccppbrasil::XY_aos>( "aos" );
	for( int i = 0; i < 10; ++i )
		least_square<ccppbrasil::XY_soa>( "soa" );
    for( int i = 0; i < 10; ++i )
        scale_aos();
    for( int i = 0; i < 10; ++i )
        scale_soa();
    return 0;
}