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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#define BOOST_GEOMETRY_NO_BOOST_TEST

#include <iostream>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>

#include <boost/geometry/extensions/triangulation/strategies/cartesian/side_robust.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expressions.hpp>
#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/stage_a.hpp>

namespace bg = boost::geometry;
using point = bg::model::point<double, 2, bg::cs::cartesian>;

template <typename CalculationType>
struct side_robust_with_static_filter
{
private:
    using ct = CalculationType;
    using expression = bg::detail::generic_robust_predicates::orient2d;
    using filter = bg::detail::generic_robust_predicates::stage_a_static
            <
                expression,
                ct
            >;
    filter m_filter;
public:
    side_robust_with_static_filter(ct x_max, ct y_max, ct x_min, ct y_min)
        : m_filter(x_max, y_max, x_max, y_max, x_max, y_max,
                   x_min, y_min, x_min, y_min, x_min, y_min) {};

    template
    <
        typename P1,
        typename P2,
        typename P
    >
    inline int apply(P1 const& p1, P2 const& p2, P const& p) const
    {
        int sign = m_filter.apply(bg::get<0>(p1),
                                  bg::get<1>(p1),
                                  bg::get<0>(p2),
                                  bg::get<1>(p2),
                                  bg::get<0>(p),
                                  bg::get<1>(p));
        if(sign != bg::detail::generic_robust_predicates::sign_uncertain)
        {
            return sign;
        }
        else
        {
            //fallback if filter fails.
            return bg::strategy::side::side_robust<double>::apply(p1, p2, p);
        }
    }
};

int main(int argc, char** argv)
{
    point p1(0.0, 0.0);
    point p2(1.0, 1.0);
    point p (0.0, 1.0);
    side_robust_with_static_filter<double> static_strategy(2.0, 2.0, 1.0, 1.0);
    std::cout << "Side value: " << static_strategy.apply(p1, p2, p) << "\n";
    return 0;
}


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

#ifndef SMOOTH__INTERNAL__SE3_HPP_
#define SMOOTH__INTERNAL__SE3_HPP_

#include <Eigen/Core>

#include "common.hpp"
#include "smooth/derivatives.hpp"
#include "so3.hpp"

