// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP

#include <cstddef>

#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/exception.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/tag_cast.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/strategies/centroid.hpp>
#include <boost/geometry/strategies/concepts/centroid_concept.hpp>
#include <boost/geometry/views/closeable_view.hpp>

#include <boost/geometry/util/for_each_coordinate.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#if ! defined(BOOST_GEOMETRY_CENTROID_NO_THROW)

/*!
\brief Centroid Exception
\ingroup centroid
\details The centroid_exception is thrown if the free centroid function is called with
    geometries for which the centroid cannot be calculated. For example: a linestring
    without points, a polygon without points, an empty multi-geometry.
\qbk{
[heading See also]
\* [link geometry.reference.algorithms.centroid the centroid function]
}

 */
class centroid_exception : public geometry::exception
{
public:

    inline centroid_exception() {}

    virtual char const* what() const throw()
    {
        return "Boost.Geometry Centroid calculation exception";
    }
};

#endif

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace centroid
{

struct centroid_point
{
    template<typename Point, typename PointCentroid, typename Strategy>
    static inline void apply(Point const& point, PointCentroid& centroid,
            Strategy const&)
    {
        geometry::convert(point, centroid);
    }
};

template
<
    typename Indexed,
    typename Point,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct centroid_indexed_calculator
{
    typedef typename select_coordinate_type
        <
            Indexed, Point
        >::type coordinate_type;
    static inline void apply(Indexed const& indexed, Point& centroid)
    {
        coordinate_type const c1 = get<min_corner, Dimension>(indexed);
        coordinate_type const c2 = get<max_corner, Dimension>(indexed);
        coordinate_type m = c1 + c2;
        coordinate_type const two = 2;
        m /= two;

        set<Dimension>(centroid, m);

        centroid_indexed_calculator
            <
                Indexed, Point,
                Dimension + 1, DimensionCount
            >::apply(indexed, centroid);
    }
};

template<typename Indexed, typename Point, std::size_t DimensionCount>
struct centroid_indexed_calculator<Indexed, Point, DimensionCount, DimensionCount>
{
    static inline void apply(Indexed const& , Point& )
    {
    }
};

struct centroid_indexed
{
    template<typename Indexed, typename Point, typename Strategy>
    static inline void apply(Indexed const& indexed, Point& centroid,
            Strategy const&)
    {
        centroid_indexed_calculator
            <
                Indexed, Point,
                0, dimension<Indexed>::type::value
            >::apply(indexed, centroid);
    }
};

// There is one thing where centroid is different from e.g. within.
// If the ring has only one point, it might make sense that
// that point is the centroid.
template<typename Point, typename Range>
inline bool range_ok(Range const& range, Point& centroid)
{
    std::size_t const n = boost::size(range);
    if (n > 1)
    {
        return true;
    }
    else if (n <= 0)
    {
#if ! defined(BOOST_GEOMETRY_CENTROID_NO_THROW)
        throw centroid_exception();
#endif
        return false;
    }
    else // if (n == 1)
    {
        // Take over the first point in a "coordinate neutral way"
        geometry::convert(*boost::begin(range), centroid);
        return false;
    }
    return true;
}

/*!
    \brief Calculate the centroid of a ring.
*/
template <closure_selector Closure>
struct centroid_range_state
{
    template<typename Ring, typename Strategy>
    static inline void apply(Ring const& ring,
            Strategy const& strategy, typename Strategy::state_type& state)
    {
        typedef typename closeable_view<Ring const, Closure>::type view_type;

        typedef typename boost::range_iterator<view_type const>::type iterator_type;

        view_type view(ring);
        iterator_type it = boost::begin(view);
        iterator_type end = boost::end(view);

        for (iterator_type previous = it++;
            it != end;
            ++previous, ++it)
        {
            strategy.apply(*previous, *it, state);
        }
    }
};

template <closure_selector Closure>
struct centroid_range
{
    template<typename Range, typename Point, typename Strategy>
    static inline void apply(Range const& range, Point& centroid,
            Strategy const& strategy)
    {
        if (range_ok(range, centroid))
        {
            typename Strategy::state_type state;
            centroid_range_state<Closure>::apply(range, strategy, state);
            strategy.result(state, centroid);
        }
    }
};

/*!
    \brief Centroid of a polygon.
    \note Because outer ring is clockwise, inners are counter clockwise,
    triangle approach is OK and works for polygons with rings.
*/
struct centroid_polygon_state
{
    template<typename Polygon, typename Strategy>
    static inline void apply(Polygon const& poly,
            Strategy const& strategy, typename Strategy::state_type& state)
    {
        typedef typename ring_type<Polygon>::type ring_type;
        typedef centroid_range_state<geometry::closure<ring_type>::value> per_ring;

        per_ring::apply(exterior_ring(poly), strategy, state);

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(poly);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_ring::apply(*it, strategy, state);
        }
    }
};

struct centroid_polygon
{
    template<typename Polygon, typename Point, typename Strategy>
    static inline void apply(Polygon const& poly, Point& centroid,
            Strategy const& strategy)
    {
        if (range_ok(exterior_ring(poly), centroid))
        {
            typename Strategy::state_type state;
            centroid_polygon_state::apply(poly, strategy, state);
            strategy.result(state, centroid);
        }
    }
};

}} // namespace detail::centroid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct centroid: not_implemented<Tag>
{};

template <typename Geometry>
struct centroid<Geometry, point_tag>
    : detail::centroid::centroid_point
{};

template <typename Box>
struct centroid<Box, box_tag>
    : detail::centroid::centroid_indexed
{};

template <typename Segment>
struct centroid<Segment, segment_tag>
    : detail::centroid::centroid_indexed
{};

template <typename Ring>
struct centroid<Ring, ring_tag>
    : detail::centroid::centroid_range<geometry::closure<Ring>::value>
{};

template <typename Linestring>
struct centroid<Linestring, linestring_tag>
    : detail::centroid::centroid_range<closed>
 {};

template <typename Polygon>
struct centroid<Polygon, polygon_tag>
    : detail::centroid::centroid_polygon
 {};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc{centroid} \brief_strategy
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_strategy_reasons
\tparam Geometry \tparam_geometry
\tparam Point \tparam_point
\tparam Strategy \tparam_strategy{Centroid}
\param geometry \param_geometry
\param c \param_point \param_set{centroid}
\param strategy \param_strategy{centroid}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{[include reference/algorithms/centroid_strategies.qbk]}
}

*/
template<typename Geometry, typename Point, typename Strategy>
inline void centroid(Geometry const& geometry, Point& c,
        Strategy const& strategy)
{
    //BOOST_CONCEPT_ASSERT( (geometry::concept::CentroidStrategy<Strategy>) );

    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    typedef typename point_type<Geometry>::type point_type;

    // Call dispatch apply method. That one returns true if centroid
    // should be taken from state.
    dispatch::centroid<Geometry>::apply(geometry, c, strategy);
}

/*!
\brief \brief_calc{centroid}
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_default_strategy
\tparam Geometry \tparam_geometry
\tparam Point \tparam_point
\param geometry \param_geometry
\param c The calculated centroid will be assigned to this point reference

\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{
[heading Example]
[centroid]
[centroid_output]
}
 */
template<typename Geometry, typename Point>
inline void centroid(Geometry const& geometry, Point& c)
{
    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    typedef typename strategy::centroid::services::default_strategy
        <
            typename cs_tag<Geometry>::type,
            typename tag_cast
                <
                    typename tag<Geometry>::type,
                    pointlike_tag,
                    linear_tag,
                    areal_tag
                >::type,
            dimension<Geometry>::type::value,
            Point,
            Geometry
        >::type strategy_type;

    centroid(geometry, c, strategy_type());
}

/*!
\brief \brief_calc{centroid}
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_return{centroid}.
\tparam Point \tparam_point
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{centroid}

\qbk{[include reference/algorithms/centroid.qbk]}
 */
template<typename Point, typename Geometry>
inline Point return_centroid(Geometry const& geometry)
{
    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    Point c;
    centroid(geometry, c);
    return c;
}

/*!
\brief \brief_calc{centroid} \brief_strategy
\ingroup centroid
\details \details_calc{centroid,geometric center (or: center of mass)}. \details_return{centroid}. \details_strategy_reasons
\tparam Point \tparam_point
\tparam Geometry \tparam_geometry
\tparam Strategy \tparam_strategy{centroid}
\param geometry \param_geometry
\param strategy \param_strategy{centroid}
\return \return_calc{centroid}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/centroid.qbk]}
\qbk{[include reference/algorithms/centroid_strategies.qbk]}
 */