namespace smooth {

/**
 * @brief SE(3) Lie Group represented as S^3 ⋉ R3
 *
 * Memory layout
 * -------------
 * Group:    x y z qx qy qz qw
 * Tangent:  vx vy vz Ωx Ωy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ R T ]
 * [ 0 1 ]
 *
 * where R ∈ SO(3) and T = [x y z] ∈ R3
 *
 * Lie algebra Matrix form
 * -----------------------
 * [  0 -Ωz  Ωy vx]
 * [  Ωz  0 -Ωx vy]
 * [ -Ωy Ωx   0 vz]
 * [   0  0   0  0]
 *
 * Constraints
 * -----------
 * Group:   qx * qx + qy * qy + qz * qz + qw * qw = 1
 * Tangent: -pi < Ωx Ωy Ωz <= pi
 */
template<typename _Scalar>
class SE3Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 7;
  static constexpr Eigen::Index Dim     = 4;
  static constexpr Eigen::Index Dof     = 6;
  static constexpr bool IsCommutative   = false;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out)
  {
    g_out.template head<6>().setZero();
    g_out(6) = Scalar(1);
  }

  static void setRandom(GRefOut g_out)
  {
    g_out.template head<3>().setRandom();
    SO3Impl<Scalar>::setRandom(g_out.template tail<4>());
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), m_out.template topLeftCorner<3, 3>());
    m_out.template topRightCorner<3, 1>() = g_in.template head<3>();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    SO3Impl<Scalar>::composition(
      g_in1.template tail<4>(), g_in2.template tail<4>(), g_out.template tail<4>());
    Eigen::Matrix<Scalar, 3, 3> R1;
    SO3Impl<Scalar>::matrix(g_in1.template tail<4>(), R1);
    g_out.template head<3>().noalias() = R1 * g_in2.template head<3>() + g_in1.template head<3>();
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Matrix<Scalar, 4, 1> so3inv;
    SO3Impl<Scalar>::inverse(g_in.template tail<4>(), so3inv);

    Eigen::Matrix<Scalar, 3, 3> Rinv;
    SO3Impl<Scalar>::matrix(so3inv, Rinv);

    g_out.template head<3>().noalias() = -Rinv * g_in.template head<3>();
    g_out.template tail<4>()           = so3inv;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::log(g_in.template tail<4>(), a_out.template tail<3>());

    SO3TangentMap M_dr_expinv, M_ad;
    SO3Impl<Scalar>::dr_expinv(a_out.template tail<3>(), M_dr_expinv);
    SO3Impl<Scalar>::ad(a_out.template tail<3>(), M_ad);
    a_out.template head<3>().noalias() = (-M_ad + M_dr_expinv) * g_in.template head<3>();
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(g_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template topRightCorner<3, 3>() *= A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::exp(a_in.template tail<3>(), g_out.template tail<4>());

    SO3TangentMap M_dr_exp, M_Ad;
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), M_dr_exp);
    SO3Impl<Scalar>::Ad(g_out.template tail<4>(), M_Ad);

    g_out.template head<3>().noalias() = M_Ad * M_dr_exp * a_in.template head<3>();
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 1>() = a_in.template head<3>();
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    SO3Impl<Scalar>::vee(A_in.template topLeftCorner<3, 3>(), a_out.template tail<3>());
    a_out.template head<3>() = A_in.template topRightCorner<3, 1>();
  }

  static void ad(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(a_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static Eigen::Matrix<Scalar, 3, 3> calculate_q(TRefIn a)
  {
    using std::abs, std::sqrt, std::cos, std::sin;

    const Scalar th2 = a.template tail<3>().squaredNorm();

    const auto [A, B, C] = [&]() -> std::array<Scalar, 3> {
      if (th2 < Scalar(eps2)) {
        return {
          // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x%29+%2F+x%5E3+at+x%3D0
          Scalar(1) / Scalar(6) - th2 / Scalar(120),
          // https://www.wolframalpha.com/input/?i=series+%28cos+x+-+1+%2B+x%5E2%2F2%29+%2F+x%5E4+at+x%3D0
          Scalar(1) / Scalar(24) - th2 / Scalar(720),
          // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x+-+x%5E3%2F6%29+%2F+x%5E5+at+x%3D0
          -Scalar(1) / Scalar(120) + th2 / Scalar(5040),
        };
      } else {
        const Scalar th = sqrt(th2), th_4 = th2 * th2, cTh = cos(th), sTh = sin(th);
        return {
          (th - sTh) / (th * th2),
          (cTh - Scalar(1) + th2 / Scalar(2)) / th_4,
          (th - sTh - th * th2 / Scalar(6)) / (th_4 * th),
        };
      }
    }();

    Eigen::Matrix<Scalar, 3, 3> V, W;
    SO3Impl<Scalar>::hat(a.template head<3>(), V);
    SO3Impl<Scalar>::hat(a.template tail<3>(), W);

    const Scalar vdw                     = a.template tail<3>().dot(a.template head<3>());
    const Eigen::Matrix<Scalar, 3, 3> WV = W * V, VW = V * W, WW = W * W;

    // clang-format off
    return Scalar(0.5) * V + A * (WV + VW - vdw * W) + B * (W * WV + VW * W + vdw * (3 * W - WW)) - C * 3 * vdw * WW;
    // clang-format on
  }

  static std::pair<Eigen::Matrix3<Scalar>, Eigen::Matrix<Scalar, 3, 18>> calculate_Q_dQ(TRefIn a)
  {
    const Eigen::Vector3<Scalar> v = a.template head<3>();
    const Eigen::Vector3<Scalar> w = a.template tail<3>();
    const Scalar th2               = w.squaredNorm();

    const auto [A, B, C, dA_over_th, dB_over_th, dC_over_th] = [&]() -> std::array<Scalar, 6> {
      if (th2 < Scalar(eps2)) {
        return {
          Scalar(1) / Scalar(6) - th2 / Scalar(120),
          Scalar(1) / Scalar(24) - th2 / Scalar(720),
          -Scalar(1) / Scalar(120) + th2 / Scalar(5040),
          -Scalar(1) / 60,
          -Scalar(1) / 360,
          Scalar(1) / 2520,
        };
      } else {
        const Scalar th  = sqrt(th2);
        const Scalar th3 = th2 * th;
        const Scalar th4 = th2 * th2;
        const Scalar th5 = th3 * th2;
        const Scalar th6 = th3 * th3;
        const Scalar th7 = th4 * th3;
        const Scalar sTh = sin(th);
        const Scalar cTh = cos(th);
        return {
          (th - sTh) / (th3),
          (cTh - Scalar(1) + th2 / Scalar(2)) / th4,
          (th - sTh - th * th2 / Scalar(6)) / th5,
          -cTh / th4 - 2 / th4 + 3 * sTh / th5,
          -1 / th4 - sTh / th5 - 4 * cTh / th6 + 4 / th6,
          1 / (3 * th4) - cTh / th6 - 4 / th6 + 5 * sTh / th7,
        };
      }
    }();

    Eigen::Matrix<Scalar, 3, 3> V, W;
    SO3Impl<Scalar>::hat(a.template head<3>(), V);
    SO3Impl<Scalar>::hat(a.template tail<3>(), W);
    const Scalar vdw = v.dot(w);

    const Eigen::Matrix3<Scalar> WV = W * V, VW = V * W, WW = W * W, PA = WV + VW - vdw * W,
                                 PB = W * WV + VW * W + vdw * (3 * W - WW), PC = -3 * vdw * WW;

    Eigen::Matrix3<Scalar> Q = V / 2 + A * PA + B * PB + C * PC;

    // part with derivatives from matrices
    // clang-format off
    Eigen:: Matrix<Scalar, 3, 18> dQ {{ w.x()*(B + 3*C)*(w.y()*w.y() + w.z()*w.z()),
      w.y()*(-2*A + B*(w.y()*w.y() + w.z()*w.z()) + 3*C*(w.y()*w.y() + w.z()*w.z())),
      w.z()*(-2*A + B*(w.y()*w.y() + w.z()*w.z()) + 3*C*(w.y()*w.y() + w.z()*w.z())),
      v.x()*(B + 3*C)*(w.y()*w.y() + w.z()*w.z()),
      -2*A*v.y() + B*v.y()*(w.y()*w.y() + w.z()*w.z()) + 2*B*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.y()*w.y()*w.y() + v.y()*w.z()*w.z() + 2*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      -2*A*v.z() + B*v.z()*(w.y()*w.y() + w.z()*w.z()) + 2*B*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.z()*w.y()*w.y() + v.z()*w.z()*w.z() + 2*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      -A*(w.x()*w.z() - w.y()) - B*w.x()*(w.x()*w.y() - 2*w.z()) - 3*C*w.x()*w.x()*w.y(),
      A*(w.x() - w.y()*w.z()) - B*w.y()*(w.x()*w.y() - 2*w.z()) - 3*C*w.x()*w.y()*w.y(),
      -A*w.z()*w.z() - B*w.x()*w.x() - B*w.x()*w.y()*w.z() - B*w.y()*w.y() + B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() + Scalar(0.5),
      -A*(v.x()*w.z() - v.y()) - B*(v.x()*w.z() + v.x()*(w.x()*w.y() - 3*w.z()) + 2*v.z()*w.x() + w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.x()*w.x()*w.y() - 3*C*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.x() - v.y()*w.z()) - B*(v.y()*w.z() + v.y()*(w.x()*w.y() - 3*w.z()) + 2*v.z()*w.y() + w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.y()*w.x()*w.y() - 3*C*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      -A*(v.x()*w.x() + v.y()*w.y() + 2*v.z()*w.z()) + B*(2*v.x()*w.x() + 2*v.y()*w.y() - v.z()*w.z() - v.z()*(w.x()*w.y() - 3*w.z())) - 3*C*v.z()*w.x()*w.y(),
      A*(w.x()*w.y() + w.z()) - B*w.x()*(w.x()*w.z() + 2*w.y()) - 3*C*w.x()*w.x()*w.z(),
      A*w.y()*w.y() + B*w.x()*w.x() - B*w.x()*w.y()*w.z() - B*w.y()*w.y() + B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() - Scalar(0.5),
      A*(w.x() + w.y()*w.z()) - B*w.z()*(w.x()*w.z() + 2*w.y()) - 3*C*w.x()*w.z()*w.z(),
      A*(v.x()*w.y() + v.z()) + B*(v.x()*w.y() - v.x()*(w.x()*w.z() + 3*w.y()) + 2*v.y()*w.x() - w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.x()*w.x()*w.z() - 3*C*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.x()*w.x() + 2*v.y()*w.y() + v.z()*w.z()) - B*(2*v.x()*w.x() - v.y()*w.y() + v.y()*(w.x()*w.z() + 3*w.y()) + 2*v.z()*w.z()) - 3*C*v.y()*w.x()*w.z(),
      A*(v.x() + v.z()*w.y()) + B*(2*v.y()*w.z() + v.z()*w.y() - v.z()*(w.x()*w.z() + 3*w.y()) - w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.z()*w.x()*w.z() - 3*C*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())
      }, {
      A*(w.x()*w.z() + w.y()) - B*w.x()*(w.x()*w.y() + 2*w.z()) - 3*C*w.x()*w.x()*w.y(),
      A*(w.x() + w.y()*w.z()) - B*w.y()*(w.x()*w.y() + 2*w.z()) - 3*C*w.x()*w.y()*w.y(),
      A*w.z()*w.z() + B*w.x()*w.x() - B*w.x()*w.y()*w.z() + B*w.y()*w.y() - B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() - Scalar(0.5),
      A*(v.x()*w.z() + v.y()) + B*(v.x()*w.z() - v.x()*(w.x()*w.y() + 3*w.z()) + 2*v.z()*w.x() - w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.x()*w.x()*w.y() - 3*C*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.x() + v.y()*w.z()) + B*(v.y()*w.z() - v.y()*(w.x()*w.y() + 3*w.z()) + 2*v.z()*w.y() - w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.y()*w.x()*w.y() - 3*C*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.x()*w.x() + v.y()*w.y() + 2*v.z()*w.z()) - B*(2*v.x()*w.x() + 2*v.y()*w.y() - v.z()*w.z() + v.z()*(w.x()*w.y() + 3*w.z())) - 3*C*v.z()*w.x()*w.y(),
      w.x()*(-2*A + B*(w.x()*w.x() + w.z()*w.z()) + 3*C*(w.x()*w.x() + w.z()*w.z())),
      w.y()*(B + 3*C)*(w.x()*w.x() + w.z()*w.z()),
      w.z()*(-2*A + B*(w.x()*w.x() + w.z()*w.z()) + 3*C*(w.x()*w.x() + w.z()*w.z())),
      -2*A*v.x() + B*v.x()*(w.x()*w.x() + w.z()*w.z()) + 2*B*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.x()*w.x()*w.x() + v.x()*w.z()*w.z() + 2*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      v.y()*(B + 3*C)*(w.x()*w.x() + w.z()*w.z()),
      -2*A*v.z() + B*v.z()*(w.x()*w.x() + w.z()*w.z()) + 2*B*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.z()*w.x()*w.x() + v.z()*w.z()*w.z() + 2*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      -A*w.x()*w.x() + B*w.x()*w.x() - B*w.x()*w.y()*w.z() - B*w.y()*w.y() - B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() + Scalar(0.5),
      -A*(w.x()*w.y() - w.z()) + B*w.y()*(2*w.x() - w.y()*w.z()) - 3*C*w.y()*w.y()*w.z(),
      -A*(w.x()*w.z() - w.y()) + B*w.z()*(2*w.x() - w.y()*w.z()) - 3*C*w.y()*w.z()*w.z(),
      -A*(2*v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + B*(-v.x()*w.x() + v.x()*(3*w.x() - w.y()*w.z()) + 2*v.y()*w.y() + 2*v.z()*w.z()) - 3*C*v.x()*w.y()*w.z(),
      -A*(v.y()*w.x() - v.z()) - B*(2*v.x()*w.y() + v.y()*w.x() - v.y()*(3*w.x() - w.y()*w.z()) + w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.y()*w.y()*w.z() - 3*C*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.y() - v.z()*w.x()) - B*(2*v.x()*w.z() + v.z()*w.x() - v.z()*(3*w.x() - w.y()*w.z()) + w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.z()*w.y()*w.z() - 3*C*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())
      }, {
      -A*(w.x()*w.y() - w.z()) - B*w.x()*(w.x()*w.z() - 2*w.y()) - 3*C*w.x()*w.x()*w.z(),
      -A*w.y()*w.y() - B*w.x()*w.x() - B*w.x()*w.y()*w.z() + B*w.y()*w.y() - B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() + Scalar(0.5),
      A*(w.x() - w.y()*w.z()) - B*w.z()*(w.x()*w.z() - 2*w.y()) - 3*C*w.x()*w.z()*w.z(),
      -A*(v.x()*w.y() - v.z()) - B*(v.x()*w.y() + v.x()*(w.x()*w.z() - 3*w.y()) + 2*v.y()*w.x() + w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.x()*w.x()*w.z() - 3*C*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      -A*(v.x()*w.x() + 2*v.y()*w.y() + v.z()*w.z()) + B*(2*v.x()*w.x() - v.y()*w.y() - v.y()*(w.x()*w.z() - 3*w.y()) + 2*v.z()*w.z()) - 3*C*v.y()*w.x()*w.z(),
      A*(v.x() - v.z()*w.y()) - B*(2*v.y()*w.z() + v.z()*w.y() + v.z()*(w.x()*w.z() - 3*w.y()) + w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.z()*w.x()*w.z() - 3*C*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*w.x()*w.x() - B*w.x()*w.x() - B*w.x()*w.y()*w.z() + B*w.y()*w.y() + B*w.z()*w.z() - 3*C*w.x()*w.y()*w.z() - Scalar(0.5),
      A*(w.x()*w.y() + w.z()) - B*w.y()*(2*w.x() + w.y()*w.z()) - 3*C*w.y()*w.y()*w.z(),
      A*(w.x()*w.z() + w.y()) - B*w.z()*(2*w.x() + w.y()*w.z()) - 3*C*w.y()*w.z()*w.z(),
      A*(2*v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) - B*(-v.x()*w.x() + v.x()*(3*w.x() + w.y()*w.z()) + 2*v.y()*w.y() + 2*v.z()*w.z()) - 3*C*v.x()*w.y()*w.z(),
      A*(v.y()*w.x() + v.z()) + B*(2*v.x()*w.y() + v.y()*w.x() - v.y()*(3*w.x() + w.y()*w.z()) - w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.y()*w.y()*w.z() - 3*C*w.z()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      A*(v.y() + v.z()*w.x()) + B*(2*v.x()*w.z() + v.z()*w.x() - v.z()*(3*w.x() + w.y()*w.z()) - w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())) - 3*C*v.z()*w.y()*w.z() - 3*C*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()),
      w.x()*(-2*A + B*(w.x()*w.x() + w.y()*w.y()) + 3*C*(w.x()*w.x() + w.y()*w.y())),
      w.y()*(-2*A + B*(w.x()*w.x() + w.y()*w.y()) + 3*C*(w.x()*w.x() + w.y()*w.y())),
      w.z()*(B + 3*C)*(w.x()*w.x() + w.y()*w.y()),
      -2*A*v.x() + B*v.x()*(w.x()*w.x() + w.y()*w.y()) + 2*B*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.x()*w.x()*w.x() + v.x()*w.y()*w.y() + 2*w.x()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      -2*A*v.y() + B*v.y()*(w.x()*w.x() + w.y()*w.y()) + 2*B*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z()) + 3*C*(v.y()*w.x()*w.x() + v.y()*w.y()*w.y() + 2*w.y()*(v.x()*w.x() + v.y()*w.y() + v.z()*w.z())),
      v.z()*(B + 3*C)*(w.x()*w.x() + w.y()*w.y()) }};
    // clang-format on

    // parts with dA, dB, dC
    for (auto i = 0u; i < 3; ++i) {
      const Scalar dA_dwi = dA_over_th * w(i);
      const Scalar dB_dwi = dB_over_th * w(i);
      const Scalar dC_dwi = dC_over_th * w(i);
      for (auto j = 0u; j < 3; ++j) {
        dQ.col(3 + i + 6 * j) += dA_dwi * PA.row(j).transpose() + dB_dwi * PB.row(j).transpose()
                               + dC_dwi * PC.row(j).transpose();
      }
    }

    return {Q, dQ};
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>()    = calculate_q(-a_in);
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_expinv(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>().noalias() = -A_out.template topLeftCorner<3, 3>()
                                                    * calculate_q(-a_in)
                                                    * A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void d2r_exp(TRefIn a_in, THessRefOut H_out)
  {
    H_out.setZero();

    // DERIVATIVES OF SO3 JACOBIAN
    Eigen::Matrix<Scalar, 3, 9> Hso3;
    SO3Impl<Scalar>::d2r_exp(a_in.template tail<3>(), Hso3);

    for (auto i = 0u; i < 3; ++i) {
      H_out.template block<3, 3>(0, 6 * i + 3)      = Hso3.template block<3, 3>(0, 3 * i);
      H_out.template block<3, 3>(3, 18 + 6 * i + 3) = Hso3.template block<3, 3>(0, 3 * i);
    }

    // DERIVATIVE OF Q TERM
    const auto [Q, dQ]                = calculate_Q_dQ(-a_in);
    H_out.template block<3, 18>(3, 0) = -dQ;
  }

  static void d2r_expinv(TRefIn a_in, THessRefOut H_out)
  {
    H_out.setZero();

    // DERIVATIVES OF SO3 JACOBIAN
    Eigen::Matrix<Scalar, 3, 9> Hso3;
    SO3Impl<Scalar>::d2r_expinv(a_in.template tail<3>(), Hso3);

    for (auto i = 0u; i < 3; ++i) {
      H_out.template block<3, 3>(0, 6 * i + 3)      = Hso3.template block<3, 3>(0, 3 * i);
      H_out.template block<3, 3>(3, 18 + 6 * i + 3) = Hso3.template block<3, 3>(0, 3 * i);
    }

    // DERIVATIVE OF -J Q J TERM
    auto [Q, dQ] = calculate_Q_dQ(-a_in);
    dQ *= -1;  // account for -a_in

    Eigen::Matrix3<Scalar> Jso3;
    SO3Impl<Scalar>::dr_expinv(a_in.template tail<3>(), Jso3);
    // Hso3 contains derivatives w.r.t. w, we extend for derivatives w.r.t. [v, w]
    Eigen::Matrix<Scalar, 3, 18> Hso3_exp = Eigen::Matrix<Scalar, 3, 18>::Zero();
    for (auto i = 0u; i < 3; ++i) {
      Hso3_exp.template middleCols<3>(6 * i + 3) = Hso3.template middleCols<3>(3 * i);
    }

    const Eigen::Matrix3<Scalar> Jtmp       = Jso3 * Q;
    const Eigen::Matrix<Scalar, 3, 18> Htmp = d_matrix_product(Jso3, Hso3_exp, Q, dQ);
    H_out.template block<3, 18>(3, 0)       = -d_matrix_product(Jtmp, Htmp, Jso3, Hso3_exp);
  }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__SE3_HPP_


#pragma once

/* boost includes */
#include <boost/math/quaternion.hpp>

/* opencv includes */
#include <opencv2/core/affine.hpp>
#include <opencv2/core/core.hpp>

/* pcl headers */
#include <pcl/point_types.h>

/* sys headers */
#include <math.h>
#include <cassert>
#include <iostream>

template <class T>
class DualQuaternion {
private:
    /* Rotational part */
    boost::math::quaternion<T> real;
    /* Translation (displacement) part */
    boost::math::quaternion<T> dual;

    T dotProduct(boost::math::quaternion<T> q1, boost::math::quaternion<T> q2) {
        return q1.R_component_1() * q2.R_component_1() + q1.R_component_2() * q2.R_component_2() +
               q1.R_component_3() * q2.R_component_3() + q1.R_component_4() * q2.R_component_4();
    }

    boost::math::quaternion<T> normalize(boost::math::quaternion<T> q) { return q / boost::math::norm(q); }

    /* FLOAT_EPSILON */
    const float epsilon = 1.192092896e-07f;

public:
    /* constructor for dual quaternion from rotation unit quaternion and translation quaternion */
    DualQuaternion(boost::math::quaternion<T> rotation, boost::math::quaternion<T> translation)
        : real(rotation), dual(translation) {}

    /* constructor for dual quaternion from rotation quaternion and translation vector */
    DualQuaternion(boost::math::quaternion<T> rotation, cv::Vec3f translation) {
        real = normalize(rotation);
        dual = (boost::math::quaternion<T>(0, translation[0], translation[1], translation[2]) * real) * 0.5f;
    }

    /* constructor for dual quaternion from Euler angles (yaw, pitch, roll) and translation vector */
    DualQuaternion(T yaw, T pitch, T roll, T x, T y, T z) {
        T cy = cos(yaw * 0.5);
        T sy = sin(yaw * 0.5);
        T cr = cos(roll * 0.5);
        T sr = sin(roll * 0.5);
        T cp = cos(pitch * 0.5);
        T sp = sin(pitch * 0.5);

        T qw = cy * cr * cp + sy * sr * sp;
        T qx = cy * sr * cp - sy * cr * sp;
        T qy = cy * cr * sp + sy * sr * cp;
        T qz = sy * cr * cp - cy * sr * sp;

        boost::math::quaternion<T> rotation(qw, qx, qy, qz);

        DualQuaternion<T> dq(rotation, cv::Vec3f(x, y, z));

        real = dq.getReal();
        dual = dq.getDual();
    }

    /* constructor for dual quaternion from Euler-Rodrigues vector and translation vector */
    DualQuaternion(cv::Vec3f rodrigues, cv::Vec3f translation) {
        auto theta          = 2 * atan(cv::norm(rodrigues));  // rotation angle
        auto axis           = rodrigues / theta;
        auto axisNormalised = axis / cv::norm(axis);  // normalised rotation axis

        auto s  = sin(0.5 * theta);
        auto q1 = s * axisNormalised(0);
        auto q2 = s * axisNormalised(1);
        auto q3 = s * axisNormalised(2);
        auto q4 = cos(0.5 * theta);

        boost::math::quaternion<float> rotation(q4, q1, q2, q3);  // rotation quaternion
        DualQuaternion<T> dq(normalize(rotation), translation);

        real = dq.getReal();
        dual = dq.getDual();
    }

    boost::math::quaternion<T> getReal() const { return real; }

    boost::math::quaternion<T> getDual() const { return dual; }

    boost::math::quaternion<T> getRotation() const { return real; }

    cv::Vec3f getTranslation() const {
        boost::math::quaternion<T> q = (dual * 2.0f) * boost::math::conj(real);
        return cv::Vec3f(q.R_component_2(), q.R_component_3(), q.R_component_4());
    }

    DualQuaternion<T> operator+(const DualQuaternion<T>& other) {
        return DualQuaternion<T>(real + other.getReal(), dual + other.getDual());
    }

    DualQuaternion<T>& operator+=(const DualQuaternion<T>& other) {
        real += other.getReal();
        dual += other.getDual();
        return *this;
    }

    DualQuaternion<T> operator-(const DualQuaternion<T>& other) {
        return DualQuaternion<T>(real - other.getReal(), dual - other.getDual());
    }

    DualQuaternion<T>& operator-=(const DualQuaternion<T>& other) {
        real -= other.getReal();
        dual -= other.getDual();
        return *this;
    }

    /* TODO: allow 0.5*dq to be used */
    DualQuaternion<T> operator*(T scale) { return DualQuaternion<T>(real, dual * scale); }

    DualQuaternion<T>& operator*=(T scale) {
        dual *= scale;
        return *this;
    }

    DualQuaternion<T> operator*(const DualQuaternion<T>& other) {
        return DualQuaternion<T>(real * other.getReal(), real * other.getDual() + dual * other.getReal());
    }

    DualQuaternion<T>& operator*=(const DualQuaternion<T>& other) {
        /* Make sure the real is updated after dual to avoid using updated real */
        dual = real * other.getDual() + dual * other.getReal();
        real *= other.getReal();
    }

    DualQuaternion<T> conj() { return DualQuaternion<T>(boost::math::conj(real), boost::math::conj(dual)); }

    DualQuaternion<T>& normalize() {
        T magnitude = sqrtf(dotProduct(real, real));
        assert(magnitude > epsilon);
        real *= (1.0f / magnitude);
        return *this;
    }

    ~DualQuaternion() {}

    T getRoll() const {
        boost::math::quaternion<T> q = real;

        // roll (x-axis rotation)
        float sinr = +2.0 * (q.R_component_1() * q.R_component_2() + q.R_component_3() * q.R_component_4());
        float cosr = +1.0 - 2.0 * (q.R_component_2() * q.R_component_2() + q.R_component_3() * q.R_component_3());
        T roll     = atan2(sinr, cosr);

        if (roll > M_PI) {
            roll -= M_PI_2;
        }

        return roll;
    }

    T getPitch() const {
        boost::math::quaternion<T> q = real;

        // pitch (y-axis rotation)
        T pitch;
        float sinp = +2.0 * (q.R_component_1() * q.R_component_3() - q.R_component_4() * q.R_component_2());

        if (fabs(sinp) >= 1)
            pitch = copysign(M_PI / 2, sinp);  // use 90 degrees if out of range
        else {
            pitch = asin(sinp);
        }

        return pitch;
    }

    T getYaw() const {
        boost::math::quaternion<T> q = real;

        // yaw (z-axis rotation)
        float siny = +2.0 * (q.R_component_1() * q.R_component_4() + q.R_component_2() * q.R_component_3());
        float cosy = +1.0 - 2.0 * (q.R_component_3() * q.R_component_3() + q.R_component_4() * q.R_component_4());
        T yaw      = atan2(siny, cosy);

        if (yaw > M_PI) {
            yaw -= M_PI_2;
        }

        return yaw;
    }

    cv::Vec3f getEulerAngles() const { return cv::Vec3f(getRoll(), getPitch(), getYaw()); }

    cv::Vec3f getRodrigues() {
        auto q     = cv::Vec3f(real.R_component_2(), real.R_component_3(), real.R_component_4());
        auto norm  = cv::norm(q);
        auto theta = 2 * acos(real.R_component_1());

        return tan(0.5 * theta) * q / norm;
    }

    pcl::PointXYZ transformVertex(pcl::PointXYZ v) {
        cv::Vec3f vect(v.x, v.y, v.z);

        cv::Vec3f realVect = cv::Vec3f(real.R_component_2(), real.R_component_3(), real.R_component_4());
        cv::Vec3f dualVect = cv::Vec3f(dual.R_component_2(), dual.R_component_3(), dual.R_component_4());

        cv::Vec3f result =
            vect + 2.f * realVect.cross(realVect.cross(vect) + real.R_component_1() * vect) +
            2.f * (real.R_component_1() * dualVect - dual.R_component_1() * realVect + realVect.cross(dualVect));

        return pcl::PointXYZ(result[0], result[1], result[2]);
    }

    pcl::Normal transformNormal(pcl::Normal n) {
        cv::Vec3f vect(n.data_c[0], n.data_c[1], n.data_c[2]);

        cv::Vec3f realVect = cv::Vec3f(real.R_component_2(), real.R_component_3(), real.R_component_4());
        cv::Vec3f dualVect = cv::Vec3f(dual.R_component_2(), dual.R_component_3(), dual.R_component_4());

        cv::Vec3f result =
            vect + 2.f * realVect.cross(realVect.cross(vect) + real.R_component_1() * vect) +
            2.f * (real.R_component_1() * dualVect - dual.R_component_1() * realVect + realVect.cross(dualVect));

        return pcl::Normal(result[0], result[1], result[2]);
    }

    friend std::ostream& operator<<(std::ostream& os, const DualQuaternion<T>& dq) {
        return os << "real: " << dq.getReal() << "\ndual: " << dq.getDual() << std::endl;
    }
};


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cstddef>
#include <cassert>
#include <limits>
#include <Eigen/Sparse>

using namespace std;

class Array2D
{
private:
	vector<double> m_data;
	size_t m_Nx, m_Ny;

public:
	Array2D(size_t nx, size_t ny, double val = 0.0) : m_Nx(nx), m_Ny(ny), m_data(nx*ny, val) {}

	// 0-based indexing
	double &at(int i, int j)
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	double at(int i, int j) const
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	// 1-based indexing
	double &operator()(int i, int j)
	{
		return at(i - 1, j - 1);
	}

	double operator()(int i, int j) const
	{
		return at(i - 1, j - 1);
	}
};

const size_t WIDTH = 16;
const size_t DIGITS = 7;

const double L = 0.5; // m
const double D = 0.01; // m
const double Ue = 1.0; // m/s
const double Pe = 0.0;
const double rho = 1.225; // Kg/m3
const double mu = 3.737e-5; // Kg/m/s

const int Nx = 21, Ny = 11;
const double dx = L / (Nx - 1), dy = D / (Ny - 1);
const double dx2 = 2 * dx, dy2 = 2 * dy;
const double dxdx = dx * dx, dydy = dy * dy;
vector<double> x(Nx, 0.0), y(Ny, 0.0);

const double dt = 0.001;
double t = 0.0;
int iter_cnt = 0;
const int MAX_ITER_NUM = 2000;

const double a = 2 * (dt / dxdx + dt / dydy);
const double b = -dt / dxdx;
const double c = -dt / dydy;
double d_min = numeric_limits<double>::max(), d_max = numeric_limits<double>::min(), d_15_5 = 0.0;

Array2D p(Nx, Ny, Pe), p_star1(Nx, Ny, Pe), p_star2(Nx, Ny, 0.0);
Array2D u(Nx + 1, Ny, 0.0), u_star1(Nx + 1, Ny, 0.0), u_star2(Nx + 1, Ny, 0.0), u_wedge(Nx + 1, Ny, 0.0);
Array2D A(Nx + 1, Ny, 0.0), A_star(Nx + 1, Ny, 0.0);
Array2D v(Nx + 2, Ny + 1, 0.0), v_star1(Nx + 2, Ny + 1, 0.0), v_star2(Nx + 2, Ny + 1, 0.0), v_wedge(Nx + 2, Ny + 1, 0.0);
Array2D B(Nx + 2, Ny + 1, 0.0), B_star(Nx + 2, Ny + 1, 0.0);

// Full flowfield in TECPLOT ASCII Format.
void output1()
{
	Array2D u_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 1; i <= Nx; ++i)
			u_interp(i, j) = (u(i, j) + u(i + 1, j)) / 2; // Inner
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, Ny) = Ue; // Top

	Array2D v_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
	{
		v_interp(1, j) = 0.0; // Left
		for (int i = 3; i <= Nx + 1; ++i)
			v_interp(i - 1, j) = (v(i, j) + v(i, j + 1)) / 2; // Inner and Right
	}
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, Ny) = 0.0; // Top

	// Create Tecplot data file.
	ofstream result("flow" + to_string(iter_cnt) + ".dat");
	if (!result)
		throw "Failed to create data file!";

	// Header
	result << "TITLE = \"t=" << t << "\"" << endl;
	result << "VARIABLES = \"X\", \"Y\", \"P\", \"U\", \"V\"" << endl;
	result << "ZONE I=" << Nx << ", J=" << Ny << ", F=POINT" << endl;

	// Flowfield data
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
		{
			result << setw(WIDTH) << setprecision(DIGITS) << x[i - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << y[j - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << p(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << u_interp(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << v_interp(i, j);
			result << endl;
		}

	// Finalize
	result.close();
}

// Statistics at (15, 5) and i=15
void output2(int iter)
{
	static const string fn("history_at_15_5.txt");

	ofstream fout;
	if (iter == 0)
	{
		fout.open(fn, ios::out);
		if (!fout)
			throw "Failed to open history file.";

		for (int j = 0; j < Ny; ++j)
			fout << setw(WIDTH) << setprecision(DIGITS) << y[j];
		fout << endl;
	}
	else
	{
		fout.open(fn, ios::app);
		if (!fout)
			throw "Failed to open history file.";
	}

	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << u(15, j);
	fout << endl;
	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << v(15, j);
	fout << endl;
	fout << d_15_5 << endl;

	fout.close();
}

void init()
{
	cout << "mu=" << mu << endl;
	cout << "dt=" << dt << endl;

	// Init
	for (int i = 1; i < Nx; ++i)
		x[i] = L * i / (Nx - 1); // X-Coordinates
	for (int j = 1; j < Ny; ++j)
		y[j] = D * j / (Ny - 1); // Y-Coordinates

	for (int i = 1; i <= Nx + 1; ++i)
		u(i, Ny) = Ue; // U at top
	v(15, 5) = 0.5; // Initial peak to ensure 2D flow structure
}

// Solve the pressure equation.
void ImplicitMethod1()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.emplace_back(id, id, 1.0);
				rhs(id) = 0.0;
			}
			else if (j == 0) // Bottom
			{
				coef.emplace_back(id, id, 1.0);
				coef.emplace_back(id, id_n, -1.0);
				rhs(id) = 0.0;
			}
			else if (j == Ny - 1) // Top
			{
				coef.emplace_back(id, id, 1.0);
				coef.emplace_back(id, id_s, -1.0);
				rhs(id) = 0.0;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho * u_star1.at(i + 1, j) - rho * u_star1.at(i, j)) / dx + (rho * v_star1.at(i + 1, j + 1) - rho * v_star1.at(i + 1, j)) / dy;

				coef.emplace_back(id, id, a);
				coef.emplace_back(id, id_w, b);
				coef.emplace_back(id, id_e, b);
				coef.emplace_back(id, id_n, c);
				coef.emplace_back(id, id_s, c);
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chl(A);
	Eigen::VectorXd x = chl.solve(rhs);

	// Update p*
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p_star1.at(i, j) = p.at(i, j) + x(id);
		}
}

// Solve the pressure-correction equation
void ImplicitMethod2()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.emplace_back(id, id, 1.0);
				rhs(id) = 0.0;
			}
			else if (j == 0) // Bottom
			{
				coef.emplace_back(id, id, 1.0);
				coef.emplace_back(id, id_n, -1.0);
				rhs(id) = 0.0;
			}
			else if (j == Ny - 1) // Top
			{
				coef.emplace_back(id, id, 1.0);
				coef.emplace_back(id, id_s, -1.0);
				rhs(id) = 0.0;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho*u_wedge.at(i + 1, j) - rho * u_wedge.at(i, j)) / dx + (rho * v_wedge.at(i + 1, j + 1) - rho * v_wedge.at(i + 1, j)) / dy;
				if (d > d_max)
					d_max = d;
				if (d < d_min)
					d_min = d;
				if (i == 15 && j == 5)
					d_15_5 = d;

				coef.emplace_back(id, id, a);
				coef.emplace_back(id, id_w, b);
				coef.emplace_back(id, id_e, b);
				coef.emplace_back(id, id_n, c);
				coef.emplace_back(id, id_s, c);
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chl(A);
	Eigen::VectorXd x = chl.solve(rhs);

	// Update p_prime
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p_star2.at(i, j) = p_star1.at(i, j) + x(id);
		}
}

void PISO()
{
	/********************************************** Prediction Step ***************************************************/
	// u* at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			const double v_bar1 = 0.5*(v(i, j + 1) + v(i + 1, j + 1));
			const double v_bar2 = 0.5*(v(i, j) + v(i + 1, j));

			const double t11 = rho * pow(u(i + 1, j), 2) - rho * pow(u(i - 1, j), 2);
			const double t12 = rho * u(i, j + 1)*v_bar1 - rho * u(i, j - 1)*v_bar2;
			const double t21 = u(i + 1, j) - 2 * u(i, j) + u(i - 1, j);
			const double t22 = u(i, j + 1) - 2 * u(i, j) + u(i, j - 1);
			A(i, j) = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			const double dpdx = (p(i, j) - p(i - 1, j)) / dx;

			u_star1(i, j) = (rho * u(i, j) + A(i, j) * dt - dt * dpdx) / rho;
		}

	// u* at boundary
	for (int i = 1; i <= Nx + 1; ++i)
	{
		u_star1(i, 1) = 0.0;
		u_star1(i, Ny) = Ue;
	}
	for (int j = 2; j <= Ny - 1; ++j)
	{
		u_star1(1, j) = 2 * u_star1(2, j) - u_star1(3, j);
		u_star1(Nx + 1, j) = 2 * u_star1(Nx, j) - u_star1(Nx - 1, j);
	}

	// v* at inner points
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			const double u_bar1 = 0.5 *(u(i, j - 1) + u(i, j));
			const double u_bar2 = 0.5 *(u(i - 1, j - 1) + u(i - 1, j));

			const double t11 = rho * v(i + 1, j) * u_bar1 - rho * v(i - 1, j) * u_bar2;
			const double t12 = rho * pow(v(i, j + 1), 2) - rho * pow(v(i, j - 1), 2);
			const double t21 = v(i + 1, j) - 2 * v(i, j) + v(i - 1, j);
			const double t22 = v(i, j + 1) - 2 * v(i, j) + v(i, j - 1);
			B(i, j) = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			const double dpdy = (p(i - 1, j) - p(i - 1, j - 1)) / dy;

			v_star1(i, j) = (rho * v(i, j) + B(i, j) * dt - dt * dpdy) / rho;
		}

	// v* at boundary
	for (int j = 2; j <= Ny; ++j)
	{
		v_star1(2, j) = 0.0;
		v_star1(1, j) = -v_star1(3, j);
		v_star1(Nx + 2, j) = 2 * v_star1(Nx + 1, j) - v_star1(Nx, j);
	}
	for (int i = 1; i <= Nx + 2; ++i)
	{
		v_star1(i, 1) = -v_star1(i, 2);
		v_star1(i, Ny + 1) = -v_star1(i, Ny);
	}

	/************************************************ Correction Step1 ************************************************/
	// Solve (p* - p)
	ImplicitMethod1();

	// u** at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			const double dpdx = (p_star1(i, j) - p_star1(i - 1, j)) / dx;
			u_star2(i, j) = (rho * u(i, j) + A(i, j) * dt - dt * dpdx) / rho;
		}

	// u** at boundary
	for (int i = 1; i <= Nx + 1; ++i)
	{
		u_star2(i, 1) = 0.0;
		u_star2(i, Ny) = Ue;
	}
	for (int j = 2; j <= Ny - 1; ++j)
	{
		u_star2(1, j) = 2 * u_star2(2, j) - u_star2(3, j);
		u_star2(Nx + 1, j) = 2 * u_star2(Nx, j) - u_star2(Nx - 1, j);
	}

	// v** at inner points
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			const double dpdy = (p_star1(i - 1, j) - p_star1(i - 1, j - 1)) / dy;
			v_star2(i, j) = (rho * v(i, j) + B(i, j) * dt - dt * dpdy) / rho;
		}

	// v** at boundary
	for (int j = 2; j <= Ny; ++j)
	{
		v_star2(2, j) = 0.0;
		v_star2(1, j) = -v_star2(3, j);
		v_star2(Nx + 2, j) = 2 * v_star2(Nx + 1, j) - v_star2(Nx, j);
	}
	for (int i = 1; i <= Nx + 2; ++i)
	{
		v_star2(i, 1) = -v_star2(i, 2);
		v_star2(i, Ny + 1) = -v_star2(i, Ny);
	}

	/************************************************ Correction Step2 ************************************************/
	// u**^ at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			const double v_bar1 = 0.5*(v_star1(i, j + 1) + v_star1(i + 1, j + 1));
			const double v_bar2 = 0.5*(v_star1(i, j) + v_star1(i + 1, j));

			const double t11 = rho * pow(u_star1(i + 1, j), 2) - rho * pow(u_star1(i - 1, j), 2);
			const double t12 = rho * u_star1(i, j + 1)*v_bar1 - rho * u_star1(i, j - 1)*v_bar2;
			const double t21 = u_star1(i + 1, j) - 2 * u_star1(i, j) + u_star1(i - 1, j);
			const double t22 = u_star1(i, j + 1) - 2 * u_star1(i, j) + u_star1(i, j - 1);
			A_star(i, j) = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			u_wedge(i, j) = (rho * u_star2(i, j) + (A_star(i, j) - A(i, j)) * dt) / rho;
		}

	// v**^ at inner points
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			const double u_bar1 = 0.5 *(u_star1(i, j - 1) + u_star1(i, j));
			const double u_bar2 = 0.5 *(u_star1(i - 1, j - 1) + u_star1(i - 1, j));

			const double t11 = rho * v_star1(i + 1, j) * u_bar1 - rho * v_star1(i - 1, j) * u_bar2;
			const double t12 = rho * pow(v_star1(i, j + 1), 2) - rho * pow(v_star1(i, j - 1), 2);
			const double t21 = v_star1(i + 1, j) - 2 * v_star1(i, j) + v_star1(i - 1, j);
			const double t22 = v_star1(i, j + 1) - 2 * v_star1(i, j) + v_star1(i, j - 1);
			B_star(i, j) = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			v_wedge(i, j) = (rho * v_star2(i, j) + (B_star(i, j) - B(i, j)) * dt) / rho;
		}

	d_min = numeric_limits<double>::max();
	d_max = numeric_limits<double>::min();
	ImplicitMethod2();

	/************************************************* Update u and v *************************************************/
	// Correct u at inner nodes
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			const double dpdx = (p_star2(i, j) - p_star2(i - 1, j)) / dx;
			u(i, j) = (rho * u_wedge(i, j) - dt * dpdx) / rho;
		}

	// Linear extrapolation of u at virtual nodes
	for (int j = 2; j <= Ny - 1; ++j)
	{
		u(1, j) = 2 * u(2, j) - u(3, j);
		u(Nx + 1, j) = 2 * u(Nx, j) - u(Nx - 1, j);
	}

	// Correct v at inner nodes
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			const double dpdy = (p_star2(i - 1, j) - p_star2(i - 1, j - 1)) / dy;
			v(i, j) = (rho * v_wedge(i, j) - dt * dpdy) / rho;
		}

	// Linear extrapolation of v at right virtual nodes
	for (int j = 2; j <= Ny; ++j)
		v(Nx + 2, j) = 2 * v(Nx + 1, j) - v(Nx, j);

	// Linear extrapolation of v at both top and bottom virtual nodes
	// No-Penetration at both top and bottom
	for (int i = 1; i <= Nx + 2; ++i)
	{
		v(i, 1) = -v(i, 2);
		v(i, Ny + 1) = -v(i, Ny);
	}

	// Update p
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
			p(i, j) = p_star2(i, j);
}