template<typename Point, typename Geometry, typename Strategy>
inline Point return_centroid(Geometry const& geometry, Strategy const& strategy)
{
    //BOOST_CONCEPT_ASSERT( (geometry::concept::CentroidStrategy<Strategy>) );

    concept::check_concepts_and_equal_dimensions<Point, Geometry const>();

    Point c;
    centroid(geometry, c, strategy);
    return c;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CENTROID_HPP


//
// Created by Сергей Кривонос on 25.09.17.
//
#include "Product.h"

#include "Fraction.h"
#include "Modulo.h"
#include "Infinity.h"
#include "Integer.h"
#include "Sum.h"
#include "e.h"
#include "i.h"
#include "pi.h"
#include <type_traits>
#include <boost/multiprecision/cpp_int.hpp>

namespace omnn{
namespace math {

    using namespace std;

    type_index order[] = {
        // for fast optimizing
        typeid(NaN),
        typeid(MInfinity),
        typeid(Infinity),
        typeid(Sum),
        typeid(Product),
        // general order
        typeid(Integer),
        typeid(MinusOneSq),
        typeid(Euler),
        typeid(Pi),
        typeid(Fraction),
        typeid(Exponentiation),
        typeid(Variable),
        typeid(Modulo),
    };

    auto ob = std::begin(order);
    auto oe = std::end(order);

    // inequality should cover all cases
    bool ProductOrderComparator::operator()(const Valuable& x, const Valuable& y) const
    {
        auto it1 = std::find(ob, oe, x.Type());
        if (it1==oe) IMPLEMENT

        auto it2 = std::find(ob, oe, y.Type());
        if (it2==oe) IMPLEMENT

        return it1 == it2 ? x.IsComesBefore(y) : it1 < it2;
    }

    Product::Product() : members{1}
    {
        hash = members.begin()->Hash();
    }

    Product::Product(const std::initializer_list<Valuable>& l)
    {
        for (const auto& arg : l)
        {
            if (arg.IsProduct())
                for (auto& m : arg.as<Product>().members)
                    this->Add(m, end());
            else
                this->Add(arg, end());
        }
    }

    int Product::findMaxVaExp()
    {
        vaExpsSum = 0;
        for (auto& i:vars) {
            if (!i.second.IsInt()) {
                IMPLEMENT;
            }
            vaExpsSum += i.second;
        }
        auto it = std::max_element(vars.begin(), vars.end(), [](auto& x, auto& y){
            return x.second < y.second;
        });
        if (it != vars.end()) {
            return static_cast<int>(it->second.as<Integer>());
        }
        return 0;
    }

    void Product::AddToVars(const Variable & va, const Valuable & exponentiation)
    {
        if (!exponentiation.IsInt()) {
            std::cerr << va.str() << '^' << exponentiation.str() << std::endl;
            IMPLEMENT // estimate in to be greater for those which you want to see first in sum sequence
        }
        if(exponentiation==0)
            return;

        auto& eai = exponentiation.as<Integer>();
        vaExpsSum += eai;

        auto& e = vars[va];
        if (!e.IsInt()) {
            IMPLEMENT
        }
        auto wasMax = maxVaExp == max_exp_t(e.ca(), 1);
        e += exponentiation;

        auto isMax = maxVaExp < e.ca();
        if (isMax) {
            maxVaExp = e.ca();
        }

        if (e == 0) {
            vars.erase(va);
        }

        if (!isMax && wasMax) {
            assert(eai < 0);
            maxVaExp = findMaxVaExp();
        }
    }

    void Product::AddToVarsIfVaOrVaExp(const Valuable &item)
    {
        if(item.IsVa())
        {
            AddToVars(item.as<Variable>(), 1_v);
        }
        else if (item.IsExponentiation())
        {
            auto& e = item.as<Exponentiation>();
            auto& base = e.getBase();
            if (base.IsVa())
            {
                AddToVars(base.as<Variable>(), e.getExponentiation());
            }
            else
            {
                auto itemMaxVaExp = item.getMaxVaExp();
                if (itemMaxVaExp > maxVaExp) {
                    maxVaExp = itemMaxVaExp;
                }
            }
        }
        else if (!item.IsSum() && item.FindVa())
        {
            IMPLEMENT
        }
    }

    Product::iterator Product::Had(iterator it)
    {
        hash ^= it->Hash();
        AddToVarsIfVaOrVaExp(*it);
        base::Update(it, *it ^ 2);
        return it;
    }

    const Product::iterator Product::Add(const Valuable& item, const iterator hint)
    {
        iterator it;

        if (members.size() == 1 && !item.IsSimple()) {
            it = begin();
            if (it->Same(1_v)) {
                Delete(it);
            }
        }

        if (item.IsInt()) {
            if (item == 1_v)
                it = begin();
            else if ((it = GetFirstOccurence<Integer>()) != end()
                || ((it = GetFirstOccurence<Fraction>()) != end() && it->IsSimpleFraction())
            ) {
                Update(it, *it * item);
            } else {
                it = base::Add(item, hint);
            }
        } else if (item.IsSimpleFraction()) {
            if ((it = GetFirstOccurence<Fraction>()) != end()
                || (it = GetFirstOccurence<Integer>()) != end()
            ) {
                Update(it, item * *it);
            } else {
                it = base::Add(item, hint);
            }
        } else if (item.IsProduct()) {
            it = hint;
            for (auto& i : item.as<Product>()) {
                it = Add(i, it);
            }
        } else if (item.IsFraction() && item.FindVa()) {
            auto& f = item.as<Fraction>();
            auto cmp = members.value_comp();
            it = std::min(Add(f.getNumerator()),
                          Add(f.getDenominator() ^ -1),
                          [&](auto& _1, auto& _2){ return cmp(*_1, *_2); });
        }
        else
        {
            it = std::find(members.begin(), members.end(), item);
            if(it==end()) {
                it = base::Add(item, hint);
                AddToVarsIfVaOrVaExp(item);
            } else
                Update(it, item.Sq());
        }
        return it;
    }

    void Product::Delete(typename cont::iterator& it)
    {
        std::function<void()> addToVars;
        if(it->IsVa())
        {
            addToVars = std::bind(&Product::AddToVars, this, it->as<Variable>(), -1_v);
        }
        else if (it->IsExponentiation())
        {
            auto& e = it->as<Exponentiation>();
            auto& ebase = e.getBase();
            if (ebase.IsVa()) {
                addToVars = std::bind(&Product::AddToVars, this, ebase.as<Variable>(), -e.getExponentiation());
            }
        }

        base::Delete(it);

        if(addToVars)
            addToVars();
    }

    void Product::optimize()
    {
        if (!optimizations || optimized)
            return;
        optimized = true;

        // zero
        auto it = GetFirstOccurence<Integer>();
        if (it != end()) {
            if (it->Same(0)) {
                Become(0);
                return;
            }

        // one
            if (it->Same(1) && size() > 1) {
                Delete(it);
            }
        }

        // fractionless
        bool updated;
        do {
            updated = {};
            for (auto it = GetFirstOccurence<Fraction>(); it != end(); ++it) {
                if (it->IsFraction() && it->FindVa()) {
                    auto& f = it->as<Fraction>();
                    auto& dn = f.getDenominator();
                    auto defract = dn.FindVa() ? (dn ^ (-1_v)) : 1_v / dn;
                    auto& n = f.getNumerator();
                    if(n==1_v)
                    {
                        Delete(it);
                    }
                    else
                    {
                        Update(it, n);
                        OptimizeOff o;
                        operator*=(defract);
                    }
                    updated = true;
                }
            }
        } while (updated);

        // optimize members, if found a sum then become the sum multiplied by other members
        for (auto it = members.begin(); it != members.end();)
        {
            if (it->IsSum())
            {
                auto sum = std::move(const_cast<Valuable&&>(*it));
                sum.optimize();
                Delete(it);
//                auto was = optimizations;
//                optimizations = false;
                for (auto& it : members)
                {
                    sum *= it;
                }
//                optimizations = was;
                Become(std::move(sum));
                return;
            }
            auto c = *it;
            c.optimize();
            if (!it->Same(c)) {
                Update(it, c);
                continue;
            }
            else
                ++it;
        }

        // emerge inner products
        for (auto it = members.begin(); it != members.end();)
        {
            if (it->IsProduct()) {
                for (auto& m : it->as<Product>())
                    Add(m);
                Delete(it);
            }
            else
                ++it;
        }

        do
        {
            updated = {};
            for (auto it = members.begin(); it != members.end();)
            {
                if (members.size() == 1)
                {
                    Become(std::move(*const_cast<Valuable*>(&*it)));
                    return;
                }

                if (*it == 1)
                {
                    Delete(it);
                    continue;
                }

                if (*it == 0)
                {
                    Become(0);
                    return;
                }

                auto c = *it;
                auto it2 = it;
                ++it2;
                while (it2 != members.end())
                {
                    if (c.MultiplyIfSimplifiable(*it2)) {
                        if (c.IsProduct()) {
                            auto& cAsP = c.as<Product>();
                            if(cAsP.size() == 2 && cAsP.begin()->IsSimple()) {
                                break;
                            } else {
                                IMPLEMENT
                            }
                        }
                        Delete(it2);
                        continue;
                    }
                    else
                        ++it2;
                }

                if (!it->Same(c)) {
                    Update(it, c);
                    updated = true;
                } else {
                    ++it;
                }
            }
        } while (updated);

        // fraction optimizations
        auto f = GetFirstOccurence<Fraction>();
        if (f != members.end()) {
            Valuable fo = *f;
            auto& dn = fo.as<Fraction>().getDenominator();
            if (dn.IsProduct()) {
                auto& pd = dn.as<Product>();
                for (auto it = members.begin(); it != members.end();)
                {
                    if (it != f && pd.Has(*it)) {
                        fo *= *it;
                        Delete(it);

                        if (!fo.IsFraction() ||
                            !fo.as<Fraction>().getDenominator().IsProduct()
                            ) {
                            break;
                        }
                    }
                    else  ++it;
                }
            }

            fo.optimize();

            if (fo.IsFraction()) {
                auto& dn = fo.as<Fraction>().getDenominator();
                if (!dn.IsProduct()) {
                    for (auto it = members.begin(); it != members.end();)
                    {
                        if (dn == *it) {
                            Delete(it);
                            fo *= dn;
                            break;
                        } else if (it->IsExponentiation() && it->as<Exponentiation>().getBase() == dn) {
                            Update(it, *it/dn);
                            fo *= dn;
                            break;
                        }
                        else  ++it;
                    }
                }
            }

            if(!f->Same(fo))
            {
                Update(f,fo);
            }
        }

        if(members.size()==0)
            Become(1_v);
        else if (members.size()==1)
            Become(std::move(const_cast<Valuable&&>(*members.begin())));
    }

    Valuable Product::Sqrt() const
    {
        return Each([](auto& m){ return m.Sqrt(); });
    }

    Valuable & Product::sq()
    {
        Product p;
        for (auto m : members)
        {
            p.Add(m.Sq());
        }
        return Become(std::move(p));
    }

    const Product::vars_cont_t& Product::getCommonVars() const
    {
        return vars;
    }

    Product::vars_cont_t Product::getCommonVars(const vars_cont_t& with) const
    {
        vars_cont_t common;
        for(auto& kv : vars)
        {
            auto it = with.find(kv.first);
            if (it != with.end()) {
                auto& i1 = kv.second.ca();
                auto& i2 = it->second.ca();
                if (i1 > 0_v && i2 > 0_v) {
                    common[kv.first] = std::min(i1, i2);
                }
                else
                {
                    IMPLEMENT
                }
            }
        }
        return common;
    }

    Valuable Product::calcFreeMember() const
    {
        Valuable _ = 1_v;
        if (getCommonVars().empty()) {
            for(auto& m : *this) {
                auto c = m.calcFreeMember();
                if(c.IsInt()) {
                    _ *= c;
                } else
                    IMPLEMENT
            }
        } else
            _ = 0_v;
        return _;
    }

    Valuable Product::getCommVal(const Product& with) const
    {
        return VaVal(getCommonVars(with.getCommonVars()));
    }

    Valuable Product::InCommonWith(const Valuable& v) const
    {
        auto _ = 1_v;
        auto check = [&](auto& with){
            if (std::find(members.begin(), members.end(), with) != members.end())
                _ *= with;
            else
                for(auto&m:members){
                    auto c = m.InCommonWith(with);
                    if (c!=1_v)
                        _*=c;
                }
        };

        if (v.IsProduct())
            for(auto& m : v.as<Product>())
                check(m);
        else if (v.IsSum())
            _ = v.InCommonWith(*this);
        else
            check(v);
        return _;
    }

    // NOTE : inequality must cover all cases for bugless Sum::Add
    bool Product::IsComesBefore(const Valuable& v) const
    {
        auto mae = getMaxVaExp();
        auto vme = v.getMaxVaExp();

        if (mae != vme)
            return mae > vme;

        char vp[sizeof(Product)];
        auto isSameType = v.IsProduct();
        if(!isSameType)
        {
            static const ProductOrderComparator oc;
            return oc(*this,v);
        }
        auto p = isSameType ? &v.as<Product>() : new(vp) Product{v};
        auto d = [isSameType](const Product* _){
            if (!isSameType && _){
                _->~Product();
            }
        };
        std::unique_ptr<const Product , decltype(d)> up(p,d);
        if (p)
        {
            if (vme != p->getMaxVaExp()) {
                std::cout << vme << std::endl;
                std::cout << p->getMaxVaExp() << std::endl;
                IMPLEMENT
            }

            if (vaExpsSum.ca() != p->vaExpsSum.ca())
            {
                return vaExpsSum > p->vaExpsSum;
            }

            if (members == p->members)
                return false;

            if (members.size() != p->members.size()) {
                return members.size() > p->members.size();
            }

            auto i1 = members.begin();
            auto i2 = p->members.begin();
            for (; i1 != members.end(); ++i1, ++i2) {
                auto it1 = std::find(ob, oe, i1->Type());
                assert(it1!=oe); // IMPLEMENT, add to order table
                auto it2 = std::find(ob, oe, i2->Type());
                assert(it2!=oe); // IMPLEMENT
                if (it1 != it2) {
                    return it1 < it2;
                }
            }
            // same types set, compare by value
            i1 = members.begin();
            i2 = p->members.begin();
            for (; i1 != members.end(); ++i1, ++i2) {
                if(*i1 != *i2)
                {
                    return i1->IsComesBefore(*i2);
                }
            }

            // everything is equal, should not be so
            IMPLEMENT
        }
        else
            IMPLEMENT
    }

    Valuable& Product::operator +=(const Valuable& v)
    {
        if(v == 0_v)
            return *this;
        if (*this == v)
            return *this *= 2;
        if(*this == -v)
            return Become(0_v);
        if(v.IsProduct()){
            auto& vAsP = v.as<Product>();
            Product thisHasNotCommonWithV, vHasNotInCommonWithThis;
            Product common;
            for(auto&& item : GetCont()){
                if(vAsP.Has(item))
                    common.Add(std::move(item));
                else
                    thisHasNotCommonWithV.Add(std::move(item));
            }
            for(auto& item : vAsP.GetConstCont()){
                if(!common.Has(item))
                    vHasNotInCommonWithThis.Add(item);
            }
            if(!(common.size()==1 && *common.begin()==1)){ // unchanged
                Valuable sum = Sum {thisHasNotCommonWithV, vHasNotInCommonWithThis};
                sum.optimize();
                sum *= common;
                sum.optimize();
                return Become(std::move(sum));
            }
        } else if (v.IsSum()) {
            return Become(v + *this);
        }
        else{
        auto& cv = getCommonVars();
        if (!cv.empty() && cv == v.getCommonVars())
        {
            auto valuable = varless() + v.varless();
            if(!valuable.IsSum())
                valuable *= getVaVal();
            return Become(std::move(valuable));
        }
        }
        return Become(Sum { *this, v });
    }

    std::pair<Valuable, Valuable> Product::SplitSimplePart() const {
        std::pair<Valuable, Valuable> parts;
        auto it = begin();
        IMPLEMENT
    }

    std::pair<Valuable, Valuable> Product::split_simple_part(){
        IMPLEMENT

    }

   std::pair<bool,Valuable> Product::IsSummationSimplifiable(const Valuable& v) const
   {
       std::pair<bool,Valuable> is;
       is.first = v == 0;
       if (is.first)
           is.second = *this;
       else if ((is.first = operator==(v)))
           is.second = *this * 2;
       else if ((is.first = operator==(-v)))
           is.second = 0;
       else if (Has(v)) {
           //OptimizeOn o;
           auto div = *this / v;
           auto divPlusOneIsSimple = div.IsSummationSimplifiable(vo<1>::get());
           is.first = divPlusOneIsSimple.first;
           if(divPlusOneIsSimple.first) {
               is.second = divPlusOneIsSimple.second * v;
           }
       } else if (v.IsExponentiation()) {
           is.first = Has(v) && size() == 2 && begin()->IsSimple();
           if(is.first) {
               is.second++ = *this;
               is.first = !is.second.IsSum();
           }
       } else if (v.IsSimple()) {
       } else if (v.IsProduct()
                  || v.IsVa()
                  || v.IsFraction()
                  ) {
           //OptimizeOn o;
           auto icw = InCommonWith(v);
           if (icw != 1) {
               auto thisNoCommon = *this / icw;
               if (!operator==(thisNoCommon)) { //multivalue cases (example ((-17)/16)*(4^((1/2))) / (1/2)*(4^((1/2))) = ((-17)/16)*(4^((1/2)))
                   auto vNoCommon = v / icw;
                   //std::cout << thisNoCommon << ".IsSummationSimplifiable(" << vNoCommon << ')' << std::endl;
                   is = thisNoCommon.IsSummationSimplifiable(vNoCommon);
                   if(is.first){
                       is.second *= icw;
                   }
               }
           }
//           auto& vp = v.as<Product>();
//           auto sp = SplitSimplePart();
//           auto vsp = vp.SplitSimplePart();
//           if(sp.second == vsp.second){
//
//               IMPLEMENT
//           }
       } else {
//           std::cout << v <<std::endl;
           LOG_AND_IMPLEMENT("Unknown case (need to implement) in Product::IsSummationSimplifiable for " << *this << " and " << v);
       }
       return is;
   }

    Valuable& Product::operator *=(const Valuable& v)
    {
        if ((size() == 1 && *begin() == 1) || size() == 0)
            return Become(Valuable(v));

        if (v.IsInt()){
            if (v==0) {
                return Become(0);
            } else if (v==1) {
                goto yes;
            } else {
                auto b = begin();
                if(b->IsSimple()){
                    auto up = *b * v;
                    if(up == 1)
                        Delete(b);
                    else
                        Update(b, up);
                    goto yes;
                }
            }
        }

        if (v.IsSum())
            return Become(v**this);

        if (v.IsVa())
        {
            auto& va = v.as<Variable>();
            for (auto it = members.begin(); it != members.end(); ++it)
            {
                if (it->Same(v)) {
                    Update(it, Exponentiation(va, 2));
                    goto yes;
                }
                else if (it->IsExponentiation())
                {
                    auto& e = it->as<Exponentiation>();
                    if (e.getBase() == va) {
                        Update(it, va ^ (e.getExponentiation()+1));
                        goto yes;
                    }
                }
            }
        }
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            auto& vExpBase = e.getBase();
            for (auto it = members.begin(); it != members.end();)
            {
                if (it->IsExponentiation() &&
                    it->as<Exponentiation>().getBase() == vExpBase)
                {
                    Update(it, *it*v);
                    goto yes;
                }
                else
                {
                    if (vExpBase == *it) {
                        Update(it, vExpBase ^ (e.getExponentiation()+1));
                        goto yes;
                    }
                    else
                        ++it;
                }
            }
        }
        else if (v.IsProduct())
        {
            if(operator==(v))
                sq();
            else
                for(auto& m : v.as<Product>())
                    base::Add(m);
            goto yes;
        }
        else
        {
            for (auto it = members.begin(); it != members.end();)
            {
                if (it->OfSameType(v)) {
                    auto up = *it * v;
                    if(up == 1){
                        Delete(it);
                    } else {
                        Update(it, up);
                    }
                    goto yes;
                }
                else
                    ++it;
            }
        }

        // add new member
        Add(v);

    yes:
        optimize();
        return *this;
    }

    Valuable& Product::operator /=(const Valuable& v)
    {
        if (v.IsProduct()) {
            auto opts = optimizations;
            optimizations = {};
            for(auto& i : v.as<Product>())
                *this /= i;
            optimizations = opts;
            optimize();
            return *this;
        }
        else if (v.IsSimple()) {
            if (v == 1_v) {
                return *this;
            }
            auto first = begin();
            if (first->IsSimple()) {
                auto _ = (*first) / v;
                if (_.IsInt() && _.ca() == 1)
                    Delete(first);
                else {
                    Update(first, _);
                    optimized = {};
                }
                optimize();
                return *this;
            }
            else {
                return operator *=(Fraction{ 1_v,v });
            }
        }
        else
        {
            auto it = std::find(begin(), end(), v);
            if(it != end()){
                Delete(it); // TODO : Review this branch to make it compatible with multivalue v
                optimize();
                return *this;
            }
            auto vIsExp = v.IsExponentiation();
            auto e = vIsExp ? &v.as<Exponentiation>() : nullptr;
            for (auto it = members.begin(); it != members.end(); ++it)
            {
                if (*it == v)
                {
                    if (v.IsMultival() == YesNoMaybe::No) {
                        Delete(it);
                        optimize();
                        return *this;
//                    } else if (v.IsMultival() == YesNoMaybe::Maybe) {

                    } else {
                        Update(it, *it * (v^-1));
                        optimize();
                        return *this;
                    }
                }
                else if (vIsExp && *it == e->getBase())
                {
                    return *this *= e->getBase() ^ (-e->getExponentiation());
                }
                else if (it->IsExponentiation() && it->as<Exponentiation>().getBase() == v)
                {
                    Update(it, *it / v);
                    optimize();
                    return *this;
                }
            }
        }
//        else if (v.IsVa())
//        {
//            auto it = getCommonVars().find(v);
//            if (it != getCommonVars().end() && *it != 0_v) {
//                if (*it == 1_v) {
//                    auto _ = find(begin(), end(), v);
//                    if (_ == end()) {
//                        IMPLEMENT
//                    }
//                    else
//                    {
//                        Delete(_);
//                    }
//                } else {
//                    for(auto i = begin(); i != end(); ++i)
//                    {
//                        if (i->IsExponentiation()) {
//                            auto e = Exponentiation::cast(*i);
//                            if (e->getBase() == v) {
//                                Update(i, *e / v);
//                                optimize();
//                                break;
//                            }
//                        }
//                    }
//                }
//            }
//        }
        return *this *= v ^ -1;
	}

    Valuable& Product::operator ^=(const Valuable& v)
    {
        auto _ = 1_v;
        for(auto& m : members)
        {
            _ *= m ^ v;
        }
        Become(std::move(_));
        return *this;
    }

	Valuable& Product::operator %=(const Valuable& v)
	{
		return base::operator %=(v);
	}

	namespace {
        constexpr std::hash<a_int> Hasher;
        const size_t Hash1 = Hasher(1);
    }
    bool Product::operator ==(const Valuable& v) const
    {
        auto sameHash = (Valuable::hash & ~Hash1) == (v.Hash() & ~Hash1); // ignore multiplication by 1
        auto same = v.Is<Product>() && sameHash;
        auto& c1 = GetConstCont();
        auto sz1 = c1.size();
        if (same) {
            auto& vp = v.as<Product>();
            auto& c2 = vp.GetConstCont();
            auto sz2 = c2.size();
            auto sameSizes = sz1 == sz2;
            if(sameSizes) {
                same = c1 == c2;
            } else if (sz1-sz2==1 || sz2-sz1==1) {
                auto it1 = c1.begin(), it2 = c2.begin();
                auto e1 = c1.end(), e2 = c2.end();
                for(same = true;
                    same && it1 != e1 && it2 != e2;
                    ++it1, ++it2)
                {
                    if(it1->Same(1_v)){
                        ++it1;
                    }
                    if(it2->Same(1_v)){
                        ++it2;
                    }

                    if(it1 == e1 || it2 == e2)
                        continue;

                    same = same && it1->Same(*it2);
                }

                same = same && it1 == e1 && it2 == e2;
            } else {
                same = {};
            }
        }
        else if (sameHash
                 && (sz1 == 1
                     || (sz1 == 2 && c1.begin()->Same(1_v) )))
        {
            same = c1.rbegin()->operator==(v);
        }
        return same;
    }

    Product::operator double() const
    {
        double d=1;
        for(auto& i:members)
        {
            d*=static_cast<double>(i);
        }
        return d;
    }

    Valuable& Product::d(const Variable& x)
    {
        if(vars.find(x) != vars.end())
        {
            *this *= vars[x];
            *this /= x;
            optimize();
        }else
            Become(0_v);
        return *this;
    }

    Valuable Product::operator()(const Variable& va) const
    {
        return operator()(va, 0_v);
    }

    Valuable Product::operator()(const Variable& va, const Valuable& augmentation) const
    {
        Valuable s; s.SetView(Valuable::View::Flat);

        if(augmentation.HasVa(va)) {
            IMPLEMENT;
        } else {
            auto coVa = getCommonVars();
            auto it = coVa.find(va);
            if (it != coVa.end()) {
                if (it->second < 0) {
                    s = ((*this / (it->first ^ it->second)) / augmentation) ^ (1_v / -it->second);
                }
                else
                {
                    s = (augmentation / (*this / (it->first ^ it->second))) ^ (1_v / it->second);
                }
            }
            else
            {
                auto a = 1_v;
                auto aug = augmentation;
                for(auto& m : members)
                    if (m.HasVa(va))
                        a *= m;
                    else
                        aug *= m;
                if (a==1) {
                    IMPLEMENT
                }
                s = a(va,aug);
            }
        }

//        if(augmentation.HasVa(va)) {
//            IMPLEMENT;
//        } else {
//            auto _ = augmentation;
//            auto left = 1_v;
//            for(auto& m : members)
//            {
//                if (m.HasVa(va)) {
//                    left *= m;
//                } else {
//                    _ /= m;
//                }
//            }
//
//            left.optimize();
//            if (left.IsProduct()) {
//                IMPLEMENT
//            }
//
//            return left(va, _);
//        }
//        auto cova = getCommonVars();
//        auto it = cova.find(va);
//        if (it != cova.end()) {
//            auto _ = augmentation / (*this / (va ^ it->second));
//            if(_.HasVa(va))
//                IMPLEMENT;
//            s.insert(_);
//            if (it->second % 2 == 0) {
//                s.insert(-_);
//            }
//        }
//        else
//        {
//            IMPLEMENT
//        }
        return s;
    }

    void Product::solve(const Variable& va, solutions_t& solutions) const
    {
        auto it = std::find(members.begin(), members.end(), va);
        if(it != members.end())
            solutions.insert(0_v);
        else {
            it = begin();
            bool found = {};
            const Exponentiation* e = {};
            while (it != end()
                    && !(it->IsExponentiation()
                     && (found = it->as<Exponentiation>().getBase() == va)
                     ) ) {
                ++it;
            }
            if (found) {
                if (e->getExponentiation() == 0_v) {
                    IMPLEMENT
                }
                solutions.insert(0_v);
            }
            else {
                std::cout << "Solving " << str() << std::endl;
                IMPLEMENT // TODO: find exponentiations of va
            }
        }
    }

    Valuable::solutions_t Product::Distinct() const {
        solutions_t branches = { 1_v };
        for (auto& m : members) {
            solutions_t newBranches;
            for (auto&& branch : m.Distinct()) {
                for (auto& b : branches) {
                    newBranches.emplace(b * branch);
                }
            }
            branches = std::move(newBranches);
        }
        return branches;
    }

	std::ostream& Product::print(std::ostream& out) const
	{
        std::stringstream s;
        constexpr char sep[] = "*";
        for (auto& b : members)
            s << b << sep;
        auto str = s.str();
        auto cstr = const_cast<char*>(str.c_str());
        cstr[str.size() - sizeof(sep) + 1] = 0;
        out << cstr;
        return out;
	}

}}