bool check_convergence()
{
	// Statistics of the mass flux residue
	cout << "Max(d)=" << d_max << " Min(d)=" << d_min << endl;

	// Statistics of u
	double u_max = numeric_limits<double>::min();
	double u_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			u_max = max(u_max, u(i, j));
			u_min = min(u_min, u(i, j));
		}
	cout << "Max(u)=" << u_max << " Min(u)=" << u_min << endl;

	// Statistics of v
	double v_max = numeric_limits<double>::min();
	double v_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			v_max = max(v_max, v(i, j));
			v_min = min(v_min, v(i, j));
		}
	cout << "Max(v)=" << v_max << " Min(v)=" << v_min << endl;

	// Statistics of p
	double p_max = numeric_limits<double>::min();
	double p_min = numeric_limits<double>::max();
	for (int i = 1; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			p_max = max(p_max, p(i, j));
			p_min = min(p_min, p(i, j));
		}
	cout << "Max(p)=" << p_max << " Min(p)=" << p_min << endl;

	return iter_cnt > MAX_ITER_NUM || max(abs(d_max), abs(d_min)) < 1e-4;
}

void loop()
{
	bool converged = false;
	while (!converged)
	{
		++iter_cnt;
		cout << "Iter" << iter_cnt << ":" << endl;

		PISO();
		t += dt;

		output1();
		output2(iter_cnt);

		converged = check_convergence();
	}
}