// Copyright (c) 1997-2001  ETH Zurich (Switzerland).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Kaspar Fischer <fischerk@inf.ethz.ch>

#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cstdlib>

#include <CGAL/boost/iterator/transform_iterator.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/shared_ptr.hpp>

#include <CGAL/Exact_rational.h>
#include <CGAL/MP_Float.h>
#include <CGAL/QP_solver/QP_solver.h>
#include <CGAL/QP_solver/QP_full_exact_pricing.h>
#include <CGAL/QP_solver/QP_exact_bland_pricing.h>
#include <CGAL/QP_solver/QP_partial_exact_pricing.h>
#include <CGAL/QP_solver/QP_full_filtered_pricing.h>
#include <CGAL/QP_solver/QP_partial_filtered_pricing.h>

#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>

#include <CGAL/boost_mp.h>
//Currently already included in boost_mp.h
//#ifdef CGAL_USE_BOOST_MP
//# include <boost/multiprecision/cpp_int.hpp>
//// After some CGAL includes so we get a chance to define CGAL_USE_GMP.
//# ifdef CGAL_USE_GMP
//#  include <boost/multiprecision/gmp.hpp>
//# endif
//#endif

// Routines to output to MPS format:
namespace QP_from_mps_detail {

  template<typename T>
  struct MPS_type_name {
    static const char *name() { return nullptr; }
  };

  template<>
  struct MPS_type_name<double> {
    static const char *name() { return "floating-point"; }
  };

  template<>
  struct MPS_type_name<int> {
    static const char *name() { return "integer"; }
  };
#ifdef CGAL_USE_BOOST_MP
  template <class Backend, boost::multiprecision::expression_template_option Eto>
  struct MPS_type_name<boost::multiprecision::number<Backend, Eto> > {
    typedef boost::multiprecision::number<Backend, Eto> NT;
    static const char *name() {
      if (boost::multiprecision::number_category<NT>::value == boost::multiprecision::number_kind_integer)
        return "integer";
      else if (boost::multiprecision::number_category<NT>::value == boost::multiprecision::number_kind_rational)
        return "rational";
      else
        return nullptr;
    }
  };
#endif
#ifdef CGAL_USE_GMPXX
  template<>
  struct MPS_type_name<mpq_class> {
    static const char *name() { return "rational"; }
  };
#endif
#ifdef CGAL_USE_GMP
  template<>
  struct MPS_type_name<CGAL::Gmpq> {
    static const char *name() { return "rational"; }
  };
#endif

#ifdef CGAL_USE_LEDA
  template<>
  struct MPS_type_name<leda::rational> {
    static const char *name() { return "rational"; }
  };
#endif
  template<>
  struct MPS_type_name<CGAL::Quotient<CGAL::MP_Float> > {
    static const char *name() { return "rational"; }
  };

  template<typename IT>
  struct IT_to_ET {
  };

  template<>
  struct IT_to_ET<double> {
    typedef CGAL::MP_Float ET;
  };
#ifdef CGAL_USE_BOOST_MP
  template<>
  struct IT_to_ET<boost::multiprecision::cpp_rational> {
    typedef boost::multiprecision::cpp_rational ET;
  };
#endif

#ifdef CGAL_USE_GMP
#ifdef CGAL_USE_GMPXX
  template<>
  struct IT_to_ET<int> {
    typedef mpz_class ET;
  };

  template<>
  struct IT_to_ET<mpq_class> {
    typedef mpq_class ET;
  };
#elif defined CGAL_USE_BOOST_MP
  template<>
  struct IT_to_ET<int> {
    typedef boost::multiprecision::mpz_int ET;
  };

  template<>
  struct IT_to_ET<boost::multiprecision::mpq_rational> {
    typedef boost::multiprecision::mpq_rational ET;
  };
#else
  template<>
  struct IT_to_ET<int> {
    typedef CGAL::Gmpz ET;
  };
#endif