int main(int argc, char *argv[])
{
	// Initialize
	init();

	// Output I.C.
	output1();
	output2(0);

	// Solve
	loop();

	return 0;
}


#include "laplacian_deformation.hpp"

#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <math.h>

#include <set>

class vec3 {
public:
	double x, y, z;

	vec3(double x, double y, double z) { this->x = x; this->y = y; this->z = z; }

	vec3(double v) { this->x = v; this->y = v; this->z = v; }

	vec3() { this->x = this->y = this->z = 0; }

	vec3& operator+=(const vec3& b) { (*this) = (*this) + b; return (*this); }
	vec3& operator-=(const vec3& b) { (*this) = (*this) - b; return (*this); }

	friend vec3 operator-(const vec3& a, const vec3& b) { return vec3(a.x - b.x, a.y - b.y, a.z - b.z); }
	friend vec3 operator+(const vec3& a, const vec3& b) { return vec3(a.x + b.x, a.y + b.y, a.z + b.z); }
	friend vec3 operator*(const double s, const vec3& a) { return vec3(s * a.x, s * a.y, s * a.z); }
	friend vec3 operator*(const vec3& a, const double s) { return s * a; }
};

typedef Eigen::SparseMatrix<double> SpMat;

typedef Eigen::MatrixXd DMat;

typedef Eigen::Triplet<double> Triplet;

typedef Eigen::VectorXd Vec;

struct Sorter {
	bool operator()(const Triplet& a, const Triplet& b) {
		if (a.row() != b.row()) {
			return a.row() < b.row();
		}
		else {
			return a.col() < b.col();
		}
	}
} sorter;

struct Entry {
	int a0;
	int a1;
	double a2;

	Entry(int a0_, int a1_, double a2_) : a0(a0_), a1(a1_), a2(a2_) {
	}
};

struct SorterEntry {
	bool operator()(const Entry& a, const Entry& b) {
		if (a.a0 != b.a0) {
			return a.a0 < b.a0;
		}
		else {
			return a.a1 < b.a1;
		}
	}
} sorterEntry;