  template<>
  struct IT_to_ET<CGAL::Gmpq> {
    typedef CGAL::Gmpq ET;
  };
#endif

#ifdef CGAL_USE_LEDA
// Pick one arbitrarily if we have both LEDA and GMP
#ifndef CGAL_USE_GMP
  template<>
  struct IT_to_ET<int> {
    typedef leda::integer ET;
  };
#endif

  template<>
  struct IT_to_ET<leda::rational> {
    typedef leda::rational ET;
  };
#endif
  template<>
  struct IT_to_ET<CGAL::Quotient<CGAL::MP_Float> > {
    typedef CGAL::Quotient<CGAL::MP_Float> ET;
  };

#if defined CGAL_USE_BOOST_MP && !defined CGAL_USE_GMP && !defined CGAL_USE_LEDA
  // Last chance for int
  template<>
  struct IT_to_ET<int> {
    typedef boost::multiprecision::cpp_int ET;
  };
#endif
} // QP_from_mps_detail

template<typename QP>
void write_MPS(std::ostream& out,
               const std::string& number_type, // pass "" to deduce
                                               // the number-type from
                                               // U_iterator::value_type
               const std::string& description,
               const std::string& generator_name,
               const std::string& problem_name,
               const QP& qp)
{
  // output header:
  if (number_type.length() == 0) {
    const char *tn = QP_from_mps_detail::MPS_type_name
      <typename
      std::iterator_traits<typename QP::U_iterator>::value_type>::name();
    if (tn != nullptr)
      out << "* Number-type: " << tn << "\n";
  } else
      out << "* Number-type: " << number_type << "\n";
  out << "* Description: " << description << "\n"
      << "* Generated-by: " << generator_name << "\n";

  // print in qmatrix format
  CGAL::print_quadratic_program(out, qp, problem_name);
}

boost::shared_ptr<std::ofstream>
create_output_file(const char *filename, // Note: "Bernd3" and not
                                         // "Bernd3.mps".
                   const char *directory,
                   const char *suffix)   // Note: "shifted"
                                         // and not "_shifted".
{
  std::string new_name = std::string(directory) +
    std::string("/") + std::string(filename) + std::string("_") +
    std::string(suffix) + std::string(".mps");
  return boost::shared_ptr<std::ofstream>(new std::ofstream(new_name.c_str(),
                                                            std::ios_base::trunc |
                                                            std::ios_base::out));
}

template<typename NT>
struct tuple_add :
  public CGAL::cpp98::unary_function<const boost::tuple<NT, NT>&, NT>
{
  NT operator()(const boost::tuple<NT, NT>& t) const
  {
    return boost::tuples::get<0>(t) + boost::tuples::get<1>(t);
  }
};

template<typename IT,   // input number type
         typename ET>   // exact number type compatible with IT (ET is used,
                        // for instance, by QP in the query methods
                        // has_equalities_only_and_full_rank())
void create_shifted_instance(const CGAL::Quadratic_program_from_mps <IT>& qp,
                             const char *file,   // Note: "Bernd3" and
                                                 // not "Bernd3.mps".
                             const char *dir)
{
  // This routine implements the following transformation:
  //
  //   A  -> A
  //   b  -> b + A v
  //   c  -> c - 2 v^T
  //   D  -> D
  //   row_types -> row_types
  //   l  -> l + v
  //   u  -> u + v
  //   fl -> fl
  //   fu -> fu
  //
  // where v = [1,...,n]^T.

  // extract data from qp:
  const int n = qp.get_n();
  const int m = qp.get_m();

  // offset vector:
  std::vector<IT> v(n);
  for (int i=0; i<n; ++i)
    v[i] = i+1;

  // compute A v into Av;
  std::vector<IT> Av(m, IT(0));
  for (int i=0; i<m; ++i)
    for (int j=0; j<n; ++j)
      Av[i] += (const IT&)(*(qp.get_a()+j))[i] * v[j];

  // compute - 2 v^T D into mvTD:
  std::vector<IT> mvTD(n, IT(0));  // -2D^Tv
  for (int i=0; i<n; ++i) {
    for (int j=0; j<n; ++j)
      mvTD[i]
        += ( j <= i ? (const IT&)(*(qp.get_d()+i))[j] : (const IT&)(*(qp.get_d()+j))[i]) * v[j];
    mvTD[i] *= -1;
  }

  // output:
  using boost::make_transform_iterator;
  using boost::make_zip_iterator;
  boost::shared_ptr<std::ofstream> out = create_output_file(file, dir, "shifted");

  write_MPS(*out,
                  "", // deduce number-type
                  "Shifted instance of original file",
                  "master_mps_to_derivatives-create_shifted_instance",
                  qp.get_problem_name(),
                  CGAL::make_quadratic_program_from_iterators(
     n,
     m,
     qp.get_a(),
     make_transform_iterator(
                             make_zip_iterator(boost::make_tuple(qp.get_b(),Av.begin())),
                             tuple_add<IT>()),
     qp.get_r(),
     qp.get_fl(),
     make_transform_iterator(
                              make_zip_iterator(boost::make_tuple(qp.get_l(),v.begin())),
                              tuple_add<IT>()),
     qp.get_fu(),
     make_transform_iterator(
                             make_zip_iterator(boost::make_tuple(qp.get_u(),v.begin())),
                             tuple_add<IT>()),
     qp.get_d(),
     make_transform_iterator(
                           make_zip_iterator(boost::make_tuple(qp.get_c(),mvTD.begin())),
                           tuple_add<IT>()),
     qp.get_c0()
     )
);
  out->close();
}

template<typename IT,   // input number type
         typename ET>   // exact number type compatible with IT (ET is used,
                        // for instance, by QP in the query methods
                        // has_equalities_only_and_full_rank())
void create_free_instance(CGAL::Quadratic_program_from_mps<IT>& qp,
                          const char *file,   // Note: "Bernd3" and
                                              // not "Bernd3.mps".
                          const char *dir)
{
  // This routine converts the given instance into an equivalent
  // problem where all bounds are modelled by additional rows of A and
  // where all variables are free.
  //
  // That is, the quantities c and D do not change, but A, b, and
  // row_types are augmented by at most 2n additional rows/entries
  // (and fl and fu are adjusted as well).

  // extract data from qp:
  const unsigned int n = qp.get_n();
  unsigned int m = qp.get_m();

  // add rows to A and corresponding entries to b:
  for (unsigned int i=0; i<n; ++i) {
    if (*(qp.get_fl()+i)) {                        // x >= l
      // add a row to A:
      for (unsigned int j=0; j<n; ++j)
        qp.set_a (j, m, (i==j? 1 : 0));

      // add corresponding entry to b:
      qp.set_b(m, qp.get_l()[i]);

      // add corresponding row type:
      qp.set_r(m, CGAL::LARGER);
      ++m;
    }
    qp.set_l(i, false);                           // variable becomes free
    if (*(qp.get_fu()+i)) {                        // x <= u
      // add a row to A:
      for (unsigned int j=0; j<n; ++j)
        qp.set_a (j, m ,(i==j? 1 : 0));

      // add corresponding entry to b:
      qp.set_b(m, qp.get_u()[i]);

      // add corresponding row type:
      qp.set_r(m, CGAL::SMALLER);
      ++m;
    }
    qp.set_u(i, false);                         // variable becomes free
  }
  // output:
  boost::shared_ptr<std::ofstream> out = create_output_file(file, dir, "free");
  write_MPS(*out,
                  "", // deduce number-type
                  "Freed instance of original file",
                  "master_mps_to_derivatives-create_free_instance",
                  qp.get_problem_name(),
            qp);
  out->close();
}

template<typename IT>
bool create_derivatives(const char *path,
                        const char *file,
                        const char *dir,
                        std::string& msg)
{
  using std::cerr;
  using CGAL::Tag_true;
  using CGAL::Tag_false;

  // diagnostics:
  cerr << "  Trying to load input MPS using "
       << QP_from_mps_detail::MPS_type_name<IT>::name()
       << " number-type...\n";

  // open input file:
  std::ifstream f(path);
  if (!f) {
    cerr << "    Could not open file '" << path << "'.\n";
    return false;
  }

  // load QP instance:
  typedef typename QP_from_mps_detail::IT_to_ET<IT>::ET ET;
  typedef CGAL::Quadratic_program_from_mps<IT> QP;
  QP qp(f);

  // check for format errors in MPS file:
  if (!qp.is_valid()) {
    msg = "Input is not a valid MPS file: " + qp.get_error();
    return false;
  }
  cerr << "    MPS-file successfully input.\n";

  // no derivatives if comment says so
  if (qp.get_comment().find(std::string("Derivatives: none"))!=std::string::npos)
    cerr << "    No derivatives made.\n";
  else {
    // derivates:
    create_shifted_instance<IT, ET>(qp, file, dir);
    create_free_instance<IT, ET>(qp, file, dir);
    // Note: insert additional derivative routines here! Your routine may use
    // create_output_file() to create the output file.
  }
  // cleanup:
  f.close();
  return true;
}

int main(const int argnr, const char **argv) {
  typedef CGAL::Exact_rational Rational;

  // output usage information:
  if (argnr != 4) {
    std::cerr << "Usage: " << argv[0] << " path-to-master.mps name "
              << "dest-directory\n\n"
              << "Given a master MPS-file, this program constructs from it "
              << "several other, derived\nMPS-files and saves them in the "
              << "destination directory.\n\n"
              << "The argument 'name' should be the basename (without "
              << "extension) of the\nargument 'path-to-master.mps'.\n\n"
              << "Usually, you do not have to call this program directly; "
              << "./create_testsuite does\nit for you automagically.\n";
    return 0; // Note: 0 because otherwise the testsuite will fail...
  }

  // extract arguments:
  const char *path = argv[1];
  const char *file = argv[2];
  const char *dir  = argv[3];

  // As we do not know the number-type used in the MPS-file, we simply try
  // to load the MPS-file once with a double type, once with a Rational
  // type, and once with an int type.
  // we try the most special one first, so the order is
  // int -> double -> rational
  std::string message;
  if (!create_derivatives<int>(path, file, dir, message))
    if (!create_derivatives<double>(path, file, dir, message))
      if (!create_derivatives<Rational>(path, file, dir, message)) {
        // Here, the MPS-file must be ill-formatted.
        std::cerr << "  " << message << "\n";
        return 2;
      }

  return 0;
}