std::vector<Triplet> calcEnergyMatrixCoeffs(
	const Vec& roiPositions,
	const Vec& delta,
	const int nRoi,

	std::vector<std::vector<int> > adj,

	const std::vector<int>& rowBegins,
	const std::vector<Triplet>& laplacianCoeffs
) {
	std::vector<DMat> Ts;

	Ts.resize(nRoi);

	for (int i = 0; i < nRoi; ++i) {
		// set of {i} and the neigbbours of i.
		std::vector<int> iAndNeighbours;

		iAndNeighbours.push_back(i);
		for (int j = 0; j < adj[i].size(); ++j) {
			iAndNeighbours.push_back(adj[i][j]);
		}

		DMat At(7, iAndNeighbours.size() * 3);
		for (int row = 0; row < 7; ++row) {
			for (int col = 0; col < iAndNeighbours.size() * 3; ++col) {
				At(row, col) = 0.0f;
			}
		}

		for (int j = 0; j < iAndNeighbours.size(); ++j) {
			int k = iAndNeighbours[j];

			double vk[3];
			vk[0] = roiPositions[3 * k + 0];
			vk[1] = roiPositions[3 * k + 1];
			vk[2] = roiPositions[3 * k + 2];

			const int x = 0;
			const int y = 1;
			const int z = 2;

			At(0, j * 3 + 0) = +vk[x];
			At(1, j * 3 + 0) = 0;
			At(2, j * 3 + 0) = +vk[z];
			At(3, j * 3 + 0) = -vk[y];
			At(4, j * 3 + 0) = +1;
			At(5, j * 3 + 0) = 0;
			At(6, j * 3 + 0) = 0;

			At(0, j * 3 + 1) = +vk[y];
			At(1, j * 3 + 1) = -vk[z];
			At(2, j * 3 + 1) = 0;
			At(3, j * 3 + 1) = +vk[x];
			At(4, j * 3 + 1) = 0;
			At(5, j * 3 + 1) = +1;
			At(6, j * 3 + 1) = 0;

			At(0, j * 3 + 2) = +vk[z];
			At(1, j * 3 + 2) = +vk[y];
			At(2, j * 3 + 2) = -vk[x];
			At(3, j * 3 + 2) = 0;
			At(4, j * 3 + 2) = 0;
			At(5, j * 3 + 2) = 0;
			At(6, j * 3 + 2) = 1;
		}

		DMat invprod = (At * At.transpose()).inverse();
		DMat pseudoinv = invprod * At;
		Ts[i] = pseudoinv;
		// Ts[i] now contains (A^T A ) A^T (see equation 12 from paper.)
	}

	std::vector<Triplet> result;

	std::map<int, double> row;

	for (int i = 0; i < (nRoi * 3); ++i) {
		row.clear();

		// add uniform weights to matrix(equation 2 from paper)
		for (int ientry = rowBegins[i]; ientry < rowBegins[i + 1]; ++ientry) {
			Triplet t = laplacianCoeffs[ientry];
			row[t.col()] = t.value();
		}

		// get delta coordinates for the vertex.
		double dx = delta[int(i / 3) * 3 + 0];
		double dy = delta[int(i / 3) * 3 + 1];
		double dz = delta[int(i / 3) * 3 + 2];

		std::vector<int> iAndNeighbours;
		iAndNeighbours.push_back(int(i / 3));
		for (int j = 0; j < adj[int(i / 3)].size(); ++j) {
			iAndNeighbours.push_back(adj[int(i / 3)][j]);
		}

		DMat T = Ts[int(i / 3)];

		Vec s = T.row(0);
		Vec h1 = T.row(1);
		Vec h2 = T.row(2);
		Vec h3 = T.row(3);
		Vec tx = T.row(4);
		Vec ty = T.row(5);
		Vec tz = T.row(6);

		if ((i % 3) == 0) { // x case.
			for (int j = 0; j < T.row(0).size(); ++j) {
				int p = j % 3;
				int q = (int)floor((double)j / (double)3);
				int r = iAndNeighbours[q];

				row[p + 3 * r] -= dx * (+s[j]);
				row[p + 3 * r] -= dy * (-h3[j]);
				row[p + 3 * r] -= dz * (+h2[j]);
			}
		}
		else if ((i % 3) == 1) { // y case.
			for (int j = 0; j < T.row(0).size(); ++j) {
				int p = j % 3;
				int q = (int)floor((double)j / (double)3);
				int r = iAndNeighbours[q];

				row[p + 3 * r] -= dx * (+h3[j]);
				row[p + 3 * r] -= dy * (+s[j]);
				row[p + 3 * r] -= dz * (-h1[j]);
			}
		}
		else if ((i % 3) == 2) { // z case.
			for (int j = 0; j < T.row(0).size(); ++j) {
				int p = j % 3;
				int q = (int)floor((double)j / (double)3);
				int r = iAndNeighbours[q];

				row[p + 3 * r] -= dx * (-h2[j]);
				row[p + 3 * r] -= dy * (+h1[j]);
				row[p + 3 * r] -= dz * (+s[j]);

			}
		}

		for (const auto& p : row) {
			result.push_back(Triplet(i, p.first, p.second));
		}
	}

	return result;
}

double hypot(double x, double y, double z) {
	return sqrt(
		x * x +
		y * y +
		z * z);
}

// cotangent discretization of the laplacian.
std::vector<Triplet> calcCotangentLaplacianCoeffs(

	int* cells, const int nCells,

	const std::vector<int>& roiMap,

	int nRoi,
	std::vector<std::vector<int> > adj,

	std::vector<int>& rowBegins,

	const Vec& roiPositions
) {
	std::vector<Triplet> result;

	std::vector<int> cells_flattened;

	for (int i = 0; i < nCells; i += 3) {
		int c[3] = { roiMap[cells[i + 0]], roiMap[cells[i + 1]] , roiMap[cells[i + 2]] };

		if (c[0] == -1 || c[1] == -1 || c[2] == -1) {
			continue;
		}

		cells_flattened.push_back(c[0]);
		cells_flattened.push_back(c[1]);
		cells_flattened.push_back(c[2]);
	}

	std::map<std::pair<int, int>, double> laplacian;

	std::vector<double> areas;
	for (int i = 0; i < nRoi; ++i) {
		areas.push_back(0);
	}

	std::vector<Entry> entries;
	for (int i = 0; i < cells_flattened.size(); i += 3) {
		int ia = cells_flattened[i + 0];
		int ib = cells_flattened[i + 1];
		int ic = cells_flattened[i + 2];

		vec3 a = vec3(roiPositions[3 * ia + 0], roiPositions[3 * ia + 1], roiPositions[3 * ia + 2]);
		vec3 b = vec3(roiPositions[3 * ib + 0], roiPositions[3 * ib + 1], roiPositions[3 * ib + 2]);
		vec3 c = vec3(roiPositions[3 * ic + 0], roiPositions[3 * ic + 1], roiPositions[3 * ic + 2]);

		double abx = a.x - b.x;
		double aby = a.y - b.y;
		double abz = a.z - b.z;

		double bcx = b.x - c.x;
		double bcy = b.y - c.y;
		double bcz = b.z - c.z;

		double cax = c.x - a.x;
		double cay = c.y - a.y;
		double caz = c.z - a.z;

		double area = 0.5 * hypot(
			aby * caz - abz * cay,
			abz * cax - abx * caz,
			abx * cay - aby * cax);

		//Skip thin triangles
		if (area < 1e-8) {
			continue;
		}

		double w = -0.5 / area;
		double wa = w * (abx * cax + aby * cay + abz * caz);
		double wb = w * (bcx * abx + bcy * aby + bcz * abz);
		double wc = w * (cax * bcx + cay * bcy + caz * bcz);

		double varea = area / 3.0;
		areas[ia] += varea;
		areas[ib] += varea;
		areas[ic] += varea;

		entries.push_back(Entry(ib, ic, wa));
		entries.push_back(Entry(ic, ib, wa));
		entries.push_back(Entry(ic, ia, wb));
		entries.push_back(Entry(ia, ic, wb));
		entries.push_back(Entry(ia, ib, wc));
		entries.push_back(Entry(ib, ia, wc));
	}

	std::vector<double> weights;
	for (int i = 0; i < nRoi; ++i) {
		weights.push_back(0.0);
	}

	std::sort(entries.begin(), entries.end(), sorterEntry);

	int ptr = 0;

	for (int i = 0; i < entries.size(); ) {

		Entry entry = entries[i++];

		while (
			i < entries.size() &&
			entries[i].a0 == entry.a0 &&
			entries[i].a1 == entry.a1) {
			entry.a2 += entries[i++].a2;

		}

		entry.a2 /= areas[entry.a0];
		weights[entry.a0] += entry.a2;
		entries[ptr++] = entry;
	}

	for (int i = 0; i < ptr; ++i) {
		std::pair<int, int> e(entries[i].a0, entries[i].a1);
		Triplet t(e.first, e.second, entries[i].a2);
		result.push_back(t);
	}

	for (int i = 0; i < nRoi; ++i) {
		Triplet t(i, i, -weights[i]);
		result.push_back(t);
	}
	std::sort(result.begin(), result.end(), sorter);

	{
		std::vector<Triplet> result2;

		for (int i = 0; i < result.size(); ++i) {

			Triplet a = result[i];

			result2.push_back(Triplet(3 * a.row() + 0, 3 * a.col() + 0, a.value()));

			result2.push_back(Triplet(3 * a.row() + 1, 3 * a.col() + 1, a.value()));

			result2.push_back(Triplet(3 * a.row() + 2, 3 * a.col() + 2, a.value()));

		}

		result = result2;

		std::sort(result.begin(), result.end(), sorter);
	}

	int current = result[0].row();
	rowBegins.push_back(0);
	for (int i = 0; i < result.size(); ++i) {

		if (result[i].row() != current) {
			rowBegins.push_back(i);
			current = result[i].row();
		}
	}
	rowBegins.push_back(result.size());

	return result;
}

std::vector<Triplet> calcUniformLaplacianCoeffs(
	int nRoi,
	std::vector<std::vector<int> > adj,

	std::vector<int>& rowBegins
) {
	std::vector<Triplet> result;
	std::map<int, double> row;

	for (int i = 0; i < (nRoi * 3); ++i) {
		rowBegins.push_back(result.size());
		row.clear();

		row[(i % 3) + int(i / 3) * 3] = 1;
		double w = -1.0 / adj[int(i / 3)].size();
		for (int j = 0; j < adj[int(i / 3)].size(); ++j) {
			row[(i % 3) + 3 * adj[int(i / 3)][j]] = w;
		}

		for (const auto& p : row) {
			result.push_back(Triplet(i, p.first, p.second));
		}
	}
	rowBegins.push_back(result.size());

	return result;
}

struct State {
	bool RSI;

	Vec roiDelta;

	SpMat augEnergyMatrixTrans;
	SpMat augNormalizeDeltaCoordinatesTrans;

	Eigen::SimplicialCholesky<SpMat>*energyMatrixCholesky = nullptr;
	Eigen::SimplicialCholesky<SpMat>*normalizeDeltaCoordinatesCholesky = nullptr;

	int* roiIndices;
	int nRoi;

	SpMat lapMat;

	std::vector<double> roiDeltaLengths;

	Vec b;
};

State s;

double getLength(double ax, double ay, double az) {
	return sqrt(ax*ax + ay * ay + az * az);
}

void freeDeform() {
	if (s.energyMatrixCholesky != nullptr) {
		delete s.energyMatrixCholesky;
		s.energyMatrixCholesky = nullptr;
	}
	if (s.normalizeDeltaCoordinatesCholesky != nullptr) {
		delete s.normalizeDeltaCoordinatesCholesky;
		s.normalizeDeltaCoordinatesCholesky = nullptr;
	}
}

/*
For reference, the equation numbers refer to the paper:
https://people.eecs.berkeley.edu/~jrs/meshpapers/SCOLARS.pdf
*/
void prepareDeform(
	int* cells, const int nCells,

	double* positions, const int nPositions,

	int* roiIndices, const int nRoi,

	const int unconstrainedBegin,

	bool RSI) {

	// free memory from previous call of prepareDeform()
	if (s.energyMatrixCholesky != nullptr) {
		delete s.energyMatrixCholesky;
		s.energyMatrixCholesky = nullptr;
	}
	if (s.normalizeDeltaCoordinatesCholesky != nullptr) {
		delete s.normalizeDeltaCoordinatesCholesky;
		s.normalizeDeltaCoordinatesCholesky = nullptr;
	}

	std::vector<std::vector<int> > adj;
	std::vector<int> roiMap(nPositions, -1);

	{
		for (int i = 0; i < nRoi; ++i) {
			roiMap[roiIndices[i]] = i;
		}

		adj.resize(nRoi);
		for (int i = 0; i < adj.size(); ++i) {
			adj[i] = std::vector<int>();
		}
		for (int i = 0; i < nCells; i += 3) {
			int c[3] = { cells[i + 0], cells[i + 1] , cells[i + 2] };

			for (int j = 0; j < 3; ++j) {
				int a = roiMap[c[j]];

				int b = roiMap[c[(j + 1) % 3]];

				if (a != -1 && b != -1) {
					adj[a].push_back(b);
				}
			}
		}
	}

	// put all the positions of the vertices in ROI in a single vector.
	Vec roiPositions(nRoi * 3);
	{
		int c = 0;
		for (int i = 0; i < nRoi; ++i) {
			for (int d = 0; d < 3; ++d) {
				roiPositions[c++] = positions[3 * roiIndices[i] + d];
			}
		}
	}

	std::vector<int> rowBegins;
	std::vector<Triplet> laplacianCoeffs;

	/*
	// cotangent laplacian doesnt yield any good results, for some reason :/
	so we don't use it. instead, use uniform.
	laplacianCoeffs = calcCotangentLaplacianCoeffs(

		cells, nCells,
		roiMap,

		nRoi,
		adj,
		rowBegins,

		roiPositions);
		*/

	laplacianCoeffs = calcUniformLaplacianCoeffs(nRoi, adj, rowBegins);

	s.lapMat = SpMat(nRoi * 3, nRoi * 3);
	s.lapMat.setFromTriplets(laplacianCoeffs.begin(), laplacianCoeffs.end());

	// by simply multiplying by the laplacian matrix, we can compute the laplacian coordinates(the delta coordinates)
	// of the vertices in ROI.
	s.roiDelta = s.lapMat * roiPositions;

	// we save away the original lengths of the delta coordinates.
	// we need these when normalizing the results of our solver.
	{
		s.roiDeltaLengths = std::vector<double>(s.roiDelta.size() / 3, 0.0f);
		for (int i = 0; i < s.roiDelta.size() / 3; ++i) {
			s.roiDeltaLengths[i] = getLength(
				s.roiDelta[3 * i + 0],
				s.roiDelta[3 * i + 1],
				s.roiDelta[3 * i + 2]
			);
		}
	}

	std::vector<Triplet> energyMatrixCoeffs;

	// num rows in augmented matrix.
	// notice that we put x, y, and z in a large single matrix, and therefore it is multiplied by 3.
	int M = (nRoi + unconstrainedBegin) * 3;
	// num columns in augmented matrix.
	int N = nRoi * 3;

	if (RSI) {
		// this matrix represents the first term of the energy (5).
		energyMatrixCoeffs = calcEnergyMatrixCoeffs(
			roiPositions,
			s.roiDelta, nRoi, adj, rowBegins, laplacianCoeffs);

		for (int i = 0; i < unconstrainedBegin; ++i) {
			laplacianCoeffs.push_back(Triplet(i * 3 + N + 0, 3 * i + 0, 1));
			laplacianCoeffs.push_back(Triplet(i * 3 + N + 1, 3 * i + 1, 1));
			laplacianCoeffs.push_back(Triplet(i * 3 + N + 2, 3 * i + 2, 1));
		}

		SpMat augMat(M, N);
		augMat.setFromTriplets(laplacianCoeffs.begin(), laplacianCoeffs.end());
		s.augNormalizeDeltaCoordinatesTrans = augMat.transpose();

		s.normalizeDeltaCoordinatesCholesky = new Eigen::SimplicialCholesky<SpMat>(s.augNormalizeDeltaCoordinatesTrans * augMat);
	}
	else {
		// if not rotation-scale-invariant, we simply use the regular laplacian matrix. This is the first term of the energy (4)
		energyMatrixCoeffs = laplacianCoeffs;
	}

	// in order to add the second term of the energy (4) or (5), we now augment the matrix.
	{
		// we augment the matrix by adding constraints for the handles.
		// these constraints ensure that if the handles are dragged, the handles will strictly follow in the specified direction.
		// the handle vertices are not free, unlike the unconstrained vertices.
		for (int i = 0; i < unconstrainedBegin; ++i) {
			energyMatrixCoeffs.push_back(Triplet(i * 3 + N + 0, 3 * i + 0, 1));
			energyMatrixCoeffs.push_back(Triplet(i * 3 + N + 1, 3 * i + 1, 1));
			energyMatrixCoeffs.push_back(Triplet(i * 3 + N + 2, 3 * i + 2, 1));
		}

		SpMat augMat(M, N);
		augMat.setFromTriplets(energyMatrixCoeffs.begin(), energyMatrixCoeffs.end());
		s.augEnergyMatrixTrans = augMat.transpose();

		// for solving later, we need the cholesky decomposition of (transpose(augMat) * augMat)
		// this is a slow step! probably the slowest part of the entire algorithm.
		s.energyMatrixCholesky = new Eigen::SimplicialCholesky<SpMat>(s.augEnergyMatrixTrans * augMat);
	}

	s.b = Vec(M);
	s.roiIndices = roiIndices;
	s.nRoi = nRoi;
	s.RSI = RSI;
}

void doDeform(double* newHandlePositions, int nHandlePositions, double* outPositions) {
	{
		int count = 0;
		for (int i = 0; i < s.roiDelta.size(); ++i) {
			if (s.RSI) {
				// following from our derivations, we must set all these to zero.
				s.b[count++] = 0.0f;
			}
			else {
				s.b[count++] = s.roiDelta[i];
			}
		}
		for (int j = 0; j < nHandlePositions; ++j) {
			s.b[count++] = newHandlePositions[j * 3 + 0];
			s.b[count++] = newHandlePositions[j * 3 + 1];
			s.b[count++] = newHandlePositions[j * 3 + 2];
		}
	}

	Vec minimizerSolution;
	{
		// Now we solve
		// Ax = b
		// where A is the energy matrix, and the value of b depends on whether we are optimizing (4) or (5)
		// by solving, we obtain the deformed surface coordinates that minimizes either (4) or (5).
		Vec y = s.augEnergyMatrixTrans * s.b;
		minimizerSolution = s.energyMatrixCholesky->solve(y);
	}

	if (s.RSI) {
		// if minimizing (5), a local scaling is introduced by the solver.
		// so we need to normalize the delta coordinates of the deformed vertices back to their
		// original lengths.
		// otherwise, the mesh will increase in size when manipulating the mesh, which is not desirable.

		// the normalization step is pretty simple:
		// we find the delta coordinates of our solution.
		// then we normalize these delta coordinates, so that their lengths match the lengths of the original, undeformed delta coordinates.
		// then we simply do a minimization to find the coordinates that are as close as possible to the normalized delta coordinates
		// and the solution of this minimization is our final solution.

		Vec solutionDelta = s.lapMat * minimizerSolution;

		int count = 0;
		for (int i = 0; i < s.roiDeltaLengths.size(); ++i) {

			double len = getLength(solutionDelta[3 * i + 0], solutionDelta[3 * i + 1], solutionDelta[3 * i + 2]);
			double originalLength = s.roiDeltaLengths[i];
			double scale = originalLength / len;

			for (int d = 0; d < 3; ++d) {
				s.b[count++] = scale * solutionDelta[3 * i + d];
			}
		}

		Vec y = s.augNormalizeDeltaCoordinatesTrans * s.b;
		Vec normalizedSolution = s.normalizeDeltaCoordinatesCholesky->solve(y);

		for (int i = 0; i < s.nRoi; ++i) {
			for (int d = 0; d < 3; ++d) {
				outPositions[3 * s.roiIndices[i] + d] = normalizedSolution[3 * i + d];
			}
		}
	}
	else {
		for (int i = 0; i < s.nRoi; ++i) {
			for (int d = 0; d < 3; ++d) {
				outPositions[3 * s.roiIndices[i] + d] = minimizerSolution[3 * i + d];
			}
		}
	}
}


#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <CGAL/Simple_cartesian.h>
#include <cassert>
#include <vector>
#include <algorithm>
#include <CGAL/point_generators_2.h>
#include <CGAL/algorithm.h>

using namespace CGAL;

typedef Simple_cartesian<double>         K;
typedef K::Point_2                    Point;
typedef Creator_uniform_2<int,Point>  Creator;

void
disk(int N, double radius)
{
  std::cout.precision(12);
  CGAL::Random_points_in_disc_2<Point> rng(radius);
  std::cout << N << std::endl;
  for(double i = 0; i < N; i++){
    std::cout << *rng << std::endl;
    ++rng;
  }
}

int main(int argc, char* argv[])
{
  int N= 10;
  double radius = 1;
  try {

    po::options_description desc("Allowed options");
    desc.add_options()
      ("help", "Generator of perturbed points in a disk")
      ("N", po::value<int>(), "generate N points")
      ("radius", po::value<double>(), "radius of the disc")
      ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }

    if (vm.count("N")) {
      N = vm["N"].as<int>();
    }

    if (vm.count("radius")) {
      radius = vm["radius"].as<double>();
    }
  }
  catch(std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  disk(N, radius);
  return 0;
}


// © 2013 the Search Authors under the MIT license. See AUTHORS for the list of authors.

#include "utils.hpp"
#include "safeops.hpp"
#include <boost/cstdint.hpp>
#include <limits>
#include <cmath>
#include <functional>

void fatal(const char*, ...);

unsigned int ilog2(boost::uint32_t v) {
	if (!v)
		return 0;

	unsigned int lg = 0;

	if (v & 0xFFFF0000) {
		lg += 16;
		v >>= 16;
	}
	if (v & 0xFF00) {
		lg += 8;
		v >>= 8;
	}
	if (v & 0xF0) {
		lg += 4;
		v >>= 4;
	}
	if (v & 0xC) {
		lg += 2;
		v >>= 2;
	}
	if (v & 0x2) {
		lg += 1;
		v >>= 1;
	}
	return lg + 1;
}

unsigned long ipow(unsigned int b, unsigned int e) {
	unsigned long r = 1;
	for (unsigned int i = 0; i < e; i++)
		r *= (unsigned long) b;
	return r;
}

unsigned long fallfact(unsigned int x, unsigned int n) {
	unsigned long f = x;
	for (unsigned int i = 1; i < n; i++)
		f *= (x - i);
	return f;
}

double normcdf(double mu, double sigma, double x) {
	return 0.5 * (1 + erf((x-mu)/sqrt(2*sigma*sigma)));
}

double phi(double x) {
	static const double sqrt2 = sqrt(2);
	return 0.5 * (1 + erf(x/sqrt2));
}

double integrate(std::function<double(double)> getY, double start, double end, double stepsize) {
	double sum = 0;
	double y1 = 0;
	double y2 = getY(start);
	double cur = start;
	double next = start + stepsize;
	for( ; next < end; cur = next, next+=stepsize) {

		y1 = y2;
		y2 = getY(next);

		sum += stepsize * ((y1 + y2) / 2);
	}

	y1 = y2;
	y2 = getY(end);
	next = end;
	sum += (next - cur) * ((y1 + y2) / 2);

	return sum;
}

Normal::Normal(double m, double s) : mean(m), stdev(s) {
	pdfcoeff = 1/(stdev*sqrt(2*M_PI));
	cdfcoeff = 1/(sqrt(2*stdev*stdev));
}

double Normal::pdf(double x) const {
	return pdfcoeff * exp(-(x- mean)*(x-mean) / (2*stdev*stdev));
}

double Normal::cdf(double x) const {
	return 0.5 * (1 + erf((x-mean)*cdfcoeff));
}


#include "quadrature/angular/gauss_legendre.h"

#include <deal.II/base/quadrature_lib.h>

namespace bart {

namespace quadrature {

namespace angular {

GaussLegendre::GaussLegendre(const int n_points)
    : n_points_(n_points) {
  AssertThrow(n_points > 0,
      dealii::ExcMessage("Error in constructor of GaussLegendre, n_points must "
                         "be greater than or equal to 0"))
  this->set_description("Gauss-Legendre quadrature (1D)");
}

std::vector<GaussLegendre::PositionWeightPairType>
    GaussLegendre::GenerateSet() const {
  dealii::QGauss<1> gaussian_quadrature(n_points_);
  std::vector<PositionWeightPairType> return_vector;

  auto points = gaussian_quadrature.get_points();
  auto weights = gaussian_quadrature.get_weights();

  for (int i = 0; i < static_cast<int>(gaussian_quadrature.size()); ++i) {
    CartesianPosition<1> x_position({points.at(i)[0]});
    Weight weight(2*M_PI*weights.at(i));
    return_vector.push_back({x_position, weight});
  }

  return return_vector;
}

} // namespace angular

} // namespace quadrature

} // namespace bart


#include "visualization/gl_shapes.hh"
#include "visualization/opengl.hh"

#include "geometry/geometry.hh"

#include <Eigen/Dense>