/*
 symmetry.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <cmath>
#include <iostream>
#include <iomanip>
#include "mathfunctions.h"
#include "symmetry.h"
#include "system.h"
#include "memory.h"
#include "constants.h"
#include "timer.h"
#include "error.h"
#include "interaction.h"
#include "files.h"
#include <cmath>
#include <vector>
#include <set>
#include <algorithm>

#ifdef _USE_EIGEN
#include <Eigen/Core>
#endif

using namespace ALM_NS;

Symmetry::Symmetry(ALM *alm) : Pointers(alm)
{
    file_sym = "SYMM_INFO";
}

Symmetry::~Symmetry()
{
    memory->deallocate(symrel);
    memory->deallocate(symrel_int);
    memory->deallocate(tnons);
    memory->deallocate(map_sym);
    memory->deallocate(map_p2s);
    memory->deallocate(map_s2p);
    memory->deallocate(symnum_tran);
    memory->deallocate(sym_available);
}

void Symmetry::init()
{
    int i, j;
    int nat = system->nat;

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

    setup_symmetry_operation(nat, nsym, system->lavec, system->rlavec,
        system->xcoord, system->kd);

    memory->allocate(tnons, nsym, 3);
    memory->allocate(symrel_int, nsym, 3, 3);

    int isym = 0;
    for (std::vector<SymmetryOperation>::iterator iter = SymmList.begin(); iter != SymmList.end(); ++iter) {
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel_int[isym][i][j] = (*iter).rot[i][j];
            }
        }
        for (i = 0; i < 3; ++i) {
            tnons[isym][i] = (*iter).tran[i];
        }
        ++isym;
    }

    std::cout << "  Number of symmetry operations = " << nsym << std::endl;
    memory->allocate(symrel, nsym, 3, 3);
    symop_in_cart(system->lavec, system->rlavec);

    memory->allocate(sym_available, nsym);
    int nsym_fc;
    symop_availability_check(symrel, sym_available, nsym, nsym_fc);

    if (nsym_fc == nsym) {
        std::cout << "  All symmetry operations will be used to reduce the number of force constants." << std::endl;
    } else {
        std::cout << "  " << nsym_fc << " symmetry operations out of "
            << nsym << " will be used to reduce the number of parameters." << std::endl;
        std::cout << "  Other " << nsym - nsym_fc
            << " symmetry operations will be imposed as constraints." << std::endl;
    }
    std::cout << std::endl;

    pure_translations();

    memory->allocate(map_sym, nat, nsym);
    memory->allocate(map_p2s, natmin, ntran);
    memory->allocate(map_s2p, nat);

    genmaps(nat, system->xcoord, map_sym, map_p2s, map_s2p);

    std::cout << std::endl;
    std::cout << "  **Cell-Atom Correspondens Below**" << std::endl;
    std::cout << std::setw(6) << " CELL" << " | " << std::setw(5) << "ATOM" << std::endl;

    for (int i = 0; i < ntran; ++i) {
        std::cout << std::setw(6) << i + 1 << " | ";
        for (int j = 0; j < natmin; ++j)  {
            std::cout << std::setw(5) << map_p2s[j][i] + 1;
            if((j + 1)%5 == 0) {
                std::cout << std::endl << "       | ";
            }
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;

    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;
}

void Symmetry::setup_symmetry_operation(int nat, unsigned int &nsym,
                                        double aa[3][3], double bb[3][3], double **x, int *kd)
{
    int i, j;

    SymmList.clear();

    if (nsym == 0) {

        // Automatically find symmetries.

        std::cout << "  NSYM = 0 : Trying to find symmetry operations." << std::endl;
        std::cout << "             Please be patient. " << std::endl;
        std::cout << "             This can take a while for a large supercell." << std::endl << std::endl;

        findsym(nat, aa, x, SymmList);
        // The order in SymmList changes for each run because it was generated
        // with OpenMP. Therefore, we sort the list here to have the same result.
        std::sort(SymmList.begin()+1,SymmList.end());
        nsym = SymmList.size();

        if (is_printsymmetry) {
            std::ofstream ofs_sym;
            std::cout << "  PRINTSYM = 1: Symmetry information will be stored in SYMM_INFO file."
                << std::endl << std::endl;
            ofs_sym.open(file_sym.c_str(), std::ios::out);
            ofs_sym << nsym << std::endl;

            for (std::vector<SymmetryOperation>::iterator p = SymmList.begin(); p != SymmList.end(); ++p) {
                for (i = 0; i < 3; ++i) {
                    for (j = 0; j < 3; ++j) {
                        ofs_sym << std::setw(4) << (*p).rot[i][j];
                    }
                }
                ofs_sym << "  ";
                for (i = 0; i < 3; ++i) {
                    ofs_sym << std::setprecision(15) << std::setw(20) << (*p).tran[i];
                }
                ofs_sym << std::endl;
            }

            ofs_sym.close();
        }

    } else if (nsym == 1) {

        // Identity operation only !

        std::cout << "  NSYM = 1 : Only the identity matrix will be considered." << std::endl << std::endl;

        int rot_tmp[3][3];
        double tran_tmp[3];

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                if (i == j) {
                    rot_tmp[i][j] = 1;
                } else {
                    rot_tmp[i][j] = 0;
                }
            }
            tran_tmp[i] = 0.0;
        }

        SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));

    } else {

        std::cout << "  NSYM > 1 : Symmetry operations will be read from SYMM_INFO file" << std::endl << std::endl;

        int nsym2;
        int rot_tmp[3][3];
        double tran_tmp[3];
        std::ifstream ifs_sym;

        ifs_sym.open(file_sym.c_str(), std::ios::in);
        ifs_sym >> nsym2;

        if (nsym != nsym2) error->exit("setup_symmetry_operations",
            "nsym in the given file and the input file are not consistent.");

        for (i = 0; i < nsym; ++i) {
            ifs_sym >> rot_tmp[0][0] >> rot_tmp[0][1] >> rot_tmp[0][2]
            >> rot_tmp[1][0] >> rot_tmp[1][1] >> rot_tmp[1][2]
            >> rot_tmp[2][0] >> rot_tmp[2][1] >> rot_tmp[2][2]
            >> tran_tmp[0] >> tran_tmp[1] >> tran_tmp[2];

            SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));
        }
        ifs_sym.close();
    }

#ifdef _DEBUG
    print_symmetrized_coordinate(x);
#endif
}

void Symmetry::findsym(int nat, double aa[3][3], double **x, std::vector<SymmetryOperation> &symop_all) {

    std::vector<RotationMatrix> LatticeSymmList;

    // Generate rotational matrices that don't change the metric tensor
    LatticeSymmList.clear();
    find_lattice_symmetry(aa, LatticeSymmList);

    // Generate all the space group operations with translational vectors
    symop_all.clear();
    find_crystal_symmetry(nat, system->nclassatom, system->atomlist_class, x,
        LatticeSymmList, symop_all);

    LatticeSymmList.clear();
}

void Symmetry::find_lattice_symmetry(double aa[3][3], std::vector<RotationMatrix> &LatticeSymmList) {

    /*
    Find the rotational matrices that leave the metric tensor invariant.

    Metric tensor G = (g)_{ij} = a_{i} * a_{j} is invariant under crystal symmetry operations T,
    i.e. T^{t}GT = G. Since G can be written as G = A^{t}A, the invariance condition is given by
    (AT)^{t}(AT) = G0 (original).
    */

    int i, j, k;
    int m11, m12, m13, m21, m22, m23, m31, m32, m33;

    int nsym_tmp = 0;
    int mat_tmp[3][3];
    double det, res;
    double rot_tmp[3][3];
    double aa_rot[3][3];

    double metric_tensor[3][3];
    double metric_tensor_rot[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            metric_tensor[i][j] = 0.0;
            for (k = 0; k < 3; ++k) {
                metric_tensor[i][j] += aa[k][i] * aa[k][j];
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                mat_tmp[i][i] = 1;
            } else {
                mat_tmp[i][j] = 0;
            }
        }
    }

    // Identity matrix should be the first entry.
    LatticeSymmList.push_back(mat_tmp);

    for (m11 = -1; m11 <= 1; ++m11){
        for (m12 = -1; m12 <= 1; ++m12) {
            for (m13 = -1; m13 <= 1; ++m13){
                for (m21 = -1; m21 <= 1; ++m21){
                    for (m22 = -1; m22 <= 1; ++m22){
                        for (m23 = -1; m23 <= 1; ++m23){
                            for (m31 = -1; m31 <= 1; ++m31){
                                for (m32 = -1; m32 <= 1; ++m32){
                                    for (m33 = -1; m33 <= 1; ++m33){

                                        if (m11 == 1 && m12 == 0 && m13 == 0 &&
                                            m21 == 0 && m22 == 1 && m23 == 0 &&
                                            m31 == 0 && m32 == 0 && m33 == 1) continue;

                                        det = m11 * (m22 * m33 - m32 * m23)
                                            - m21 * (m12 * m33 - m32 * m13)
                                            + m31 * (m12 * m23 - m22 * m13);

                                        if (det != 1 && det != -1) continue;

                                        rot_tmp[0][0] = m11;
                                        rot_tmp[0][1] = m12;
                                        rot_tmp[0][2] = m13;
                                        rot_tmp[1][0] = m21;
                                        rot_tmp[1][1] = m22;
                                        rot_tmp[1][2] = m23;
                                        rot_tmp[2][0] = m31;
                                        rot_tmp[2][1] = m32;
                                        rot_tmp[2][2] = m33;

                                        // Here, aa_rot = aa * rot_tmp is correct.
                                        matmul3(aa_rot, aa, rot_tmp);

                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                metric_tensor_rot[i][j] = 0.0;
                                                for (k = 0; k < 3; ++k) {
                                                    metric_tensor_rot[i][j] += aa_rot[k][i] * aa_rot[k][j];
                                                }
                                            }
                                        }

                                        res = 0.0;
                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                res += std::pow(metric_tensor[i][j] - metric_tensor_rot[i][j], 2.0);
                                            }
                                        }

                                        // Metric tensor is invariant under symmetry operations.

                                        if (res < tolerance * tolerance) {
                                            ++nsym_tmp;
                                            for (i = 0; i < 3; ++i) {
                                                for (j = 0; j < 3; ++j) {
                                                    mat_tmp[i][j] = static_cast<int>(rot_tmp[i][j]);
                                                }
                                            }
                                            LatticeSymmList.push_back(mat_tmp);
                                        }

                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    if (LatticeSymmList.size() > 48) {
        error->exit("find_lattice_symmetry", "Number of lattice symmetry is larger than 48.");
    }
}

void Symmetry::find_crystal_symmetry(int nat, int nclass, std::vector<unsigned int> *atomclass, double **x,
                                     std::vector<RotationMatrix> LatticeSymmList,
                                     std::vector<SymmetryOperation> &CrystalSymmList)
{
    unsigned int i, j;
    unsigned int iat, jat, kat, lat;
    double x_rot[3];
    double rot[3][3], rot_tmp[3][3], rot_cart[3][3];
    double mag[3], mag_rot[3];
    double tran[3];
    double x_rot_tmp[3];
    double tmp[3];
    double diff;

    int rot_int[3][3];

    int ii, jj, kk;
    unsigned int itype;

    bool is_found;
    bool isok;
    bool mag_sym1, mag_sym2;

    bool is_identity_matrix;

    // Add identity matrix first.
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                rot_int[i][j] = 1;
            } else {
                rot_int[i][j] = 0;
            }
        }
        tran[i] = 0.0;
    }

    CrystalSymmList.push_back(SymmetryOperation(rot_int, tran));

    for (std::vector<RotationMatrix>::iterator it_latsym = LatticeSymmList.begin();
        it_latsym != LatticeSymmList.end(); ++it_latsym) {

            iat = atomclass[0][0];

            for (i = 0; i < 3; ++i) {
                for (j = 0; j < 3; ++j) {
                    rot[i][j] = static_cast<double>((*it_latsym).mat[i][j]);
                }
            }

            rotvec(x_rot, x[iat], rot);

#ifdef _OPENMP
#pragma omp parallel for private(jat, tran, isok, kat, x_rot_tmp, is_found, lat, tmp, diff, \
    i, j, itype, jj, kk, is_identity_matrix, mag, mag_rot, rot_tmp, rot_cart, mag_sym1, mag_sym2)
#endif
            for (ii = 0; ii < atomclass[0].size(); ++ii) {
                jat = atomclass[0][ii];

                for (i = 0; i < 3; ++i) {
                    tran[i] = x[jat][i] - x_rot[i];
                    tran[i] = tran[i] - nint(tran[i]);
                }

                if ((std::abs(tran[0]) > eps12 && !interaction->is_periodic[0]) ||
                    (std::abs(tran[1]) > eps12 && !interaction->is_periodic[1]) ||
                    (std::abs(tran[2]) > eps12 && !interaction->is_periodic[2])) continue;

                is_identity_matrix =
                    ( std::pow(rot[0][0] - 1.0, 2) + std::pow(rot[0][1], 2) + std::pow(rot[0][2], 2)
                    + std::pow(rot[1][0], 2) + std::pow(rot[1][1] - 1.0, 2) + std::pow(rot[1][2], 2)
                    + std::pow(rot[2][0], 2) + std::pow(rot[2][1], 2) + std::pow(rot[2][2] - 1.0, 2)
                    + std::pow(tran[0], 2) + std::pow(tran[1], 2) + std::pow(tran[2], 2) ) < eps12;
                if (is_identity_matrix) continue;

                isok = true;

                for (itype = 0; itype < nclass; ++itype) {

                    for (jj = 0; jj < atomclass[itype].size(); ++jj) {

                        kat = atomclass[itype][jj];

                        rotvec(x_rot_tmp, x[kat], rot);

                        for (i = 0; i < 3; ++i) {
                            x_rot_tmp[i] += tran[i];
                        }

                        is_found = false;

                        for (kk = 0; kk < atomclass[itype].size(); ++kk) {

                            lat = atomclass[itype][kk];

                            for (i = 0; i < 3; ++i) {
                                tmp[i] = std::fmod(std::abs(x[lat][i] - x_rot_tmp[i]), 1.0);
                                tmp[i] = std::min<double>(tmp[i], 1.0 - tmp[i]);
                            }
                            diff = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2];
                            if (diff < tolerance * tolerance) {
                                is_found = true;
                                break;
                            }
                        }

                        if (!is_found) isok = false;
                    }
                }

                if (isok && system->lspin && system->noncollinear) {
                    for (i = 0; i < 3; ++i) {
                        mag[i] = system->magmom[jat][i];
                        mag_rot[i] = system->magmom[iat][i];
                    }

                    matmul3(rot_tmp, rot, system->rlavec);
                    matmul3(rot_cart, system->lavec, rot_tmp);

                    for (i = 0; i < 3; ++i) {
                        for (j = 0; j < 3; ++j) {
                            rot_cart[i][j] /= (2.0 * pi);
                        }
                    }
                    rotvec(mag_rot, mag_rot, rot_cart);

                    // In the case of improper rotation, the factor -1 should be multiplied
                    // because the inversion operation doesn't flip the spin.
                    if (!is_proper(rot_cart)) {
                        for (i = 0; i < 3; ++i) {
                            mag_rot[i] = -mag_rot[i];
                        }
                    }

                    mag_sym1 = (std::pow(mag[0] - mag_rot[0], 2.0)
                        + std::pow(mag[1] - mag_rot[1], 2.0)
                        + std::pow(mag[2] - mag_rot[2], 2.0) ) < eps6;

                    mag_sym2 = (std::pow(mag[0] + mag_rot[0], 2.0)
                        + std::pow(mag[1] + mag_rot[1], 2.0)
                        + std::pow(mag[2] + mag_rot[2], 2.0) ) < eps6;

                    if (!mag_sym1 && !mag_sym2) {
                        isok = false;
                    } else if (!mag_sym1 && mag_sym2 && !trev_sym_mag) {
                        isok = false;
                    }
                }

                if (isok) {
#ifdef _OPENMP
#pragma omp critical
#endif
                    CrystalSymmList.push_back(SymmetryOperation((*it_latsym).mat, tran));
                }
            }

    }
}

/*
void Symmetry::find_nnp_for_translation(unsigned int &ret, std::vector<SymmetryOperationTransFloat> symminfo)
{
int i;

ret = 1;

std::set<double> translation_set;
double tran_tmp;
double tran_numerator;
bool is_integer;
bool is_found;

translation_set.clear();

for (std::vector<SymmetryOperationTransFloat>::iterator it = symminfo.begin(); it != symminfo.end(); ++it) {

for (i = 0; i < 3; ++i) {
tran_tmp = std::abs((*it).tran[i]);

if (translation_set.find(tran_tmp) == translation_set.end()) {
translation_set.insert(tran_tmp);
}
}
}

is_found = false;

while(1) {

is_integer = true;

for (std::set<double>::iterator it = translation_set.begin(); it != translation_set.end(); ++it) {

tran_numerator = (*it) * static_cast<double>(ret);
if (std::abs(tran_numerator - static_cast<double>(nint(tran_numerator))) > tolerance) {
is_integer = false;
break;
}
}

if (is_integer) {
is_found = true;
break;
}

if (ret > 1000) break;
++ret;
}

if (!is_found) {
// This should not happen.
error->exit("find_nnp_for_translation", "Cannot find nnp.");
}
}
*/

void Symmetry::symop_in_cart(double lavec[3][3], double rlavec[3][3])
{
    int i, j;

#ifdef _USE_EIGEN
    Eigen::Matrix3d aa, bb, sym_tmp;
    Eigen::Matrix3d sym_crt;

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            aa(i,j) = lavec[i][j];
            bb(i,j) = rlavec[i][j];
        }
    }

#else
    double sym_tmp[3][3], sym_crt[3][3];
    double tmp[3][3];
#endif

    for (int isym = 0; isym < nsym; ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                sym_tmp(i,j) = static_cast<double>(symrel_int[isym][i][j]);
#else
                sym_tmp[i][j] = static_cast<double>(symrel_int[isym][i][j]);
#endif
            }
        }
#ifdef _USE_EIGEN
        sym_crt = (aa * (sym_tmp * bb)) / (2.0 * pi);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel[isym][i][j] = sym_crt(i,j);
            }
        }
#else
        matmul3(tmp, sym_tmp, rlavec);
        matmul3(sym_crt, lavec, tmp);

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                symrel[isym][i][j] = sym_crt[i][j] / (2.0 * pi);
            }
        }
#endif
    }

#ifdef _DEBUG

    std::cout << "Symmetry Operations in Cartesian Coordinate" << std::endl;
    for (int isym = 0; isym < nsym; ++isym) {
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(8) << symrel[isym][i][j];
            }
        }
        std::cout << std::endl;
    }
#endif
}

void Symmetry::pure_translations()
{
    int i;

    ntran = 0;
    for (i = 0; i < nsym; ++i) {
        if (is_translation(symrel_int[i])) ++ntran;
    }

    natmin = system->nat / ntran;

    if (ntran > 1) {
        std::cout << "  Given system is not primitive cell." << std::endl;
        std::cout << "  There are " << std::setw(5) <<  ntran << " translation operations." << std::endl;
    } else {
        std::cout << "  Given system is a primitive cell." << std::endl;
    }
    std::cout << "  Primitive cell contains " << natmin << " atoms" << std::endl;

    if (system->nat % ntran) {
        error->exit("pure_translations", "nat != natmin * ntran. Something is wrong in the structure.");
    }

    memory->allocate(symnum_tran, ntran);

    int isym = 0;

    for (i = 0; i < nsym; ++i) {
        if (is_translation(symrel_int[i])) symnum_tran[isym++] = i;
    }
}