namespace sonder {

void draw_coordinate_system() {
  draw_coordinate_system(Eigen::Vector3f::Zero(), Eigen::Matrix3f::Identity());
}

void draw_coordinate_system(const se3 &pose) {
  draw_coordinate_system(pose.translation(), pose.rotationMatrix());
}

void draw_coordinate_system(const Eigen::Vector3f &position, const Eigen::Matrix3f &orientation) {
  glPushMatrix();

  const Eigen::Quaternionf q(orientation);
  glTranslate(position);
  glRotate(q);

  constexpr float narrow_scale = 0.05;
  constexpr float wide_scale   = 0.5;
  // Z
  glPushMatrix();
  glScalef(narrow_scale, narrow_scale, wide_scale);
  glColor3f(0.0, 0.0, 1.0);
  glTranslatef(0.0, 0.0, 1.0);
  draw_cube();
  glPopMatrix();

  // Y
  glPushMatrix();
  glScalef(narrow_scale, wide_scale, narrow_scale);
  glColor3f(0.0, 1.0, 0.0);
  glTranslatef(0.0, 1.0, 0.0);
  draw_cube();
  glPopMatrix();

  // X
  glPushMatrix();
  glScalef(wide_scale, narrow_scale, narrow_scale);
  glColor3f(1.0, 0.0, 0.0);
  glTranslatef(1.0, 0.0, 0.0);
  draw_cube();
  glPopMatrix();

  glPopMatrix();
}

void draw_line(const Eigen::Vector3f &from, const Eigen::Vector3f &to) {
  glBegin(GL_LINES);
  {
    glVertex(from);
    glVertex(to);
  }
  glEnd();
}

void draw_line2d(const Eigen::Vector2f &from, const Eigen::Vector2f &to) {
  glBegin(GL_LINES);
  {
    glVertex2f(from.x(), 1.0 - from.y());
    glVertex2f(to.x(), 1.0 - to.y());
  }
  glEnd();
}

void draw_point2d(const Eigen::Vector2f &point, const float radius) {
  glPushMatrix();
  glTranslatef(point.x(), point.y(), -0.5f);
  glutSolidSphere(radius, 16, 16);
  glPopMatrix();
}

void draw_point(const Eigen::Vector3f &point, const float radius) {
  glPushMatrix();
  glTranslate(point);
  glutSolidSphere(radius, 16, 16);
  glPopMatrix();
}

void draw_circle(const Eigen::Vector3f &normal, const Eigen::Vector3f &center, const float radius) {
  constexpr int   num_vertices   = 100;
  constexpr float scaling_factor = 2.0f * M_PI / num_vertices;

  //
  // Build the circle in 2D on the x-y plane
  //
  Eigen::Array<float, 3, num_vertices> xy_plane_vertices;

  for (int k = 0; k < num_vertices; ++k) {
    const float t            = scaling_factor * static_cast<float>(k);
    xy_plane_vertices.col(k) = Eigen::Vector3f(radius * std::cos(t), radius * std::sin(t), 0.0f);
  }

  //
  // Transform the x-y plane circle to the normal plane
  //

  // A rotation between the Z axis and the circle normal
  const Eigen::Quaternionf rotation = create_rotation_to(Eigen::Vector3f::UnitZ(), normal);

  const se3 transform(rotation, center);

  // Draw the lines (dotted)
  // ((IF SOLID: GL_LINE_STRIP))
  glBegin(GL_LINES);
  {
    for (int k = 0; k < num_vertices; ++k) {
      glVertex(transform * xy_plane_vertices.col(k));
    }
  }
  glEnd();
}

void draw_circle(const circle &ge_circle) {
  draw_circle(ge_circle.normal, ge_circle.center, ge_circle.radius);
}

void draw_circular_section(const circular_section &section) {
  constexpr int num_vertices = 100;

  //
  // Build the circle in 2D on the x-y plane
  //
  Eigen::Array<float, 3, num_vertices> xz_plane_vertices;
  const float angle_per_vertex = section.arc_rads / static_cast<float>(num_vertices);
  for (int k = 0; k < num_vertices; ++k) {
    const float angle = (-section.arc_rads * 0.5f) + k * angle_per_vertex;
    xz_plane_vertices.col(k) =
        Eigen::Vector3f(section.radius * std::cos(angle), 0.0f, section.radius * std::sin(angle));
  }

  //
  // Transform the x-z plane circle to the normal plane
  //
  const Eigen::Quaternionf rotation = rotation_from_xy(section.direction, section.normal);
  const se3                transform(rotation, section.center);

  // Draw the lines (dotted)
  // ((IF SOLID: GL_LINE_STRIP))
  glBegin(GL_LINE_STRIP);
  {
    for (int k = 0; k < num_vertices; ++k) {
      glVertex(transform * xz_plane_vertices.col(k));
    }
  }
  glEnd();
  // glBegin(GL_LINES);
  //{
  //  glVertex(section.center);
  //  glVertex(section.center + (section.direction * section.radius));
  //}
  // glEnd();
}

void draw_line(const line &ge_line) {
  const Eigen::Vector3f scaled_direction = 10.0f * ge_line.direction;
  draw_line(ge_line.point + scaled_direction, ge_line.point - scaled_direction);
}

void draw_plane(const plane &ge_plane) {
  constexpr float       scale   = 5.0f;
  const Eigen::Vector3f x_basis = any_perpendicular(ge_plane.normal).normalized();
  const Eigen::Vector3f y_basis = x_basis.cross(ge_plane.normal).normalized();

  glBegin(GL_QUADS);
  {
    glVertex((scale * x_basis) + ge_plane.point);
    glVertex((scale * y_basis) + ge_plane.point);
    glVertex(-(scale * x_basis) + ge_plane.point);
    glVertex(-(scale * y_basis) + ge_plane.point);
  }
  glEnd();
}

void draw_cube() {
  glBegin(GL_QUADS);
  {
    glVertex3f(-1.0, -1.0, -1.0);
    glVertex3f(-1.0, -1.0, 1.0);
    glVertex3f(-1.0, 1.0, 1.0);
    glVertex3f(-1.0, 1.0, -1.0);

    glVertex3f(-1.0, 1.0, -1.0);
    glVertex3f(-1.0, 1.0, 1.0);
    glVertex3f(1.0, 1.0, 1.0);
    glVertex3f(1.0, 1.0, -1.0);

    glVertex3f(1.0, 1.0, -1.0);
    glVertex3f(1.0, 1.0, 1.0);
    glVertex3f(1.0, -1.0, 1.0);
    glVertex3f(1.0, -1.0, -1.0);

    glVertex3f(1.0, -1.0, 1.0);
    glVertex3f(1.0, -1.0, -1.0);
    glVertex3f(-1.0, -1.0, -1.0);
    glVertex3f(-1.0, -1.0, 1.0);

    glVertex3f(-1.0, -1.0, 1.0);
    glVertex3f(1.0, -1.0, 1.0);
    glVertex3f(1.0, 1.0, 1.0);
    glVertex3f(-1.0, 1.0, 1.0);

    glVertex3f(-1.0, -1.0, -1.0);
    glVertex3f(1.0, -1.0, -1.0);
    glVertex3f(1.0, 1.0, -1.0);
    glVertex3f(-1.0, 1.0, -1.0);
  }
  glEnd();
}

//
// This isn't quite correct
//
void draw_sonar_view(const se3 &pose, const float max_bearing, const float max_elevation) {
  constexpr float range = 20.0f;

  const float x_sym = range * cos(max_bearing);
  const float y_sym = range * sin(max_bearing);
  const float z_sym = range * sin(max_elevation);

  const Eigen::Vector3f symmetric_point(x_sym, y_sym, z_sym);

  // draw_coordinate_system(pose.translation(), pose.rotationMatrix());
  draw_coordinate_system(pose);

  glPushMatrix();
  glTranslate(pose.translation());
  glRotate(pose.unit_quaternion());

  glColor3f(0.9f, 0.0f, 0.0f);
  glBegin(GL_LINE_STRIP);
  {
    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, y_sym, z_sym);
    glVertex3f(x_sym, -y_sym, z_sym);

    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, y_sym, -z_sym);
    glVertex3f(x_sym, -y_sym, -z_sym);
    glVertex3f(0.0, 0.0, 0.0);

    glVertex3f(x_sym, -y_sym, -z_sym);
    glVertex3f(x_sym, -y_sym, z_sym);

    glVertex3f(x_sym, y_sym, z_sym);
    glVertex3f(x_sym, y_sym, -z_sym);
  }
  glEnd();

  glColor4f(0.2f, 0.7f, 0.0f, 0.2f);
  // This does some weird stuff, meh
  glBegin(GL_TRIANGLES);
  {
    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, y_sym, z_sym);
    glVertex3f(x_sym, -y_sym, z_sym);

    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, -y_sym, -z_sym);
    glVertex3f(x_sym, y_sym, -z_sym);

    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, -y_sym, -z_sym);
    glVertex3f(x_sym, -y_sym, z_sym);

    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(x_sym, y_sym, z_sym);
    glVertex3f(x_sym, y_sym, -z_sym);
  }
  glEnd();
  glPopMatrix();
}
}


/* -*-C++-*- */
/*
   (c) Copyright 2007, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    implementation
*/
#include <Lintel/AssertBoost.hpp>
#include <Lintel/LeastSquares.hpp>

#include <boost/format.hpp>

using namespace std;
using boost::format;

LeastSquares::Linear LeastSquares::fitLinearVertical(const WeightedData &data) {
    double sum_w = 0,  sum_wx = 0, sum_wy = 0, sum_wxx = 0, sum_wxy = 0;

    INVARIANT(data.size() > 1, "must have at least 2 points for linear fit");

    for (WeightedData::const_iterator i = data.begin(); i != data.end(); ++i) {
	sum_w += i->weight;
	sum_wx += i->weight * i->x;
	sum_wy += i->weight * i->y;
	sum_wxx += i->weight * i->x * i->x;
	sum_wxy += i->weight * i->x * i->y;
    }

    Linear ret;
    double denom = sum_w * sum_wxx - sum_wx * sum_wx;
    INVARIANT(denom != 0.0, "denominator is 0, aborting");
    ret.slope = (sum_w * sum_wxy - sum_wx * sum_wy) / denom;
    ret.intercept = (sum_wy - ret.slope * sum_wx) / sum_w;

    return ret;
}

void LeastSquares::printText(ostream &to) const {
    for(WeightedData::const_iterator i = data.begin(); i != data.end(); ++i) {
	to << boost::format("%24.18g %24.18g %24.18g\n") % i->x % i->y % i->weight;
    }
}


// inverse_gamma.hpp

//  Copyright Paul A. Bristow 2010.
//  Copyright John Maddock 2010.
//  Use, modification and distribution are 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_STATS_INVERSE_GAMMA_HPP
#define BOOST_STATS_INVERSE_GAMMA_HPP

// Inverse Gamma Distribution is a two-parameter family
// of continuous probability distributions
// on the positive real line, which is the distribution of
// the reciprocal of a variable distributed according to the gamma distribution.

// http://en.wikipedia.org/wiki/Inverse-gamma_distribution
// http://rss.acs.unt.edu/Rdoc/library/pscl/html/igamma.html

// See also gamma distribution at gamma.hpp:
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda366b.htm
// http://mathworld.wolfram.com/GammaDistribution.html
// http://en.wikipedia.org/wiki/Gamma_distribution

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>

#include <utility>