void Symmetry::genmaps(int nat, double **x, int **map_sym, int **map_p2s, Maps *map_s2p)
{
    int isym, iat, jat;
    int i, j;
    int itype;
    int ii, jj;
    double xnew[3];
    double tmp[3], diff;
    double rot_double[3][3];

    for (iat = 0; iat < nat; ++iat) {
        for (isym = 0; isym < nsym; ++isym) {
            map_sym[iat][isym] = -1;
        }
    }

#ifdef  _OPENMP
#pragma omp parallel for private(i, j, rot_double, itype, ii, iat, xnew, jj, jat, tmp, diff, isym)
#endif
    for (isym = 0; isym < nsym; ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                rot_double[i][j] = static_cast<double>(symrel_int[isym][i][j]);
            }
        }

        for (itype = 0; itype < system->nclassatom; ++itype) {

            for (ii = 0; ii < system->atomlist_class[itype].size(); ++ii) {

                iat = system->atomlist_class[itype][ii];

                rotvec(xnew, x[iat], rot_double);

                for (i = 0; i < 3; ++i) xnew[i] += tnons[isym][i];

                for (jj = 0; jj < system->atomlist_class[itype].size(); ++jj) {

                    jat = system->atomlist_class[itype][jj];

                    for (i = 0; i < 3; ++i) {
                        tmp[i] = std::fmod(std::abs(x[jat][i] - xnew[i]), 1.0);
                        tmp[i] = std::min<double>(tmp[i], 1.0 - tmp[i]);
                    }
                    diff = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2];
                    if (diff < tolerance * tolerance) {
                        map_sym[iat][isym] = jat;
                        break;
                    }
                }
                if (map_sym[iat][isym] == -1) error->exit("genmaps", "cannot find symmetry for operation # ", isym + 1);
            }
        }
    }

    bool *is_checked;
    memory->allocate(is_checked, nat);

    for (i = 0; i < nat; ++i) is_checked[i] = false;

    jat = 0;
    int atomnum_translated;
    for (iat = 0; iat < nat; ++iat) {

        if (is_checked[iat]) continue;
        for (i = 0; i < ntran; ++i) {
            atomnum_translated = map_sym[iat][symnum_tran[i]];
            map_p2s[jat][i] = atomnum_translated;
            is_checked[atomnum_translated] = true;
        }
        ++jat;
    }

    memory->deallocate(is_checked);

    for (iat = 0; iat < natmin; ++iat) {
        for (i  = 0; i < ntran; ++i) {
            atomnum_translated = map_p2s[iat][i];
            map_s2p[atomnum_translated].atom_num = iat;
            map_s2p[atomnum_translated].tran_num = i;
        }
    }
}

bool Symmetry::is_translation(int **rot)
{
    bool ret;

    ret =
        rot[0][0] == 1 && rot[0][1] == 0 && rot[0][2] == 0 &&
        rot[1][0] == 0 && rot[1][1] == 1 && rot[1][2] == 0 &&
        rot[2][0] == 0 && rot[2][1] == 0 && rot[2][2] == 1;

    return ret;
}

void Symmetry::symop_availability_check(double ***rot, bool *flag, const int n, int &nsym_fc)
{
    int i, j, k;
    int nfinite;

    nsym_fc = 0;

    for (i = 0; i < nsym; ++i) {

        nfinite = 0;
        for (j = 0; j < 3; ++j) {
            for (k = 0; k < 3; ++k) {
                if(std::abs(rot[i][j][k]) > eps) ++nfinite;
            }
        }

        if (nfinite == 3) {
            ++nsym_fc;
            flag[i] = true;
        } else {
            flag[i] = false;
        }
    }
}

void Symmetry::print_symmetrized_coordinate(double **x)
{
    int i, j, k, l;
    int isym = 0;
    int nat = system->nat;
    int m11, m12, m13, m21, m22, m23, m31, m32, m33;
    int det;
    double tran[3];
    double **x_symm, **x_avg;
#ifdef _USE_EIGEN
    Eigen::Matrix3d rot;
    Eigen::Vector3d wsi, usi, vsi, tmp;
#else
    double rot[3][3];
    double wsi[3], usi[3], vsi[3], tmp[3];
#endif

    memory->allocate(x_symm, nat, 3);
    memory->allocate(x_avg, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_avg[i][j] = 0.0;
        }
    }

    for (std::vector<SymmetryOperation>::iterator it = SymmList.begin(); it != SymmList.end(); ++it) {

        ++isym;
        std::cout << "Symmetry No. : " << std::setw(5) << isym << std::endl;

        m11 = (*it).rot[0][0];
        m12 = (*it).rot[0][1];
        m13 = (*it).rot[0][2];
        m21 = (*it).rot[1][0];
        m22 = (*it).rot[1][1];
        m23 = (*it).rot[1][2];
        m31 = (*it).rot[2][0];
        m32 = (*it).rot[2][1];
        m33 = (*it).rot[2][2];

        for (i = 0; i < 3; ++i) tran[i] = (*it).tran[i];

        det = m11 * (m22 * m33 - m32 * m23)
            - m21 * (m12 * m33 - m32 * m13)
            + m31 * (m12 * m23 - m22 * m13);

#ifdef _USE_EIGEN
        rot(0,0) = static_cast<double>((m22 * m33 - m23 * m32) * det);
        rot(0,1) = static_cast<double>((m23 * m31 - m21 * m33) * det);
        rot(0,2) = static_cast<double>((m21 * m32 - m22 * m31) * det);
        rot(1,0) = static_cast<double>((m32 * m13 - m33 * m12) * det);
        rot(1,1) = static_cast<double>((m33 * m11 - m31 * m13) * det);
        rot(1,2) = static_cast<double>((m31 * m12 - m32 * m11) * det);
        rot(2,0) = static_cast<double>((m12 * m23 - m13 * m22) * det);
        rot(2,1) = static_cast<double>((m13 * m21 - m11 * m23) * det);
        rot(2,2) = static_cast<double>((m11 * m22 - m12 * m21) * det);
#else
        rot[0][0] = static_cast<double>((m22 * m33 - m23 * m32) * det);
        rot[0][1] = static_cast<double>((m23 * m31 - m21 * m33) * det);
        rot[0][2] = static_cast<double>((m21 * m32 - m22 * m31) * det);
        rot[1][0] = static_cast<double>((m32 * m13 - m33 * m12) * det);
        rot[1][1] = static_cast<double>((m33 * m11 - m31 * m13) * det);
        rot[1][2] = static_cast<double>((m31 * m12 - m32 * m11) * det);
        rot[2][0] = static_cast<double>((m12 * m23 - m13 * m22) * det);
        rot[2][1] = static_cast<double>((m13 * m21 - m11 * m23) * det);
        rot[2][2] = static_cast<double>((m11 * m22 - m12 * m21) * det);
#endif

        for (i = 0; i < nat; ++i) {
            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                wsi(j) = x[i][j] - tran[j];
#else
                wsi[j] = x[i][j] - tran[j];
#endif
            }

#ifdef _USE_EIGEN
            usi = rot * wsi;
#else
            rotvec(usi, wsi, rot);
#endif

            l = -1;

            for (j = 0; j < nat; ++j) {
                for (k = 0; k < 3; ++k) {
#ifdef _USE_EIGEN
                    vsi(k) = x[j][k];
                    tmp(k) = std::fmod(std::abs(usi(k) - vsi(k)), 1.0);
                    // need "std" to specify floating point operation
                    // especially for Intel compiler (there was no problem in MSVC)
                    tmp(k) = std::min<double>(tmp(k), 1.0 - tmp(k)) ;
#else
                    vsi[k] = x[j][k];
                    tmp[k] = std::fmod(std::abs(usi[k] - vsi[k]), 1.0);
                    tmp[k] = std::min<double>(tmp[k], 1.0 - tmp[k]);
#endif
                }
#ifdef _USE_EIGEN
                double diff = tmp.dot(tmp);
#else
                double diff = tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2];
#endif
                if (diff < tolerance * tolerance) {
                    l = j;
                    break;
                }
            }
            if (l == -1) error->exit("print_symmetrized_coordinate", "This cannot happen.");

            for (j = 0; j < 3; ++j) {
#ifdef _USE_EIGEN
                x_symm[l][j] = usi(j);
#else
                x_symm[l][j] = usi[j];
#endif
                /*
                do {
                if (x_symm[l][j] < 0.0) {
                x_symm[l][j] += 1.0;
                } else if (x_symm[l][j] >= 1.0) {
                x_symm[l][j] -= 1.0;
                }
                } while(x_symm[l][j] < 0.0 || x_symm[l][j] >= 1.0);
                */
            }

        }

        for (i = 0; i < nat; ++i) {
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(20) << std::scientific << x_symm[i][j];
            }
            std::cout << " ( ";
            for (j = 0; j < 3; ++j) {
                std::cout << std::setw(20) << std::scientific << x_symm[i][j]-x[i][j];
            }
            std::cout << " )" << std::endl;

            for (j = 0; j < 3; ++j) {
                x_avg[i][j] += x_symm[i][j];
            }
        }

    }

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_avg[i][j] /= static_cast<double>(SymmList.size());
        }
    }

    std::cout << "Symmetry Averaged Coordinate" << std::endl;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            std::cout << std::setw(20) << std::scientific << std::setprecision(9) << x_avg[i][j];
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;

    std::cout.setf(std::ios::floatfield);

    memory->deallocate(x_symm);
    memory->deallocate(x_avg);
}

bool Symmetry::is_proper(double rot[3][3])
{
    double det;
    bool ret;

    det = rot[0][0] * (rot[1][1] * rot[2][2] - rot[2][1] * rot[1][2])
        - rot[1][0] * (rot[0][1] * rot[2][2] - rot[2][1] * rot[0][2])
        + rot[2][0] * (rot[0][1] * rot[1][2] - rot[1][1] * rot[0][2]);

    if (std::abs(det - 1.0) < eps12) {
        ret = true;
    } else if (std::abs(det + 1.0) < eps12) {
        ret = false;
    } else {
        error->exit("is_proper", "This cannot happen.");
    }

    return ret;
}


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Implementation of Kalman filtering accounting for unknown inputs.
// Please refer to: https://hal.archives-ouvertes.fr/hal-00143941/document
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/scalar_output_smoother.h>
#include <quads/types.h>

#include <Eigen/QR>
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>

namespace quads {

void ScalarOutputSmoother::Update(double y, double dt) {
  // Discretize time.
  const Matrix4x4d A_dt = (A_ * dt).exp();

  // Predict step.
  const Vector4d x_predict = A_dt * x_;
  const Matrix4x4d Px_predict = A_dt * Px_ * A_dt.transpose() + W_ * dt;

  // Update step.
  const double innovation = y - H_ * x_predict;
  const double S = V_ + H_ * Px_predict * H_.transpose();
  const Vector4d K = Px_predict * H_.transpose() / S;

  x_ = x_predict + K * innovation;
  Px_ = (Matrix4d::Identity() - K * H_) * Px_predict;
}

}  // namespace quads