namespace boost{ namespace math
{
namespace detail
{

template <class RealType, class Policy>
inline bool check_inverse_gamma_shape(
      const char* function, // inverse_gamma
      RealType shape, // shape aka alpha
      RealType* result, // to update, perhaps with NaN
      const Policy& pol)
{  // Sources say shape argument must be > 0
   // but seems logical to allow shape zero as special case,
   // returning pdf and cdf zero (but not < 0).
   // (Functions like mean, variance with other limits on shape are checked
   // in version including an operator & limit below).
   if((shape < 0) || !(boost::math::isfinite)(shape))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Shape parameter is %1%, but must be >= 0 !", shape, pol);
      return false;
   }
   return true;
} //bool check_inverse_gamma_shape

template <class RealType, class Policy>
inline bool check_inverse_gamma_x(
      const char* function,
      RealType const& x,
      RealType* result, const Policy& pol)
{
   if((x < 0) || !(boost::math::isfinite)(x))
   {
      *result = policies::raise_domain_error<RealType>(
         function,
         "Random variate is %1% but must be >= 0 !", x, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool check_inverse_gamma(
      const char* function, // TODO swap these over, so shape is first.
      RealType scale,  // scale aka beta
      RealType shape, // shape aka alpha
      RealType* result, const Policy& pol)
{
   return check_scale(function, scale, result, pol)
     && check_inverse_gamma_shape(function, shape, result, pol);
} // bool check_inverse_gamma

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class inverse_gamma_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   inverse_gamma_distribution(RealType shape = 1, RealType scale = 1)
      : m_shape(shape), m_scale(scale)
   {
      RealType result;
      detail::check_inverse_gamma(
        "boost::math::inverse_gamma_distribution<%1%>::inverse_gamma_distribution",
        scale, shape, &result, Policy());
   }

   RealType shape()const
   {
      return m_shape;
   }

   RealType scale()const
   {
      return m_scale;
   }
private:
   //
   // Data members:
   //
   RealType m_shape;     // distribution shape
   RealType m_scale;     // distribution scale
};

typedef inverse_gamma_distribution<double> inverse_gamma;
// typedef - but potential clash with name of inverse gamma *function*.
// but there is a typedef for gamma
//   typedef boost::math::gamma_distribution<Type, Policy> gamma;

// Allow random variable x to be zero, treated as a special case (unlike some definitions).

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const inverse_gamma_distribution<RealType, Policy>& /* dist */)
{  // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(0, max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const inverse_gamma_distribution<RealType, Policy>& /* dist */)
{  // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   using boost::math::tools::min_value;
   return std::pair<RealType, RealType>(0,  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const inverse_gamma_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::pdf(const inverse_gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
   { // distribution parameters bad.
      return result;
   }
   if(x == 0)
   { // Treat random variate zero as a special case.
      return 0;
   }
   else if(false == detail::check_inverse_gamma_x(function, x, &result, Policy()))
   { // x bad.
      return result;
   }
   result = scale / x;
   if(result < tools::min_value<RealType>())
      return 0;  // random variable is infinite or so close as to make no difference.
   result = gamma_p_derivative(shape, result, Policy()) * scale;
   if(0 != result)
   {
      if(x < 0)
      {
         // x * x may under or overflow, likewise our result,
         // so be extra careful about the arithmetic:
         RealType lim = tools::max_value<RealType>() * x;
         if(lim < result)
            return policies::raise_overflow_error<RealType, Policy>(function, "PDF is infinite.", Policy());
         result /= x;
         if(lim < result)
            return policies::raise_overflow_error<RealType, Policy>(function, "PDF is infinite.", Policy());
         result /= x;
      }
      result /= (x * x);
   }
   // better than naive
   // result = (pow(scale, shape) * pow(x, (-shape -1)) * exp(-scale/x) ) / tgamma(shape);
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const inverse_gamma_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::cdf(const inverse_gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
   { // distribution parameters bad.
      return result;
   }
   if (x == 0)
   { // Treat zero as a special case.
     return 0;
   }
   else if(false == detail::check_inverse_gamma_x(function, x, &result, Policy()))
   { // x bad
      return result;
   }
   result = boost::math::gamma_q(shape, scale / x, Policy());
   // result = tgamma(shape, scale / x) / tgamma(shape); // naive using tgamma
   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const inverse_gamma_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING  // for ADL of std functions
   using boost::math::gamma_q_inv;

   static const char* function = "boost::math::quantile(const inverse_gamma_distribution<%1%>&, %1%)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, p, &result, Policy()))
      return result;
   if(p == 1)
   {
      return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = gamma_q_inv(shape, p, Policy());
   if((result < 1) && (result * tools::max_value<RealType>() < scale))
      return policies::raise_overflow_error<RealType, Policy>(function, "Value of random variable in inverse gamma distribution quantile is infinite.", Policy());
   result = scale / result;
   return result;
}

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<inverse_gamma_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const gamma_distribution<%1%>&, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_inverse_gamma_x(function, c.param, &result, Policy()))
      return result;

   //result = 1. - gamma_q(shape, c.param / scale, Policy());
   result = gamma_p(shape, scale/c.param, Policy());
   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<inverse_gamma_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::quantile(const inverse_gamma_distribution<%1%>&, %1%)";

   RealType shape = c.dist.shape();
   RealType scale = c.dist.scale();
   RealType q = c.param;

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
      return result;
   if(false == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 0)
   {
      return policies::raise_overflow_error<RealType>(function, 0, Policy());
   }
   result = gamma_p_inv(shape, q, Policy());
   if((result < 1) && (result * tools::max_value<RealType>() < scale))
      return policies::raise_overflow_error<RealType, Policy>(function, "Value of random variable in inverse gamma distribution quantile is infinite.", Policy());
   result = scale / result;
   return result;
}

template <class RealType, class Policy>
inline RealType mean(const inverse_gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::mean(const inverse_gamma_distribution<%1%>&)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;

   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
     return result;
   }
   if((shape <= 1) || !(boost::math::isfinite)(shape))
   {
     result = policies::raise_domain_error<RealType>(
       function,
       "Shape parameter is %1%, but for a defined mean it must be > 1", shape, Policy());
     return result;
   }
  result = scale / (shape - 1);
  return result;
} // mean

template <class RealType, class Policy>
inline RealType variance(const inverse_gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::variance(const inverse_gamma_distribution<%1%>&)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
      if(false == detail::check_scale(function, scale, &result, Policy()))
   {
     return result;
   }
   if((shape <= 2) || !(boost::math::isfinite)(shape))
   {
     result = policies::raise_domain_error<RealType>(
       function,
       "Shape parameter is %1%, but for a defined variance it must be > 2", shape, Policy());
     return result;
   }
   result = (scale * scale) / ((shape - 1) * (shape -1) * (shape -2));
   return result;
}

template <class RealType, class Policy>
inline RealType mode(const inverse_gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::mode(const inverse_gamma_distribution<%1%>&)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_inverse_gamma(function, scale, shape, &result, Policy()))
   {
      return result;
   }
   // Only defined for shape >= 0, but is checked by check_inverse_gamma.
   result = scale / (shape + 1);
   return result;
}

//template <class RealType, class Policy>
//inline RealType median(const gamma_distribution<RealType, Policy>& dist)
//{  // Wikipedia does not define median,
     // so rely on default definition quantile(0.5) in derived accessors.
//  return result.
//}

template <class RealType, class Policy>
inline RealType skewness(const inverse_gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::skewness(const inverse_gamma_distribution<%1%>&)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();
   RealType result;

   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
     return result;
   }
   if((shape <= 3) || !(boost::math::isfinite)(shape))
   {
     result = policies::raise_domain_error<RealType>(
       function,
       "Shape parameter is %1%, but for a defined skewness it must be > 3", shape, Policy());
     return result;
   }
   result = (4 * sqrt(shape - 2) ) / (shape - 3);
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const inverse_gamma_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function = "boost::math::kurtosis_excess(const inverse_gamma_distribution<%1%>&)";

   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;
   if(false == detail::check_scale(function, scale, &result, Policy()))
   {
     return result;
   }
   if((shape <= 4) || !(boost::math::isfinite)(shape))
   {
     result = policies::raise_domain_error<RealType>(
       function,
       "Shape parameter is %1%, but for a defined kurtosis excess it must be > 4", shape, Policy());
     return result;
   }
   result = (30 * shape - 66) / ((shape - 3) * (shape - 4));
   return result;
}

template <class RealType, class Policy>
inline RealType kurtosis(const inverse_gamma_distribution<RealType, Policy>& dist)
{
  static const char* function = "boost::math::kurtosis(const inverse_gamma_distribution<%1%>&)";
   RealType shape = dist.shape();
   RealType scale = dist.scale();

   RealType result;

  if(false == detail::check_scale(function, scale, &result, Policy()))
   {
     return result;
   }
   if((shape <= 4) || !(boost::math::isfinite)(shape))
   {
     result = policies::raise_domain_error<RealType>(
       function,
       "Shape parameter is %1%, but for a defined kurtosis it must be > 4", shape, Policy());
     return result;
   }
  return kurtosis_excess(dist) + 3;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_INVERSE_GAMMA_HPP


#ifndef MPFR_SUPPORT
#define MPFR_SUPPORT

#include <boost/multiprecision/mpfr.hpp>
#include <eigen3/Eigen/Core>
#define PREC 300  //change this to change precision

typedef boost::multiprecision::number<mpfr_float_backend<PREC> > big_float;

namespace Eigen {
  template<> struct NumTraits<big_float> {

  typedef big_float Real;
  typedef big_float NonInteger;
  typedef big_float Nested;

  enum {
    IsComplex = 0,
    IsInteger = 0,
    IsSigned = 1,
    RequireInitialization = 1,
    ReadCost = 1,
    AddCost = 3,
    MulCost = 3
  };

  };
}

#endif


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file set_test.cpp
/// \brief Basic regression test for set expressions.

#include <boost/test/minimal.hpp>

#include "mcrl2/data/standard.h"
#include "mcrl2/data/set.h"
#include "mcrl2/data/fset.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::data::sort_set;
using namespace mcrl2::data::sort_fset;

template <typename Predicate>
void test_data_expression(const std::string& s, const variable_vector& v, Predicate p)
{
  std::cerr << "testing data expression " << s << std::endl;
  data_expression e = parse_data_expression(s, v);
  BOOST_CHECK(p(e));
}

/* Test case for various set expressions, based
   on the following specification:

proc P(s: Set(Nat)) = (1 in s) -> tau . P({} + s - {20} * {40})
                    + ({10} < s) -> tau . P(!s)
                    + (s <= {20} + Bag2Set({20:4, 30:3, 40:2})) -> tau . P(s)
                    + (s <= { n:Nat | true }) -> tau . P(s);

init P({20, 30, 40});

*/
void set_expression_test()
{
  data::data_specification specification;

  specification.add_context_sort(sort_pos::pos());
  specification.add_context_sort(sort_set::set_(sort_pos::pos()));
  specification.add_context_sort(sort_set::set_(sort_bool::bool_()));

  data::rewriter normaliser(specification);

  variable_vector v;
  v.push_back(parse_variable("s:Set(Nat)"));

  test_data_expression("{x : Nat | x < 10}", v, sort_set::is_constructor_application);
  test_data_expression("!s", v, sort_set::is_complement_application);
  test_data_expression("s * {}", v, sort_set::is_intersection_application);
  test_data_expression("s * {1,2,3}", v, sort_set::is_intersection_application);
  test_data_expression("s - {3,1,2}", v, sort_set::is_difference_application);
  test_data_expression("1 in s", v, sort_set::is_in_application);
  test_data_expression("{} + s", v, sort_set::is_union_application);
  test_data_expression("(({} + s) - {20}) * {40}", v, sort_set::is_intersection_application);
  test_data_expression("{10} < s", v, is_less_application<data_expression>);
  test_data_expression("s <= {10}", v, is_less_equal_application<data_expression>);
  test_data_expression("{20} + {30}", v, sort_set::is_union_application);

  data_expression t1d1 = parse_data_expression("{1,2}");
  data_expression t1d2 = parse_data_expression("{2,1}");
  BOOST_CHECK(normaliser(t1d1) == normaliser(t1d2));

  data_expression t1d1a = parse_data_expression("{1,2,3}");
  data_expression t1d2a = parse_data_expression("{2,1}");
  BOOST_CHECK(normaliser(t1d1a) != normaliser(t1d2a));

  data_expression t2d1 = parse_data_expression("{1,2} == {1,2}");
  data_expression t2d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t2d1) == normaliser(t2d2));

  data_expression t2d1a = parse_data_expression("{1,2,3} == {1,2,1}");
  data_expression t2d2a = parse_data_expression("false");
  BOOST_CHECK(normaliser(t2d1a) == normaliser(t2d2a));

  data_expression t3d1 = parse_data_expression("({1,2} != {2,3})");
  data_expression t3d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t3d1) == normaliser(t3d2));

  data_expression t4d1 = parse_data_expression("(!{1,2}) == {1,2}");
  data_expression t4d2 = parse_data_expression("false");
  BOOST_CHECK(normaliser(t4d1) == normaliser(t4d2));

  data_expression t5d1 = parse_data_expression("(!!{1,2}) == {2,1}");
  data_expression t5d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t5d1) == normaliser(t5d2));

  data_expression e = parse_data_expression("{20}", v);
  BOOST_CHECK(sort_fset::is_cons_application(normaliser(e)));

  e = parse_data_expression("{20, 30, 40}", v);
  BOOST_CHECK(sort_fset::is_cons_application(normaliser(e)));

  data_expression t6d1 = parse_data_expression("{} == { b: Bool | true } - { true, false }");
  data_expression t6d2 = parse_data_expression("true");

  BOOST_CHECK(normaliser(t6d1) == normaliser(t6d2));

  data_expression t7d1 = parse_data_expression("{ b: Bool | true } - { true, false } == {}");
  data_expression t7d2 = parse_data_expression("true");
  BOOST_CHECK(normaliser(t7d1) == normaliser(t7d2));

}

int test_main(int argc, char** argv)
{
  set_expression_test();

  return EXIT_SUCCESS;
}


#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

Eigen::MatrixXd read_matrix(std::string file_name);

int main(int argc, char *argv[]) {
    if (argc < 2) {
        std::cerr << "# error: no file specified" << std::endl;
        return 1;
    }
    Eigen::MatrixXd A = read_matrix(std::string(argv[1]));
    std::cout << "A =" << std::endl << A << std::endl;
    return 0;
}

Eigen::MatrixXd read_matrix(std::string file_name) {
    std::size_t nr_rows {0};
    std::vector<double> elements;
    std::size_t nr_cols {0};
    std::ifstream ifs(file_name);
    std::string line;
    while (std::getline(ifs, line)) {
        std::stringstream str(line);
        double val;
        std::size_t nr_vals {0};
        while (str >> val) {
            elements.push_back(val);
            ++nr_vals;
        }
        if (nr_cols == 0) {
            nr_cols = nr_vals;
        } else  if (nr_vals > 0 && nr_vals != nr_cols) {
            std::cerr << "Error: incorrect file format" << std::endl;
            std::exit(1);
        }
        if (nr_vals > 0)
            ++nr_rows;
    }
    ifs.close();
    Eigen::MatrixXd matrix(nr_rows, nr_cols);
    for (long i = 0; i < matrix.rows(); ++i)
        for (long j = 0; j < matrix.cols(); ++j)
            matrix(i, j) = elements[i*matrix.cols() + j];
    return matrix;
}