#include <string>
#include <unordered_map>
#include <Eigen/Dense>
#include <cmath>
#include "../include/optimizer.h"

namespace MyDL
{

    // ------------------------------------------------------------
    //                  SGD
    // ------------------------------------------------------------
    SGD::SGD(double learning_rate): _learning_rate(learning_rate)
    {
    }

    void SGD::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        for (auto grad : grads)
        {
            params[grad.first] -= _learning_rate * grad.second;
        }
    }

    void SGD::update(unordered_map<string, shared_ptr<MatrixXd>> & params, unordered_map<string, MatrixXd> &grads)
    {
        for (auto grad : grads)
        {
            *(params[grad.first]) -= _learning_rate * grad.second;
        }
    }

    // ------------------------------------------------------------
    //                  Momentum
    // ------------------------------------------------------------
    Momentum::Momentum(double learning_rate, double momentum) : _learning_rate(learning_rate), _momentum(momentum)
    {
    }

    void Momentum::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_v.empty())
        {
            for (auto param : params)
            {
                _v[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            _v[key] = _momentum * _v[key] - _learning_rate * grads[key];
            params[key] += _v[key];
        }
    }

    void Momentum::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_v.empty())
        {
            for (auto param : params)
            {
                MatrixXd tmp_mat = *(param.second);
                _v[param.first] = MatrixXd::Zero(tmp_mat.rows(), tmp_mat.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            _v[key] = _momentum * _v[key] - _learning_rate * grads[key];
            *(params[key]) += _v[key];
        }
    }

    // ------------------------------------------------------------
    //                  AdaGrad
    // ------------------------------------------------------------
    AdaGrad::AdaGrad(double learning_rate) : _learning_rate(learning_rate)
    {
    }

    void AdaGrad::update(unordered_map<string, MatrixXd> &params, unordered_map<string, MatrixXd> & grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // 勾配変化の大きかったパラメータの二乗平方根で割る → 要は勾配の絶対値が大きかったパラメータの更新幅を小さくするということ
            // ※ 厳密には勾配の絶対値で割っているわけではない(直前の内部状態に加算して平方根を取っている)
            _h[key].array() += grads[key].array() * grads[key].array();
            params[key].array() -= _learning_rate * _h[key].unaryExpr([](double p){return 1/(sqrt(p)+1e-7);}).array() * grads[key].array();
        }
    }

    void AdaGrad::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                MatrixXd tmp_mat = *(param.second);
                _h[param.first] = MatrixXd::Zero(tmp_mat.rows(), tmp_mat.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // 勾配変化の大きかったパラメータの二乗平方根で割る → 要は勾配の絶対値が大きかったパラメータの更新幅を小さくするということ
            // ※ 厳密には勾配の絶対値で割っているわけではない(直前の内部状態に加算して平方根を取っている)
            _h[key].array() += grads[key].array() * grads[key].array();
            params[key]->array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }
    }

    // ------------------------------------------------------------
    //                  RMSProp
    // ------------------------------------------------------------
    RMSprop::RMSprop(double learning_rate, double decay_rate): _learning_rate(learning_rate), _decay_rate(decay_rate)
    {
    }

    void RMSprop::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // AdaGradの内部状態hを指数移動平均に置き換えたものがRMSprop -> 指数移動平均の減衰率(平滑化係数)がdecay_rate
            _h[key].array() *= _decay_rate;
            _h[key].array() += (1 - _decay_rate) * grads[key].array() * grads[key].array();
            params[key].array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }
    }

    void RMSprop::update(unordered_map<string, shared_ptr<MatrixXd>> &params, unordered_map<string, MatrixXd> &grads)
    {
        if (_h.empty())
        {
            for (auto param : params)
            {
                _h[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
            }
        }

        for (auto param : params)
        {
            string key = param.first;
            // AdaGradの内部状態hを指数移動平均に置き換えたものがRMSprop -> 指数移動平均の減衰率(平滑化係数)がdecay_rate
            _h[key].array() *= _decay_rate;
            _h[key].array() += (1 - _decay_rate) * grads[key].array() * grads[key].array();
            params[key]->array() -= _learning_rate * _h[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array() * grads[key].array();
        }

    }

    // ------------------------------------------------------------
    //                  Adam
    // ------------------------------------------------------------
    Adam::Adam(double learning_rate, double beta1, double beta2): _learning_rate(learning_rate), _beta1(beta1), _beta2(beta2)
    {
        _iter = 0;
    }

    void Adam::update(unordered_map<string, MatrixXd>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_m.empty())
        {
            for (auto param : params)
            {
                _m[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
                _v[param.first] = MatrixXd::Zero(param.second.rows(), param.second.cols());
            }
        }

        _iter++;
        double lr_t = _learning_rate * sqrt(1.0 - std::pow(_beta2, _iter)) / (1.0 - std::pow(_beta1, _iter));

        for (auto param : params)
        {
            string key = param.first;
            _m[key] += (1 - _beta1) * (grads[key] - _m[key]);
            _v[key] += (1 - _beta2) * (grads[key].unaryExpr([](double p){return p*p;}) - _v[key]);

            params[key].array() -= lr_t * _m[key].array() * _v[key].unaryExpr([](double p){return 1 / (sqrt(p) + 1e-7);}).array();
        }
    }

    void Adam::update(unordered_map<string, shared_ptr<MatrixXd>>& params, unordered_map<string, MatrixXd>& grads)
    {
        if (_m.empty())
        {
            for (auto param : params)
            {
                _m[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
                _v[param.first] = MatrixXd::Zero(param.second->rows(), param.second->cols());
            }
        }

        _iter++;
        double lr_t = _learning_rate * sqrt(1.0 - std::pow(_beta2, _iter)) / (1.0 - std::pow(_beta1, _iter));

        for (auto param : params)
        {
            string key = param.first;
            _m[key] += (1 - _beta1) * (grads[key] - _m[key]);
            _v[key] += (1 - _beta2) * (grads[key].unaryExpr([](double p) { return p * p; }) - _v[key]);

            params[key]->array() -= lr_t * _m[key].array() * _v[key].unaryExpr([](double p) { return 1 / (sqrt(p) + 1e-7); }).array();
        }

    }

}


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// bilinear interpolation
inline float get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth, const Eigen::Matrix3d& K)
{
    return Eigen::Vector3d(depth * (p.x() - K(0, 2)) / K(0, 0),
                           depth * (p.y() - K(1, 2)) / K(1, 1),
                           depth);
}

using namespace Sophus;
// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta);
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct PhotometricError: public ceres::SizedCostFunction<9, 7>
{
    PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K, int half_w_size)
    : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K), _half_w_size(half_w_size) {}

    virtual bool Evaluate(double const* const *params,
                          double *residuals,
                          double **jacobians) const {
        const Eigen::Map<const Sophus::SE3d> Rt(params[0]);

        Eigen::Vector3d P2 = Rt * _P1;
        Eigen::Vector3d p2 = _K * P2;
        p2 /= p2.z();

        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X = P2.x();
        double Y = P2.y();
        double Z = P2.z();
        double X2 = X * X;
        double Y2 = Y * Y;
        double Z2 = Z * Z;

        double x3 = _P1.x();
        double y3 = _P1.y();
        double z3 = _P1.z();
        double qx = params[0][0];
        double qy = params[0][1];
        double qz = params[0][2];
        double qw = params[0][3];

        if (P2.z() < 0) // invalid depth
        {
            // fill residuals and jacobians to 0
            for (int i = 0; i < 9; ++i)
            {
                residuals[i] = 0;
                if (jacobians!=nullptr && jacobians[0]!=nullptr)
                {
                    for (int j = 0; j < 7; ++j)
                    {
                        jacobians[0][i*7+j] = 0.0;
                    }
                }
            }
            return true;
        }

        if (p2.x() < _half_w_size || p2.x() > _img2.cols - _half_w_size
            || p2.y() < _half_w_size || p2.y() > _img2.rows - _half_w_size)
        {
            // fill residuals and jacobians to 0
            for (int i = 0; i < 9; ++i)
            {
                residuals[i] = 0;
                if (jacobians!=nullptr && jacobians[0]!=nullptr)
                {
                    for (int j = 0; j < 7; ++j)
                    {
                        jacobians[0][i*7+j] = 0.0;
                    }
                }
            }
            return true;
        }

        int cnt = 0;
        for (int xx = -_half_w_size; xx <= _half_w_size; ++xx)
        {
            for (int yy = -_half_w_size; yy <= _half_w_size; ++yy)
            {

                double v1 = get(_img1, _p1.x() + xx, _p1.y() + yy);
                double v2 = get(_img2,  p2.x() + xx, p2.y() + yy);
                double err = v1 - v2;
                residuals[cnt] = err;

                if (jacobians && jacobians[0])
                {
                    double dx = 0.5 * (get(_img2, p2.x() + xx + 1, p2.y() + yy) - get(_img2, p2.x() + xx - 1, p2.y() + yy));
                    double dy = 0.5 * (get(_img2, p2.x() + xx, p2.y() + yy + 1) - get(_img2, p2.x() + xx, p2.y() + yy - 1));
                    Eigen::Vector2d dIdu(dx, dy);

                    Eigen::Matrix<double, 2, 3> dudXc;
                    dudXc << fx / Z, 0.0, -X * fx / Z2,
                            0.0, fy / Z, -Y * fy / Z2;

                    Eigen::Matrix<double, 3, 4> dXcdq; // derivative of Xcam wrt. quaternions
                    dXcdq(0, 0) = 2*qy*y3 + 2*qz*z3;
                    dXcdq(0, 1) = 2*qw*z3 + 2*qx*y3 - 4*qy*x3;
                    dXcdq(0, 2) = -2*qw*y3 + 2*qx*z3 - 4*qz*x3;
                    dXcdq(0, 3) = 2*qy*z3 - 2*qz*y3;

                    dXcdq(1, 0) = -2*qw*z3 - 4*qx*y3 + 2*qy*x3;
                    dXcdq(1, 1) = 2*qx*x3 + 2*qz*z3;
                    dXcdq(1, 2) = 2*qw*x3 + 2*qy*z3 - 4*qz*y3;
                    dXcdq(1, 3) = -2*qx*z3 + 2*qz*x3;

                    dXcdq(2, 0) = 2*qw*y3 - 4*qx*z3 + 2*qz*x3;
                    dXcdq(2, 1) = -2*qw*x3 - 4*qy*z3 + 2*qz*y3;
                    dXcdq(2, 2) = 2*qx*x3 + 2*qy*y3;
                    dXcdq(2, 3) = 2*qx*y3 - 2*qy*x3;

                    Eigen::Matrix<double, 1, 7, Eigen::RowMajor> J;
                    J.block<1, 4>(0, 0) = -dIdu.transpose() * dudXc * dXcdq;
                    J.block<1, 3>(0, 4) = -dIdu.transpose() * dudXc;

                    for (int i = 0; i < 7; ++i)
                    {
                        jacobians[0][cnt*7 + i] = J(0, i);
                    }
                }
                ++cnt;
            }
        }

        return true;
    }

    private:
        cv::Mat _img1, _img2;
        Eigen::Vector2d _p1;
        Eigen::Vector3d _P1;
        Eigen::Matrix3d _K;
        int _half_w_size;

};

// TO TEST with autodiff or numeric diff, but not easy to differentiate wrt. image (maybe try to combine Jets + image gradient with chain rule)
// struct PhotometricError
// {
//     PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K, int half_w_size)
//     : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K), _half_w_size(half_w_size) {}

//     virtual bool operator() (const double* const params,
//                              double *residuals) const {
//         const Eigen::Map<const Sophus::SE3d> Rt(params);

//         Eigen::Vector3d P2 = Rt * _P1;
//         Eigen::Vector3d p2 = _K * P2;
//         p2 /= p2.z();

//         int j = 0;
//         for (int xx = -_half_w_size; xx <= _half_w_size; ++xx)
//         {
//             for (int yy = -_half_w_size; yy <= _half_w_size; ++yy)
//             {
//                 double v1 = get(_img1, _p1.x() + xx, _p1.y() + yy);
//                 double v2 = get(_img2, p2.x() + xx, p2.y() + yy);
//                 // debug << _p1.x() << " " << _p1.y() << " " << p2.x() << " " << p2.y() << "\n";
//                 debug << v1 << " " << v2 << "\n";
//                 double err = v1 - v2;
//                 residuals[j++] = err;
//             }
//         }

//         return true;
//     }

//     private:
//         cv::Mat _img1, _img2;
//         Eigen::Vector2d _p1;
//         Eigen::Vector3d _P1;
//         Eigen::Matrix3d _K;
//         int _half_w_size;
// };

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 11;
    int half_w_size = 1;

    ceres::Problem problem;

    problem.AddParameterBlock(Rt.data(), 7, new LocalParameterizationSE3());
    for (int i = 0; i < px_ref.size(); ++i)
    {
        const auto& p1 = px_ref[i];
        Eigen::Vector3d P1 = get_3D_point_from_depth(p1, depth_ref[i], K);
        problem.AddResidualBlock(
            // new ceres::NumericDiffCostFunction<PhotometricError, ceres::CENTRAL, 9, 7>(
            //     new PhotometricError(img1, img2, p1, P1, K, half_w_size)
            // ),
            new PhotometricError(img1, img2, p1, P1, K, half_w_size),
            nullptr,
            Rt.data()
        );
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = false;
    options.max_num_iterations = 11;

    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
    cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;

    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 4;
    double factor = 0.5;

    std::vector<cv::Mat> pyr1, pyr2;
    std::vector<double> scales;
    for (int i = 0; i < nb_levels; ++i)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            scales.push_back(1.0);
        }
        else
        {
            cv::Mat img1_r, img2_r;
            cv::resize(pyr1[i-1], img1_r, cv::Size(pyr1[i-1].cols * factor, pyr1[i-1].rows * factor));
            cv::resize(pyr2[i-1], img2_r, cv::Size(pyr2[i-1].cols * factor, pyr2[i-1].rows * factor));
            pyr1.push_back(img1_r);
            pyr2.push_back(img2_r);
            scales.push_back(scales[i-1] * factor);
        }
    }

    for (int l = nb_levels-1; l >= 0; l--)
    {

        cv::Mat img1_r = pyr1[l];
        cv::Mat img2_r = pyr2[l];
        double scale = scales[l];

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);
    }
}

int main(int argc, char **argv) {

Sophus::SE3d r;
Eigen::Matrix<double, 6, 1> d;
d << 1, 2, 3, 0, 0, 0;
r =  Sophus::SE3d::exp(d) * r;
double* rr = r.data();
for (int i = 0; i<7;++i)
std::cout << rr[i] << " ";
cout << "\n\n";

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng(1994);
    int nPoints = 2000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i], K);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0 && p_cur[0] < img2_show.cols && p_cur[1] < img2_show.rows) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        // cv::imshow("current", img2_show);
        // cv::waitKey();
        cv::imwrite("img_"+std::to_string(i) + "_ceres.png", img2_show);

    }
    // debug.close();
    return 0;
}


#include "plane_param.h"

#include <Eigen/Dense>

PlaneParam::PlaneParam()
{}

PlaneParam::PlaneParam(
		const Eigen::Vector3d& v0,
		const Eigen::Vector3d& v1,
		const Eigen::Vector3d& v2) {

	Eigen::Vector3d V0(v0[0] * v0[2], v0[1] * v0[2], v0[2]);
	Eigen::Vector3d V1(v1[0] * v1[2], v1[1] * v1[2], v1[2]);
	Eigen::Vector3d V2(v2[0] * v2[2], v2[1] * v2[2], v2[2]);
	/*
	Eigen::Vector3d V0(v0);
	Eigen::Vector3d V1(v1);
	Eigen::Vector3d V2(v2);
	*/
	Eigen::Vector3d norm = (V1 - V0).cross(V2 - V0);
	if (norm.norm() < 1e-10 || norm[2] == 0) {
		invalid_ = true;
	}
	norm.normalize();
	d_ = -V0.dot(norm);
	n1_ = norm[0];
	n2_ = norm[1];
	n3_ = norm[2];
	invalid_ = false;
	if (d_ == 0)
		invalid_ = true;
}

bool PlaneParam::ProjectiveIntersection(const PlaneParam& other, LineSegment* l) const {
	if (invalid_ || other.invalid_)
		return false;
	//figure out the intersection line direction
	l->n1 = n2_ * other.n3_ - n3_ * other.n2_;
	l->n2 = n3_ * other.n1_ - n1_ * other.n3_;
	l->n3 = n1_ * other.n2_ - n2_ * other.n1_;

	//figure out one intersection point
	if (l->n1 != 0) {
		l->x = 0;
		l->z = (other.n2_ * d_ - n2_ * other.d_) / l->n1;
		l->y = (-other.n3_ * d_ + n3_ * other.d_) / l->n1;
	}
	else if (l->n2 != 0) {
		l->y = 0;
		l->x = (other.n3_ * d_ - n3_ * other.d_) / l->n2;
		l->z = (-other.n1_ * d_ + n1_ * other.d_) / l->n2;
	}
	else {
		return false;
	}

	if (l->x == 0 && l->y == 0)
		return false;

	if (l->z == 0) {
		l->x += l->n1;
		l->y += l->n2;
		l->z += l->n3;
	}
	K nx, ny;
	if (l->z + l->n3 != 0) {
		nx = (l->x+l->n1) / (l->z+l->n3);
		ny = (l->y+l->n2) / (l->z+l->n3);
	} else if (l->z - l->n3 != 0) {
		nx = (l->x-l->n1) / (l->z-l->n3);
		ny = (l->y-l->n2) / (l->z-l->n3);
	} else {
		return false;
	}

	l->x /= l->z;
	l->y /= l->z;
	l->n1 = nx - l->x;
	l->n2 = ny - l->y;
	return true;
}


#include <stdio.h>  /* for sprintf */
#include <stdlib.h> /* for strtod */
#include <string>
#include <cassert>
#include <iostream>
#include <fstream>
#include <math.h>
#include <time.h>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>
#include <boost/unordered_map.hpp>

using namespace std;
using namespace boost;

const float pi = acos(-1);

int numTerms;
int numVars;
int Order;
vector<vector<double> > multipliers;
vector<vector<double> > obsRanges;

/********** GDK's Fourier Code (Ported from Java) **************/
/**                                                                                * This method iterates through a coefficient vector                                                                                * up to a given degree. (like counting in a base of                                                                                * that degree).                                                                                *                                                                                * @param cthe coefficient vector.                                                                                * @param NVariablesthe number of variables in c.                                                                                * @param Degreethe degree up to which to increment.                                                                                */
void Iterate(int *c, int NVariables, int Degree)
{
  c[NVariables - 1] = c[NVariables-1] + 1;

  if(c[NVariables - 1] > Degree)
    {
      if(NVariables > 1)
        {
          c[NVariables - 1]  = 0;
          Iterate(c, NVariables - 1, Degree);
        }
    }
}

/*
 * Compute the full Fourier Basis coefficient matrix for
 * a given number of variables up to a given order.
 *
 * @param nvarsthe number of variables.
 * @param orderthe highest coefficient value for any individual variable.
 * @returna two dimensional array of doubles. The first dimension length is
 * the number of basis functions, and the second is the number of state variables.
 */
void computeFourierCoefficients(int nvars, int order) {
  int nterms = (int)pow(order + 1.0, nvars);
  numTerms = nterms;
  numVars = nvars;
  Order = order;

  int pos = 0;
  multipliers.resize(nterms);
  for (int i=0; i<nterms; i++)
    multipliers[i].resize(nvars);
  int *c = new int[nvars];
  for(int j = 0; j < nvars; j++)
    c[j] = 0;

  do
    {
      for(int k = 0; k < nvars; k++)
        {
          multipliers[pos][k] = c[k];
        }

      pos++;
      // Iterate c
      Iterate(c, nvars, order);
    }
  while(c[0] <= order);
}

/**
 * Scale a state variable to between 0 and 1.
 * (this is required for the Fourier Basis).
 *
 * @param valthe state variable.
 * @param posthe state variable number.
 * @returnthe normalized state variable.
 */
double scale(double val, int pos)
{
  //  cout << pos << "," << val << " " << obsRanges[pos][0] << "," << obsRanges[pos][1] << " : " << (val - obsRanges[pos][0])/(obsRanges[pos][1] - obsRanges[pos][0]) << endl;
  return (val - obsRanges[pos][0]) / (obsRanges[pos][1] - obsRanges[pos][0]);
}

/**
 * Compute the feature vector for a given state.
 * This is achieved by evaluating each Fourier Basis function
 * at that state.
 *
 * @param sthe state in question.
 * @return a vector of doubles representing each basis function evaluated at s.
 */
vector<double> computeFeatures(vector<double> features)
{
  vector<double> phi(numTerms);
  for(int pos = 0; pos < numTerms; pos++)
    {
      double dsum = 0;
      for(int j = 0; j < numVars; j++)
        {
          double sval = scale(features[j], j);
          dsum += sval*multipliers[pos][j];
        }

      phi[pos] = cos((pi) * dsum);
    }

  return phi;
}

  typedef boost::unordered_map<std::string, int> map;

int main(int argc, char **argv) {
  char input[4096];
  char_separator<char> sep(", "); // lets start with csv and move to VW input later
  char_separator<char> vwsep(":");
  double order = lexical_cast<int>(*++argv);
  //  vector<string> features;
  int vw_offset = 0;
  bool skip_normal = false;
  boost::unordered_map<string, int> features;
  // Default to CSV conversion
  // Take parameter flag: --vw
  // to switch to vw input based
  while(*++argv) {
      if(strcmp(*argv, "--vw") == 0)
          vw_offset = 1;
      else if(strcmp(*argv, "--nonorm") == 0)
  	  skip_normal = true;
      else {
	cout << "Unknown argument: " << *argv << endl;
	exit(1);
      }
  }

  //cout << "Fourier Order: " << order << endl;
  int line_count = 0;
  bool translating = false;

  while(!cin.eof()) {
    bool line_has_data = false;
    cin.getline(input,4096);

    string line(input);
    tokenizer< char_separator<char> > tokens(line, sep);
    vector<double> vanilla_features;
    int feature_counter = 0;

    if(vw_offset > 0 && line_count > 0)  {
      vanilla_features.resize(features.size());
    }
    BOOST_FOREACH (const string& t, tokens) {
      try {
	if(line_count == 0) {
	  obsRanges.push_back(vector<double>(2));
	  obsRanges[feature_counter][0] = 0.0;
	  obsRanges[feature_counter][1] = 1.0;
	}
	// min then max then features
	if(line_count < vw_offset) { // first line, and we ARE doing VW inputs
	  features[t] = feature_counter;
	} else if(line_count == vw_offset && !skip_normal) { // ready for the min-max values
	  obsRanges[feature_counter][0] = lexical_cast<double>(t);
	} else if(line_count == vw_offset+1 && !skip_normal) {
	  obsRanges[feature_counter][1] = lexical_cast<double>(t);
	} else {
	  translating = true;
	  if(vw_offset > 0) {
	    boost::tokenizer< char_separator<char> > vwtoken(t, vwsep);
	    boost::tokenizer< char_separator<char> >::iterator beg=vwtoken.begin();
	    string fname(*beg);
	    if(features.find(*beg) != features.end()) {
	      int findex = features.at(*beg);
	      vanilla_features[findex] = lexical_cast<double>(*++beg);
	    } else {
	      cout << t << " ";
	      feature_counter--;
	      line_has_data = true;
	    }
	  } else {
	    vanilla_features.push_back(lexical_cast<double>(t));
	  }
	  if(feature_counter >= 0 && (vanilla_features[feature_counter] < obsRanges[feature_counter][0] || vanilla_features[feature_counter] > obsRanges[feature_counter][1])) {
	    cout << "ERROR: feature " << feature_counter << " is out of supplied range"<<endl;
	    exit(1);
	  } //else cout << vanilla_features[feature_counter] << endl;
	}
      }
      catch(bad_lexical_cast&)
	{}

      feature_counter++;
    }
    //    cout << "Counts: " << feature_counter << " " << line_count << " " << vanilla_features.size() << endl;
    //head ../uk3day/train.vw | sed '1d' | ./fourie 3 --vw
    if(line_count == 0) {
      if(vw_offset > 0 && features.size() == 0) {
	cout << "ERROR: feature labels not provided" << endl;
	exit(1);
      }
      //      cout << feature_counter << endl;
      //cout << "Num Features: " << feature_counter << endl;
      computeFourierCoefficients(feature_counter, order);
      //      cout << "Num Fourier Terms: " << numTerms << endl;
    }

    if(translating && feature_counter > 0) {
      //cout << "Computing features... " <<endl;
      vector<double> fourie_features = computeFeatures(vanilla_features);
      if(vw_offset > 0) {
	cout <<"FOURIER0:" << fourie_features[0] << " ";
	for(int i=1; i<(int)fourie_features.size(); i++) {
	  cout << "FOURIER" << i << ":" << fourie_features[i] << " ";
	}
	cout << endl;
      } else {
	cout << fourie_features[0];
	for(int i=1; i<(int)fourie_features.size(); i++) {
	  cout << "," << fourie_features[i];
	}
	cout << endl;
      }
    } else if(line_has_data) {
      cout << endl;
    }
    line_count++;
  }
  return 0;
}


#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <stdexcept>
#include <string>

#ifndef CPPDEBUG /* Ubuntu's Boost does not provide binaries compatible with libstdc++'s debug mode so we just reduce functionality here */
#include <boost/program_options.hpp>
#endif

#include "boost_profile.cpp"
#include "libiop/algebra/fft.hpp"
#include "libiop/algebra/fields/gf64.hpp"
#include "libiop/algebra/fields/gf128.hpp"
#include "libiop/algebra/fields/gf192.hpp"
#include "libiop/algebra/fields/gf256.hpp"
#include "libiop/algebra/field_subset/subspace.hpp"
#include "libiop/common/profiling.hpp"

#ifndef CPPDEBUG
bool process_prover_command_line(const int argc, const char** argv,
                                 std::size_t &log_n_min,
                                 std::size_t &log_n_max,
                                 std::size_t &field_size)
{
    namespace po = boost::program_options;

    try
    {
        po::options_description desc("Usage");
        desc.add_options()
        ("help", "print this help message")
        ("log_n_min", po::value<std::size_t>(&log_n_min)->default_value(8))
        ("log_n_max", po::value<std::size_t>(&log_n_max)->default_value(20))
        ("field_size", po::value<std::size_t>(&field_size)->default_value(64));

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

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

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libiop;

template<typename FieldT>
void instrument_algebra(std::size_t log_n_min,
                        std::size_t log_n_max)
{
    if (log_n_min % 2 != 0)
    {
        log_n_min += 1;
    }
    if (log_n_max % 2 != 0)
    {
        log_n_max += 1;
    }

    for (std::size_t log_n = log_n_min; log_n <= log_n_max; log_n += 2)
    {
        print_separator();
        const std::size_t n = 1ul << log_n;
        print_indent(); printf("* size of n: %zu\n", n);
        const std::size_t sqrt_n = 1ul << (log_n / 2);
        print_indent(); printf("* size of sqrt(n): %zu\n", sqrt_n);

        /* FFT(n) */
        std::vector<FieldT> n_vec;
        enter_block("n");
        for (size_t i = 0; i < n; ++i)
        {
            n_vec.push_back(FieldT::random_element());
        }
        leave_block("n");
        affine_subspace<FieldT> n_subspace = linear_subspace<FieldT>::standard_basis(libiop::log2(n));
        enter_block("FFT(n)");
        std::vector<FieldT> fft_results = additive_FFT<FieldT>(n_vec, n_subspace);
        leave_block("FFT(n)");

        /* sqrt(n) * FFT(sqrt(n)) */
        std::vector<FieldT> sqrt_n_vec;
        enter_block("sqrt(n)");
        for (size_t i = 0; i < sqrt_n; ++i)
        {
            sqrt_n_vec.push_back(FieldT::random_element());
        }
        leave_block("sqrt(n)");
        affine_subspace<FieldT> sqrt_n_subspace = linear_subspace<FieldT>::standard_basis(libiop::log2(sqrt_n));
        enter_block("sqrt(n) * FFT(sqrt(n))");
        for (size_t i = 0; i < sqrt_n; ++i)
        {
            std::vector<FieldT> sqrt_results = additive_FFT<FieldT>(sqrt_n_vec, sqrt_n_subspace);
        }
        leave_block("sqrt(n) * FFT(sqrt(n))");
    }
}

int main(int argc, const char * argv[])
{
    std::size_t log_n_min;
    std::size_t log_n_max;
    std::size_t field_size;

#ifdef CPPDEBUG
    /* set reasonable defaults */
    if (argc > 1)
    {
        printf("There is no argument parsing in CPPDEBUG mode.");
        exit(1);
    }
    libiop::UNUSED(argv);

    log_n_min = 8;
    log_n_max = 20;
    field_size = 64;
#else
    if (!process_prover_command_line(argc, argv, log_n_min, log_n_max, field_size))
    {
        return 1;
    }
#endif

    printf("Selected parameters:\n");
    printf("* log_n_min = %zu\n", log_n_min);
    printf("* log_n_max = %zu\n", log_n_max);

    switch (field_size)
    {
        case 64:
            instrument_algebra<gf64>(log_n_min, log_n_max);
            break;
        case 128:
            instrument_algebra<gf128>(log_n_min, log_n_max);
            break;
        case 192:
            instrument_algebra<gf192>(log_n_min, log_n_max);
            break;
        case 256:
            instrument_algebra<gf256>(log_n_min, log_n_max);
            break;
        default:
            throw std::invalid_argument("Field size not supported.");
    }
}


#include "filter.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <math.h>

using namespace Eigen;
using namespace std;

inline double Gauss(double sigma, double x)
{
	double expVal = -1 * (pow(x, 2) / (2*pow(sigma, 2)));
	double divider = sqrt(2 * PI * pow(sigma, 2));
	return (1 / divider) * exp(expVal);
}

// a : signal , b : filter
inline MatrixXd Conv(MatrixXd a, MatrixXd b)
{
	int size = a.rows();
	int sizeb = b.rows();
	int window(0);
	MatrixXd out(size, 1);
	double sum(0);

	for (int i = 0; i < size; i++) {
		if (i >= sizeb) window = sizeb;
		else  window = i;

		for (int j = 0; j < window; j++) {
			sum += b(j, 0) * a(i - j);
		}
		out(i, 0) = sum;
		sum = 0;
	}

	return out;
}

Filter::Filter()
	: sum_kernel_(0.0), weight_(0.0)
{
	kernel_.resize(kkernelLevel, 1);
}

Filter::~Filter()
{}

void Filter::KernelMake(int samples, double sigma)
{
	kernel_.resize(samples, 1);
	bool doubleCenter = false;
	if (kkernelLevel % 2 == 0) {
		doubleCenter = true;
		samples--;
	}

	int steps = (samples - 1) / 2;
	double stepSize = (3 * sigma) / steps;

	for (int i = steps; i >= 1; i--) {
		kernel_(steps - i, 0) = Gauss(sigma, -1*i*stepSize);
		kernel_(kernel_.rows() + i - steps - 1, 0) = Gauss(sigma, i * stepSize);
	}
	kernel_(steps, 0) = Gauss(sigma, 0);
	if (doubleCenter) kernel_(steps + 1, 0) = Gauss(sigma, 0);

	sum_kernel_ = 0;
	for (int i = 0; i < kernel_.rows(); i++) {
		sum_kernel_ += kernel_(i, 0);
	}
	weight_ = 1 / sum_kernel_;
}

MatrixXd Filter::LanczosDiffLow(MatrixXd const& a, int order = 5, int circle = 0)
{
	int n = a.rows();

	if (n < 4) {
		cout << "size of matrix is wrong" << endl;
	}

	diff_.resize(n, 1);

	int m = (order - 1) / 2;
	double temp(0);
	int start(m);

	if (circle) start = 0;

	for (int i = start; i < n - start; i++) {
		temp = 0;
		for (int j = 1; j < m + 1; j++) {
			int front = i + j;
			int back = i - j;

			if (front > n - 1) front = i + j - n;
			if (back < 0) back = i - j + n;
			temp += j * (a(front, 0) - a(back, 0)) / (m * (m + 1) * (2 * m + 1));
		}
		diff_(i, 0) = 3 * temp;
	}

	if(!circle) {
		for (int i = 0; i < m; i++) {
			diff_(i, 0) = a(i + 1) - a(i, 0);
			diff_(n - i - 1, 0) = a(n - i - 1, 0) - a(n - i - 2, 0);
		}
	}

	return diff_;
}

void Filter::Gaussian(double sigma,
	MatrixXd const& in,
	MatrixXd& out,
	int circle)
{
	int samples = kkernelLevel;
	KernelMake(samples, sigma);
	int sampleSide = samples / 2;
	int valueIdx = samples / 2 + 1;
	int ubound = in.rows();
	out.resize(ubound, 1);
	MatrixXd temp(ubound, 1);
	temp = in;
	int start(sampleSide);

	if (circle) start = 0;

	else if (!circle) {
		for (int i = 0; i < sampleSide; i++) {
			out(i, 0) = in(i, 0);
			out(ubound - i - 1) = in(ubound - i - 1);
		}
	}

	for (int i = start; i < ubound - start; i++) {
		double sum = 0;
		int sampleCtr = 0;
		int init = i - sampleSide;
		int limit = i + sampleSide;

		for (int j = init ; j < limit ; j++) {
			int index(j);
			if (j < 0) index = j + ubound;
			else if (j > ubound - 1) index = j - ubound;

			int sampleWeightIndex = sampleSide + (j - i);
			sum += kernel_(sampleWeightIndex, 0) * temp(index);
			sampleCtr++;
		}
		double smoothed = sum * weight_;
		out(i, 0) = smoothed;
	}
}

void CalculateFeatureAxisless(Geom& shard, int axis_index)
{
	Filter filter;

	if (!shard.is_matching_) return;

	// Feature calculation
	int number_of_points = shard.edge_line_.point_.cols();
	MatrixXd Dist(number_of_points, 1), Thickness(number_of_points, 1);
	MatrixXd Height(number_of_points, 1), Theta(number_of_points, 1);

	for (int i = 0; i < number_of_points; i++) {
		Vector3d r = { shard.edge_line_.point_(0, i), shard.edge_line_.point_(1, i), 0 };
		Dist(i) = r.norm();
		Theta(i) = atan2(shard.edge_line_.point_(1, i), shard.edge_line_.point_(0, i));
		Height(i) = shard.edge_line_.point_(2, i);
	}
	for (int i = 1; i < Theta.rows(); i++) {
		if (Theta(i, 0) - Theta(i - 1, 0) > PI) {
			Theta(i, 0) = Theta(i, 0) - 2 * PI;
		}
		else if (Theta(i, 0) - Theta(i - 1, 0) < -PI) {
			Theta(i, 0) = Theta(i, 0) + 2 * PI;
		}
	}

	if ((axis_index == 0) && (!shard.is_thickness_)) {
		Thickness = GetThickness(shard);
	}
	else {
		Thickness = shard.edge_line_.feature_[0].row(6);
	}

	MatrixXd Dist_Diff = filter.LanczosDiffLow(Dist, 7, 0);
	MatrixXd Height_Diff = filter.LanczosDiffLow(Height, 7, 0);
	MatrixXd Theta_Diff = filter.LanczosDiffLow(Theta, 7, 0);

	//Calculate Dist*d(Theta)
	for (int i = 0; i < Theta_Diff.rows(); i++) {
		Theta_Diff(i) = Theta_Diff(i) * Dist(i);
	}

	filter.Gaussian(2, Dist_Diff, Dist_Diff, 1);
	filter.Gaussian(2, Height_Diff, Height_Diff, 1);
	filter.Gaussian(2, Theta_Diff, Theta_Diff, 1);

	int num_features = Dist_Diff.rows();

	shard.edge_line_.feature_[axis_index].resize(7, num_features);
	for (int i = 0; i < num_features; i++) {
		shard.edge_line_.feature_[axis_index].col(i) << Dist_Diff(i),
			Height_Diff(i),
			Theta_Diff(i),
			Dist(i),
			Height(i),
			Theta(i),
			Thickness(i);
	}
}

MatrixXd GetThickness(Geom& shard)
{
	int num_breakline = shard.edge_line_.point_.cols();
	int num_o_sur = shard.sur_out_.point_.cols();
	MatrixXd thickness(num_breakline, 1);

	int index_out(0);
	for (size_t i = 0; i < num_breakline; i++) {
		Eigen::Vector4f pt(1, 0, 0, 0), line_pt(0, 0, 0, 0), line_dir(1, 1, 0, 0);
		double point2line_disance = 99999999, dist_Tmp = 0;
		line_pt << shard.edge_line_.point_(0, i), shard.edge_line_.point_(1, i), shard.edge_line_.point_(2, i), 0;
		line_dir << shard.edge_line_.normal_(0, i), shard.edge_line_.normal_(1, i), shard.edge_line_.normal_(2, i), 0;

		for (size_t j = 0; j < num_o_sur; j++) {
			pt << shard.sur_out_.point_(0, j), shard.sur_out_.point_(1, j), shard.sur_out_.point_(2, j), 0;
			dist_Tmp = sqrt(pcl::sqrPointToLineDistance(pt, line_pt, line_dir));
			if (dist_Tmp < point2line_disance)
			{
				point2line_disance = dist_Tmp;
				index_out = j;
			}
		}

		Eigen::Vector4f a;
		a << shard.sur_out_.point_(0, index_out), shard.sur_out_.point_(1, index_out),
			shard.sur_out_.point_(2, index_out), 0;

		if ((point2line_disance > 1.0) || (line_dir.dot(a - line_pt) > 0)) {
			thickness(i) = -1.0;
		}
		else {
			thickness(i) = (shard.sur_out_.point_.col(index_out) - shard.edge_line_.point_.col(i)).norm();
		}

	}

	shard.is_thickness_ = true;

	return thickness;
}

// (r, theta, z) 3*n matrix
MatrixXd ToCylindricalInterpolation(const BreakLine& breakline,
	bool theta_sort,
	bool r_sort)
{
	MatrixXd b1 = breakline.point_;
	int number_of_points = b1.cols();
	vector<Vector3d> cy;
	double base_theta;
	for (int i = 0; i < number_of_points; i++) {
		Vector3d r = { b1(0, i), b1(1, i), 0 }, tmp;
		tmp(0) = r.norm();
		tmp(1) = atan2(b1(1, i), b1(0, i));
		tmp(2) = b1(2, i);
		cy.push_back(tmp);
	}

	for (int i = 1; i < number_of_points; i++) {
		double gap = abs(cy[i](1) - cy[i - 1](1));
		if ((gap > 0.04) && (gap < 3.14)) {
			int num_interpol = gap / 0.04;
			double r_step = (cy[i](0) - cy[i - 1](0)) / num_interpol;
			double theta_step = (cy[i](1) - cy[i - 1](1)) / num_interpol;
			double z_step = (cy[i](2) - cy[i - 1](2)) / num_interpol;
			for (int j = 0; j < num_interpol; j++) {
				Vector3d tmp;
				tmp(0) = cy[i - 1](0) + r_step * (j + 1);
				tmp(1) = cy[i - 1](1) + theta_step * (j + 1);
				tmp(2) = cy[i - 1](2) + z_step * (j + 1);
				cy.push_back(tmp);
			}
		}
	}

	if (theta_sort) {
		sort(cy.begin(), cy.end(), [](Vector3d a, Vector3d b) -> bool {
			return a(1) < b(1);
		});
	}
	else if (r_sort) {
		sort(cy.begin(), cy.end(), [](Vector3d a, Vector3d b) -> bool {
			return a(0) > b(0);
		});
	}
	number_of_points = cy.size();
	MatrixXd output(3, number_of_points);

	for(int i = 0; i < number_of_points; i++){
		output(0, i) = cy[i](0);
		output(1, i) = cy[i](1);
		output(2, i) = cy[i](2);
	}

	return output;
}

void ToCylindricalInterpolation(const BreakLine& breakline,
	vector<Vector3d>& out,
	bool interpol)
{
	MatrixXd b1 = breakline.point_;
	int number_of_points = b1.cols();
	double base_theta;
	for (int i = 0; i < number_of_points; i++) {
		Vector3d r = { b1(0, i), b1(1, i), 0 }, tmp;
		tmp(0) = r.norm();
		tmp(1) = atan2(b1(1, i), b1(0, i));
		tmp(2) = b1(2, i);
		out.push_back(tmp);
	}

	if (interpol) {
		for (int i = 1; i < number_of_points; i++) {
			double gap = abs(out[i](1) - out[i - 1](1));
			if ((gap > 0.04) && (gap < 3.14)) {
				int num_interpol = gap / 0.04;
				double r_step = (out[i](0) - out[i - 1](0)) / num_interpol;
				double theta_step = (out[i](1) - out[i - 1](1)) / num_interpol;
				double z_step = (out[i](2) - out[i - 1](2)) / num_interpol;
				for (int j = 0; j < num_interpol; j++) {
					Vector3d tmp;
					tmp(0) = out[i - 1](0) + r_step * (j + 1);
					tmp(1) = out[i - 1](1) + theta_step * (j + 1);
					tmp(2) = out[i - 1](2) + z_step * (j + 1);
					out.push_back(tmp);
				}
			}

			double r_gap = abs(out[i](0) - out[i - 1](0));
			if ((r_gap > 0.1) && (gap < 3.14)) {
				int num_interpol = r_gap / 0.1;
				double r_step = (out[i](0) - out[i - 1](0)) / num_interpol;
				double theta_step = (out[i](1) - out[i - 1](1)) / num_interpol;
				double z_step = (out[i](2) - out[i - 1](2)) / num_interpol;
				for (int j = 0; j < num_interpol; j++) {
					Vector3d tmp;
					tmp(0) = out[i - 1](0) + r_step * (j + 1);
					tmp(1) = out[i - 1](1) + theta_step * (j + 1);
					tmp(2) = out[i - 1](2) + z_step * (j + 1);
					out.push_back(tmp);
				}
			}
		}
	}
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     univariate_distribution_estimator_impl.cpp
* \author   Collin Johnson
*
* Definition of implementations of the UnivariateDistributionEstimator for the subclasses of UnivariateDistribution:
*
*   - UnivariateGaussianDistribution
*   - GammaDistribution
*   - BetaDistribution
*   - ExponentialDistribution
*   - TruncatedGaussianDistribution
*/

#include <math/univariate_distribution_estimator_impl.h>
#include <math/univariate_gaussian.h>
#include <math/discrete_gaussian.h>
#include <math/beta_distribution.h>
#include <math/exponential_distribution.h>
#include <math/gamma_distribution.h>
#include <math/truncated_gaussian_distribution.h>
#include <math/statistics.h>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

namespace vulcan
{
namespace math
{

using DistPtr = std::unique_ptr<UnivariateDistribution>;

double log_likelihood_gamma_func(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return (k-1)*sumXi - n*k - n*k*std::log(sumXi/(k*n)) - n*std::log(boost::math::tgamma(k));
}

double log_likelihood_gamma_deriv(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return n*(std::log(k) - boost::math::digamma(k) - std::log(sumXi/n)) + sumLogXi;
}

DistPtr UnivariateGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                                      const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new UnivariateGaussianDistribution(estimate(dataBegin, dataEnd))};
}

UnivariateGaussianDistribution UnivariateGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                                                const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if(var == 0.0)
    {
        std::cerr << "WARNING: UnivariateGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return UnivariateGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr DiscreteGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                                    const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new DiscreteGaussianDistribution(estimate(dataBegin, dataEnd))};
}

DiscreteGaussianDistribution DiscreteGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                                             const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if(var == 0.0)
    {
        std::cerr << "WARNING: DiscreteGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return DiscreteGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr GammaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                         const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{ new GammaDistribution(estimate(dataBegin, dataEnd)) };
}

GammaDistribution GammaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                       const std::vector<double>::const_iterator dataEnd) const
{
    std::vector<double> filtered(dataBegin, dataEnd);
    auto validDataEnd = std::remove_if(filtered.begin(), filtered.end(), [](double x) { return x <= 0.0; });

    int numValidData = std::distance(filtered.begin(), validDataEnd);
    assert(numValidData);

    using namespace std::placeholders;

    double sumXi    = std::accumulate(filtered.begin(), validDataEnd, 0.0);
    double sumLogXi = 0.0;
    for(auto val : boost::make_iterator_range(filtered.begin(), validDataEnd))
    {
        sumLogXi += std::log(val);
    }

    double s  = std::log(sumXi/numValidData) - sumLogXi/numValidData;
    double k0 = (3.0 - s + std::sqrt(std::pow(s-3.0, 2.0) + 24.0*s)) / (12.0 * s);

    //     NewtonRaphsonErrorFunc<double> newton(std::bind(log_likelihood_func,  _1, sumXi, sumLogXi, filtered.size()),
    //                                           std::bind(log_likelihood_deriv, _1, sumXi, sumLogXi, filtered.size()));
    //
    //     double k     = find_single_root(newton, k0, 1e-5);
    double k     = k0;
    double theta = sumXi / (numValidData * k);
    return GammaDistribution(k, theta);
}

DistPtr BetaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                        const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{ new BetaDistribution(estimate(dataBegin, dataEnd)) };
}

BetaDistribution BetaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                     const std::vector<double>::const_iterator dataEnd) const
{
    double mean     = math::mean    (dataBegin, dataEnd);
    double variance = math::variance(dataBegin, dataEnd);

    if(variance == 0.0)
    {
        std::cerr << "WARNING: BetaDistributionEstimator: Variance of data was 0. Setting to 1e-4.\n";
        variance = 1e-4;
    }

    if(variance < mean*(1-mean) && (variance > 0.0))
    {
        double alpha = mean         * ((mean * (1.0-mean) / variance) - 1.0);
        double beta  = (1.0 - mean) * ((mean * (1.0-mean) / variance) - 1.0);
        return BetaDistribution(alpha, beta);
    }
    else
    {
        std::cerr << "ERROR: BetaDistributionEstimator: Could not use method-of-moments to find parameters. Mean:" << mean << " Variance:" << variance << '\n'
                  << " Variance should be less than " << (mean * (1-mean)) << '\n';
        return BetaDistribution();
    }
}

ExponentialDistributionEstimator::ExponentialDistributionEstimator(double maxValue)
: max_(maxValue)
{
}

DistPtr ExponentialDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                               const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{ new ExponentialDistribution(estimate(dataBegin, dataEnd)) };
}

ExponentialDistribution ExponentialDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                                   const std::vector<double>::const_iterator dataEnd) const
{
    double mean = math::mean(dataBegin, dataEnd);
    if(mean > 0.0)
    {
        return ExponentialDistribution(1.0/mean, max_);
    }
    else
    {
        std::cerr << "ERROR: ExponentialDistributionEstimator: Invalid mean for the data:" << mean <<" Must be greater than 0.\n";
        return ExponentialDistribution(1.0, max_);
    }
}

TruncatedGaussianDistributionEstimator::TruncatedGaussianDistributionEstimator(double lower, double upper)
: lower_(lower)
, upper_(upper)
{
}

DistPtr TruncatedGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                                     const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{ new TruncatedGaussianDistribution(estimate(dataBegin, dataEnd)) };
}

TruncatedGaussianDistribution TruncatedGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                                               const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if(var == 0.0)
    {
        std::cerr << "WARNING: TruncatedGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return TruncatedGaussianDistribution(mean(dataBegin, dataEnd), var, lower_, upper_);
}

} // namespace math
} // namespace vulcan


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     em.cpp
 * \author   Collin Johnson
 *
 * Definition of em_1d_linear and em_2d_fixed.
 */

#include <math/clustering.h>
#include <core/multivariate_gaussian.h>
#include <math/statistics.h>
#include <math/univariate_gaussian.h>
#include <boost/range/iterator_range.hpp>
#include <algorithm>
#include <iomanip>
#include <iostream>

namespace vulcan
{
namespace math
{

template <class Data>
using DataIt = typename std::vector<Data>::const_iterator;

template<class Data, class Dist>
struct em_data_t
{
    int numClusters;
    std::vector<int> clusterSizes;
    std::vector<Dist> dists;
    std::vector<int> assignments;
    std::vector<Data> clusterData;  // temporary storage for computing distributions
    double totalProb;
};

template <class Data, class Dist, class ProbFunc, class Estimator>
em_data_t<Data, Dist> run_em(DataIt<Data> begin,
                             DataIt<Data> end,
                             int k,
                             int maxIterations,
                             ProbFunc prob,
                             Estimator estimator);

template <class Data, class Dist, class Estimator>
void initialize_dists(DataIt<Data> begin, DataIt<Data> end, int k, em_data_t<Data, Dist>& dists, Estimator estimator);

template <class Data, class Dist, class Estimator>
void calculate_dists(DataIt<Data> begin, DataIt<Data> end, em_data_t<Data, Dist>& dists, Estimator estimator);

template <class Data, class Dist, class ProbFunc>
int assign_dists(DataIt<Data> begin, DataIt<Data> end, em_data_t<Data, Dist>& dists, ProbFunc prob);

template <class Data, class Dist, class ProbFunc>
int most_probable_cluster(Data data, const em_data_t<Data, Dist>& dists, ProbFunc prob);

template <class Data, class Dist>
void print_dists(const em_data_t<Data, Dist>& dists);

struct UnivariateEstimator
{
    UnivariateGaussianDistribution operator()(DataIt<double> begin, DataIt<double> end)
    {
        double var = variance(begin, end);
        return UnivariateGaussianDistribution(mean(begin, end), std::max(var, 1e-10));
    }
};

struct MultivariateEstimator
{
    MultivariateGaussian operator()(DataIt<Point<float>> begin, DataIt<Point<float>> end)
    {
        Vector mean(2);
        mean.zeros();
        for(auto p : boost::make_iterator_range(begin, end))
        {
            mean[0] += p.x;
            mean[1] += p.y;
        }

        if(std::distance(begin, end) > 0)
        {
            mean /= std::distance(begin, end);
        }

        Matrix cov(2, 2);
        cov.zeros();
        Vector diff(2);
        for(auto p : boost::make_iterator_range(begin, end))
        {
            diff[0] = p.x - mean[0];
            diff[1] = p.y - mean[1];

            cov += diff * arma::trans(diff);
        }

        if(std::distance(begin, end) < 2)
        {
            cov.zeros();
            cov.diag().fill(1e-16);
        }
        else
        {
            cov /= std::distance(begin, end) - 1;
        }

        try
        {
            Matrix inv = arma::inv(cov);
        }
        catch(std::exception& e)
        {
            std::cerr << "Failed to take inverse: " << e.what() << " Matrix:\n" << cov;
            cov.zeros();
            cov.diag().fill(1e-16);
        }

        return MultivariateGaussian(mean, cov);
    }
};

inline double probability_univariate(double value, const UnivariateGaussianDistribution& mean)
{
    return mean.likelihood(value);
}

inline double probability_multivariate(const Point<float>& value , const MultivariateGaussian& mean)
{
    Vector vec(2);
    vec[0] = value.x;
    vec[1] = value.y;
    return mean.probability(vec);
}

clustering_result_t em_1d_linear(std::vector<double>::const_iterator begin,
                                 std::vector<double>::const_iterator end,
                                 int kMax,
                                 int maxIterations)
{
    int dataSize = std::distance(begin, end);
    kMax = std::min(kMax, dataSize);    // can't have more clusters than data!

    std::vector<em_data_t<double, UnivariateGaussianDistribution>> attemptedMeans;
    for(int k = 1; k <= kMax; ++k)
    {
        attemptedMeans.push_back(run_em<double, UnivariateGaussianDistribution>(begin,
                                                                                end,
                                                                                k,
                                                                                maxIterations,
                                                                                probability_univariate,
                                                                                UnivariateEstimator()));
    }

    auto maxProbIt = std::max_element(attemptedMeans.begin(), attemptedMeans.end(), [](const auto& lhs, const auto& rhs) {
        return lhs.totalProb < rhs.totalProb;
    });

    // Convert the em_data_t to a clustering_result_t
    clustering_result_t results;
    results.numClusters = maxProbIt->dists.size();
    results.clusterSizes = std::move(maxProbIt->clusterSizes);
    results.assignedCluster = std::move(maxProbIt->assignments);

#ifdef DEBUG_RESULTS
    std::cout << "INFO: em_1d_linear: Cluster results:\nNum clusters:" << results.numClusters << " Cluster sizes:\n";
    for(std::size_t n = 0; n < minMeanIt->dists.size(); ++n)
    {
        std::cout << std::setprecision(5) << std::setw(5) << minMeanIt->dists[n] << "->" << results.clusterSizes[n] << '\n';
    }
#endif

    return results;
}

clustering_result_t em_2d_linear(std::vector<Point<float>>::const_iterator begin,
                                 std::vector<Point<float>>::const_iterator end,
                                 const int kMax,
                                 const int maxIterations)
{
    std::vector<em_data_t<Point<float>, MultivariateGaussian>> clusters;

    for(int k = 1; k < kMax; ++k)
    {
        clusters.push_back(run_em<Point<float>, MultivariateGaussian>(begin,
                                                                      end,
                                                                      k,
                                                                      maxIterations,
                                                                      probability_multivariate,
                                                                      MultivariateEstimator()));
    }

    auto maxProbIt = std::max_element(clusters.begin(), clusters.end(), [](const auto& lhs, const auto& rhs) {
        return lhs.totalProb < rhs.totalProb;
    });

    // Convert the em_data_t to a clustering_result_t
    clustering_result_t results;
    results.numClusters = maxProbIt->dists.size();
    results.clusterSizes = std::move(maxProbIt->clusterSizes);
    results.assignedCluster = std::move(maxProbIt->assignments);

#ifdef DEBUG_RESULTS
    std::cout << "INFO: em_2d_fixed: Cluster results:\nNum clusters:" << results.numClusters << " Cluster sizes:\n";
    for(std::size_t n = 0; n < dists.dists.size(); ++n)
    {
        std::cout << std::setprecision(5) << std::setw(5) << dists.dists[n] << "->" << results.clusterSizes[n] << '\n';
    }
#endif

    return results;
}

template <class Data, class Dist, class ProbFunc, class Estimator>
em_data_t<Data, Dist> run_em(DataIt<Data> begin,
                             DataIt<Data> end,
                             int k,
                             int maxIterations,
                             ProbFunc prob,
                             Estimator estimator)
{
    em_data_t<Data, Dist> dists;
    dists.numClusters = k;
    initialize_dists(begin, end, k, dists, estimator);
    calculate_dists(begin, end, dists, estimator);

    int numChanges = 0;
    int numIterations = 0;

    do
    {
        numChanges = assign_dists(begin, end, dists, prob);
        calculate_dists(begin, end, dists, estimator);
        ++numIterations;

#ifdef DEBUG_KMEANS
        std::cout << "Iteration:" << numIterations << " Changes:" << numChanges << '\n';
        print_dists(dists);
#endif

    } while((numChanges > 0) && (numIterations <= maxIterations));

    // Once complete, sum the error amongst all the dists
    for(std::size_t n = 0, size = std::distance(begin, end); n < size; ++n)
    {
        dists.totalProb += prob(*(begin + n), dists.dists[dists.assignments[n]]);
    }

    // If any cluster is empty, zero probability of correctness
    for(std::size_t n = 0; n < dists.clusterSizes.size(); ++n)
    {
        if(dists.clusterSizes[n] == 0)
        {
            dists.totalProb = 0.0;
        }
    }

#ifdef DEBUG_KMEANS
    std::cout << "INFO: em_1d: k:" << k << " Error:" << dists.totalProb << " Num iterations:" << numIterations
        << '\n';
    print_dists(dists);
#endif

    return dists;
}

template <class Data, class Dist, class Estimator>
void initialize_dists(DataIt<Data> begin, DataIt<Data> end, int k, em_data_t<Data, Dist>& dists, Estimator estimator)
{
    // Allocate the buffers
    dists.clusterSizes.resize(k);
    dists.dists.resize(k);
    dists.assignments.resize(std::distance(begin, end));
    std::iota(dists.assignments.begin(), dists.assignments.end(), 0);
    dists.totalProb = 0.0;

    std::transform(dists.assignments.begin(), dists.assignments.end(), dists.assignments.begin(), [k](int c) {
        return c % k;
    });
}

template <class Data, class Dist, class Estimator>
void calculate_dists(DataIt<Data> begin, DataIt<Data> end, em_data_t<Data, Dist>& dists, Estimator estimator)
{
    for(int cluster = 0; cluster < dists.numClusters; ++cluster)
    {
        dists.clusterData.clear();

        for(std::size_t n = 0; n < dists.assignments.size(); ++n)
        {
            if(dists.assignments[n] == cluster)
            {
                dists.clusterData.push_back(*(begin + n));
            }
        }

        dists.dists[cluster] = estimator(dists.clusterData.begin(), dists.clusterData.end());
    }
}

template <class Data, class Dist, class ProbFunc>
int assign_dists(DataIt<Data> begin, DataIt<Data> end, em_data_t<Data, Dist>& dists, ProbFunc prob)
{
    std::fill(dists.clusterSizes.begin(), dists.clusterSizes.end(), 0);

    int numChanged = 0;

    for(std::size_t n = 0, size = std::distance(begin, end); n < size; ++n)
    {
        int newAssignment = most_probable_cluster(*(begin + n), dists, prob);
        if(newAssignment != dists.assignments[n])
        {
            dists.assignments[n] = newAssignment;
            ++numChanged;
        }

        ++dists.clusterSizes[newAssignment];
    }

    return numChanged;
}

template <class Data, class Dist, class ProbFunc>
int most_probable_cluster(Data data, const em_data_t<Data, Dist>& dists, ProbFunc prob)
{
    auto maxIt = std::max_element(dists.dists.begin(), dists.dists.end(), [&](auto& lhs, auto& rhs) {
        return prob(data, lhs) < prob(data, rhs);
    });

    return std::distance(dists.dists.begin(), maxIt);
}

template <class Data, class Dist>
void print_dists(const em_data_t<Data, Dist>& dists)
{
    for(std::size_t n = 0; n < dists.dists.size(); ++n)
    {
        std::cout << std::setprecision(5) << std::setw(5) << dists.dists[n] << "->" << dists.clusterSizes[n] << '\n';
    }
}

} // namespace utils
} // namespace vulcan


// SPDX-FileCopyrightText: 2015 - 2021 Marcin Łoś <marcin.los.91@gmail.com>
// SPDX-License-Identifier: MIT

#ifndef ADS_SIMULATION_BASIC_SIMULATION_3D_HPP
#define ADS_SIMULATION_BASIC_SIMULATION_3D_HPP

#include <array>
#include <cstddef>

#include <boost/range/counting_range.hpp>

#include "ads/lin/tensor.hpp"
#include "ads/simulation/boundary.hpp"
#include "ads/simulation/dimension.hpp"
#include "ads/util/function_value.hpp"
#include "ads/util/iter/product.hpp"

namespace ads {

class basic_simulation_3d {
public:
    virtual ~basic_simulation_3d() = default;

    basic_simulation_3d() = default;
    basic_simulation_3d(const basic_simulation_3d&) = delete;
    basic_simulation_3d& operator=(const basic_simulation_3d&) = delete;
    basic_simulation_3d(basic_simulation_3d&&) = delete;
    basic_simulation_3d& operator=(basic_simulation_3d&&) = delete;

protected:
    using vector_type = lin::tensor<double, 3>;
    using vector_view = lin::tensor_view<double, 3>;
    using value_type = function_value_3d;

    using index_type = std::array<int, 3>;
    using index_1d_iter_type = boost::counting_iterator<int>;
    using index_iter_type = util::iter_product3<index_1d_iter_type, index_type>;
    using index_range = boost::iterator_range<index_iter_type>;

    using point_type = std::array<double, 3>;

    struct L2 {
        double operator()(value_type a) const { return a.val * a.val; }
    };

    struct H10 {
        double operator()(value_type a) const { return a.dx * a.dx + a.dy * a.dy + a.dz * a.dz; }
    };

    struct H1 {
        double operator()(value_type a) const {
            return a.val * a.val + a.dx * a.dx + a.dy * a.dy + a.dz * a.dz;
        }
    };

    value_type eval_basis(index_type e, index_type q, index_type a, const dimension& x,
                          const dimension& y, const dimension& z) const {
        auto loc = dof_global_to_local(e, a, x, y, z);

        const auto& bx = x.basis;
        const auto& by = y.basis;
        const auto& bz = z.basis;

        double B1 = bx.b[e[0]][q[0]][0][loc[0]];
        double B2 = by.b[e[1]][q[1]][0][loc[1]];
        double B3 = bz.b[e[2]][q[2]][0][loc[2]];
        double dB1 = bx.b[e[0]][q[0]][1][loc[0]];
        double dB2 = by.b[e[1]][q[1]][1][loc[1]];
        double dB3 = bz.b[e[2]][q[2]][1][loc[2]];

        double v = B1 * B2 * B3;
        double dxv = dB1 * B2 * B3;
        double dyv = B1 * dB2 * B3;
        double dzv = B1 * B2 * dB3;

        return {v, dxv, dyv, dzv};
    }

    double laplacian(index_type e, index_type q, index_type a, const dimension& x,
                     const dimension& y, const dimension& z) const {
        auto loc = dof_global_to_local(e, a, x, y, z);

        const auto& bx = x.basis;
        const auto& by = y.basis;
        const auto& bz = z.basis;

        double B1 = bx.b[e[0]][q[0]][0][loc[0]];
        double B2 = by.b[e[1]][q[1]][0][loc[1]];
        double B3 = bz.b[e[2]][q[2]][0][loc[2]];

        double ddB1 = bx.b[e[0]][q[0]][2][loc[0]];
        double ddB2 = by.b[e[1]][q[1]][2][loc[1]];
        double ddB3 = by.b[e[2]][q[2]][2][loc[2]];

        return ddB1 * B2 * B3 + B1 * ddB2 * B3 + B1 * B2 * ddB3;
    }

    template <typename Sol>
    value_type eval(const Sol& v, index_type e, index_type q, const dimension& x,
                    const dimension& y, const dimension& z) const {
        value_type u{};
        for (auto b : dofs_on_element(e, x, y, z)) {
            double c = v(b[0], b[1], b[2]);
            value_type B = eval_basis(e, q, b, x, y, z);
            u += c * B;
        }
        return u;
    }

    index_range elements(const dimension& x, const dimension& y, const dimension& z) const {
        return util::product_range<index_type>(x.element_indices(), y.element_indices(),
                                               z.element_indices());
    }

    index_range quad_points(const dimension& x, const dimension& y, const dimension& z) const {
        auto rx = boost::counting_range(0, x.basis.quad_order);
        auto ry = boost::counting_range(0, y.basis.quad_order);
        auto rz = boost::counting_range(0, z.basis.quad_order);
        return util::product_range<index_type>(rx, ry, rz);
    }

    index_range dofs_on_element(index_type e, const dimension& x, const dimension& y,
                                const dimension& z) const {
        auto rx = x.basis.dof_range(e[0]);
        auto ry = y.basis.dof_range(e[1]);
        auto rz = z.basis.dof_range(e[2]);
        return util::product_range<index_type>(rx, ry, rz);
    }

    index_range elements_supporting_dof(index_type dof, const dimension& x, const dimension& y,
                                        const dimension& z) const {
        auto rx = x.basis.element_range(dof[0]);
        auto ry = y.basis.element_range(dof[1]);
        auto rz = z.basis.element_range(dof[2]);
        return util::product_range<index_type>(rx, ry, rz);
    }

    bool supported_in(index_type dof, index_type e, const dimension& x, const dimension& y,
                      const dimension& z) const {
        auto xrange = x.basis.element_ranges[dof[0]];
        auto yrange = y.basis.element_ranges[dof[1]];
        auto zrange = z.basis.element_ranges[dof[2]];

        return e[0] >= xrange.first && e[0] <= xrange.second && e[1] >= yrange.first
            && e[1] <= yrange.second && e[2] >= zrange.first && e[2] <= zrange.second;
    }

    index_type dof_global_to_local(index_type e, index_type a, const dimension& x,
                                   const dimension& y, const dimension& z) const {
        const auto& bx = x.basis;
        const auto& by = y.basis;
        const auto& bz = z.basis;
        return {{a[0] - bx.first_dof(e[0]), a[1] - by.first_dof(e[1]), a[2] - bz.first_dof(e[2])}};
    }

    template <typename RHS>
    void update_global_rhs(RHS& global, const vector_type& local, index_type e, const dimension& x,
                           const dimension& y, const dimension& z) const {
        for (auto a : dofs_on_element(e, x, y, z)) {
            auto loc = dof_global_to_local(e, a, x, y, z);
            global(a[0], a[1], a[2]) += local(loc[0], loc[1], loc[2]);
        }
    }

    index_range dofs(const dimension& x, const dimension& y, const dimension& z) const {
        auto rx = boost::counting_range(0, x.dofs());
        auto ry = boost::counting_range(0, y.dofs());
        auto rz = boost::counting_range(0, z.dofs());
        return util::product_range<index_type>(rx, ry, rz);
    }

    index_range internal_dofs(const dimension& x, const dimension& y, const dimension& z) const {
        auto rx = boost::counting_range(1, x.dofs() - 1);
        auto ry = boost::counting_range(1, y.dofs() - 1);
        auto rz = boost::counting_range(1, z.dofs() - 1);
        return util::product_range<index_type>(rx, ry, rz);
    }

    double jacobian(index_type e, const dimension& x, const dimension& y,
                    const dimension& z) const {
        return x.basis.J[e[0]] * y.basis.J[e[1]] * z.basis.J[e[2]];
    }

    double weight(index_type q, const dimension& x, const dimension& y, const dimension& z) const {
        return x.basis.w[q[0]] * y.basis.w[q[1]] * z.basis.w[q[2]];
    }

    point_type point(index_type e, index_type q, const dimension& x, const dimension& y,
                     const dimension& z) const {
        double px = x.basis.x[e[0]][q[0]];
        double py = y.basis.x[e[1]][q[1]];
        double pz = z.basis.x[e[2]][q[2]];
        return {px, py, pz};
    }

    auto overlapping_dofs(int dof, int begin, int end, const dimension& x) const {
        using std::max;
        using std::min;

        auto minx = max(begin, dof - x.B.degree);
        auto maxx = min(end, dof + x.B.degree + 1);

        return boost::counting_range(minx, maxx);
    }

    index_range overlapping_dofs(index_type dof, const dimension& x, const dimension& y,
                                 const dimension& z) const {
        auto rx = overlapping_dofs(dof[0], 0, x.dofs(), x);
        auto ry = overlapping_dofs(dof[1], 0, y.dofs(), y);
        auto rz = overlapping_dofs(dof[1], 0, z.dofs(), z);
        return util::product_range<index_type>(rx, ry, rz);
    }

    index_range overlapping_dofs(index_type dof, const dimension& Ux, const dimension& Uy,
                                 const dimension& Uz, const dimension& Vx, const dimension& Vy,
                                 const dimension& Vz) const {
        auto xrange = Ux.basis.element_ranges[dof[0]];
        auto yrange = Uy.basis.element_ranges[dof[1]];
        auto zrange = Uz.basis.element_ranges[dof[2]];

        auto x0 = Vx.basis.first_dof(xrange.first);
        auto x1 = Vx.basis.last_dof(xrange.second) + 1;

        auto y0 = Vy.basis.first_dof(yrange.first);
        auto y1 = Vy.basis.last_dof(yrange.second) + 1;

        auto z0 = Vz.basis.first_dof(zrange.first);
        auto z1 = Vz.basis.last_dof(zrange.second) + 1;

        auto rx = boost::counting_range(x0, x1);
        auto ry = boost::counting_range(y0, y1);
        auto rz = boost::counting_range(z0, z1);

        return util::product_range<index_type>(rx, ry, rz);
    }

    index_range overlapping_internal_dofs(index_type dof, const dimension& x, const dimension& y,
                                          const dimension& z) const {
        auto rx = overlapping_dofs(dof[0], 1, x.dofs() - 1, x);
        auto ry = overlapping_dofs(dof[1], 1, y.dofs() - 1, y);
        auto rz = overlapping_dofs(dof[2], 1, z.dofs() - 1, z);
        return util::product_range<index_type>(rx, ry, rz);
    }

    int linear_index(index_type dof, const dimension& x, const dimension& y,
                     const dimension& z) const {
        auto order = reverse_ordering<3>({x.dofs(), y.dofs(), z.dofs()});
        return order.linear_index(dof[0], dof[1], dof[2]);
    }

    template <typename Fun>
    void for_boundary_dofs(const dimension& x, const dimension& y, const dimension& z,
                           Fun&& fun) const {
        for (auto jx = 0; jx < x.dofs(); ++jx) {
            for (auto jy = 0; jy < y.dofs(); ++jy) {
                fun({jx, jy, 0});
                fun({jx, jy, z.dofs() - 1});
            }
        }
        for (auto jx = 0; jx < x.dofs(); ++jx) {
            for (auto jz = 1; jz < z.dofs() - 1; ++jz) {
                fun({jx, 0, jz});
                fun({jx, y.dofs() - 1, jz});
            }
        }
        for (auto jy = 1; jy < y.dofs() - 1; ++jy) {
            for (auto jz = 1; jz < z.dofs() - 1; ++jz) {
                fun({0, jy, jz});
                fun({x.dofs() - 1, jy, jz});
            }
        }
    }

    bool is_boundary(int dof, const dimension& x) const { return dof == 0 || dof == x.dofs() - 1; }

    bool is_boundary(index_type dof, const dimension& x, const dimension& y,
                     const dimension& z) const {
        return is_boundary(dof[0], x) || is_boundary(dof[1], y) || is_boundary(dof[2], z);
    }

    template <typename Norm, typename Fun>
    double norm(const dimension& Ux, const dimension& Uy, const dimension& Uz, Norm&& norm,
                Fun&& fun) const {
        double val = 0;

        for (auto e : elements(Ux, Uy, Uz)) {
            double J = jacobian(e, Ux, Uy, Uz);
            for (auto q : quad_points(Ux, Uy, Uz)) {
                double w = weight(q, Ux, Uy, Uz);
                auto x = point(e, q, Ux, Uy, Uz);
                auto d = fun(x);
                val += norm(d) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Fun>
    double normL2(const dimension& Ux, const dimension& Uy, const dimension& Uz, Fun&& fun) const {
        return norm(Ux, Uy, Uz, L2{}, fun);
    }

    template <typename Fun>
    double normH1(const dimension& Ux, const dimension& Uy, const dimension& Uz, Fun&& fun) const {
        return norm(Ux, Uy, Uz, H1{}, fun);
    }

    template <typename Sol, typename Norm>
    double norm(const Sol& u, const dimension& Ux, const dimension& Uy, const dimension& Uz,
                Norm&& norm) const {
        double val = 0;

        for (auto e : elements(Ux, Uy, Uz)) {
            double J = jacobian(e, Ux, Uy, Uz);
            for (auto q : quad_points(Ux, Uy, Uz)) {
                double w = weight(q, Ux, Uy, Uz);
                value_type uu = eval(u, e, q, Ux, Uy, Uz);
                val += norm(uu) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Sol>
    double normL2(const Sol& u, const dimension& Ux, const dimension& Uy,
                  const dimension& Uz) const {
        return norm(u, Ux, Uy, Uz, L2{});
    }

    template <typename Sol>
    double normH1(const Sol& u, const dimension& Ux, const dimension& Uy,
                  const dimension& Uz) const {
        return norm(u, Ux, Uy, Uz, H1{});
    }

    template <typename Sol, typename Fun, typename Norm>
    double error(const Sol& u, const dimension& Ux, const dimension& Uy, const dimension& Uz,
                 Norm&& norm, Fun&& fun) const {
        double error = 0;

        for (auto e : elements(Ux, Uy, Uz)) {
            double J = jacobian(e, Ux, Uy, Uz);
            for (auto q : quad_points(Ux, Uy, Uz)) {
                double w = weight(q, Ux, Uy, Uz);
                auto x = point(e, q, Ux, Uy, Uz);
                value_type uu = eval(u, e, q, Ux, Uy, Uz);

                auto d = uu - fun(x);
                error += norm(d) * w * J;
            }
        }
        return std::sqrt(error);
    }

    template <typename Sol, typename Fun, typename Norm>
    double error_relative(const Sol& u, const dimension& Ux, const dimension& Uy,
                          const dimension& Uz, Norm&& norm, Fun&& fun) const {
        double error = 0;
        double ref_norm = 0;

        for (auto e : elements(Ux, Uy, Uz)) {
            double J = jacobian(e, Ux, Uy, Uz);
            for (auto q : quad_points(Ux, Uy, Uz)) {
                double w = weight(q, Ux, Uy, Uz);
                auto x = point(e, q, Ux, Uy, Uz);
                value_type uu = eval(u, e, q, Ux, Uy, Uz);
                auto fx = fun(x);

                error += norm(uu - fx) * w * J;
                ref_norm += norm(fx) * w * J;
            }
        }
        return std::sqrt(error / ref_norm);
    }

    template <typename Sol, typename Fun>
    double errorL2(const Sol& u, const dimension& Ux, const dimension& Uy, const dimension& Uz,
                   Fun&& fun) const {
        return error(u, Ux, Uy, Uz, L2{}, fun);
    }

    template <typename Sol, typename Fun>
    double error_relative_L2(const Sol& u, const dimension& Ux, const dimension& Uy,
                             const dimension& Uz, Fun&& fun) const {
        return error_relative(u, Ux, Uy, Uz, L2{}, fun);
    }

    template <typename Sol, typename Fun>
    double errorH1(const Sol& u, const dimension& Ux, const dimension& Uy, const dimension& Uz,
                   Fun&& fun) const {
        return error(u, Ux, Uy, Uz, H1{}, fun);
    }

    template <typename Sol, typename Fun>
    double error_relative_H1(const Sol& u, const dimension& Ux, const dimension& Uy,
                             const dimension& Uz, Fun&& fun) const {
        return error_relative(u, Ux, Uy, Uz, H1{}, fun);
    }

    template <typename Sol>
    double norm_rot(const Sol& X, const Sol& Y, const Sol& Z,                          //
                    const dimension& U1x, const dimension& U1y, const dimension& U1z,  //
                    const dimension& U2x, const dimension& U2y, const dimension& U2z,  //
                    const dimension& U3x, const dimension& U3y, const dimension& U3z) const {
        double val = 0;

        for (auto e : elements(U1x, U1y, U1z)) {
            double J = jacobian(e, U1x, U1y, U1z);
            for (auto q : quad_points(U1x, U1y, U1z)) {
                double w = weight(q, U1x, U1y, U1z);
                auto x = eval(X, e, q, U1x, U1y, U1z);
                auto y = eval(Y, e, q, U2x, U2y, U2z);
                auto z = eval(Z, e, q, U3x, U3y, U3z);

                auto rx = z.dy - y.dz;
                auto ry = x.dz - z.dx;
                auto rz = y.dx - x.dy;

                val += (rx * rx + ry * ry + rz * rz) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Sol, typename FunX, typename FunY, typename FunZ>
    double error_rot(const Sol& X, const Sol& Y, const Sol& Z,                          //
                     const dimension& U1x, const dimension& U1y, const dimension& U1z,  //
                     const dimension& U2x, const dimension& U2y, const dimension& U2z,  //
                     const dimension& U3x, const dimension& U3y, const dimension& U3z,  //
                     FunX&& fx, FunY&& fy, FunZ&& fz) const {
        double val = 0;

        for (auto e : elements(U1x, U1y, U1z)) {
            double J = jacobian(e, U1x, U1y, U1z);
            for (auto q : quad_points(U1x, U1y, U1z)) {
                auto p = point(e, q, U1x, U2y, U3z);
                double w = weight(q, U1x, U2y, U3z);
                auto x = eval(X, e, q, U1x, U1y, U1z) - fx(p);
                auto y = eval(Y, e, q, U2x, U2y, U2z) - fy(p);
                auto z = eval(Z, e, q, U3x, U3y, U3z) - fz(p);

                auto rx = z.dy - y.dz;
                auto ry = x.dz - z.dx;
                auto rz = y.dx - x.dy;

                val += (rx * rx + ry * ry + rz * rz) * w * J;
            }
        }
        return std::sqrt(val);
    }

    template <typename Sol>
    double norm_div(const Sol& X, const Sol& Y, const Sol& Z,                          //
                    const dimension& U1x, const dimension& U1y, const dimension& U1z,  //
                    const dimension& U2x, const dimension& U2y, const dimension& U2z,  //
                    const dimension& U3x, const dimension& U3y, const dimension& U3z) const {
        double val = 0;

        for (auto e : elements(U1x, U1y, U1z)) {
            double J = jacobian(e, U1x, U1y, U1z);
            for (auto q : quad_points(U1x, U1y, U1z)) {
                double w = weight(q, U1x, U1y, U1z);
                auto x = eval(X, e, q, U1x, U1y, U1z);
                auto y = eval(Y, e, q, U2x, U2y, U2z);
                auto z = eval(Z, e, q, U3x, U3y, U3z);

                auto v = x.dx + y.dy + z.dz;
                val += v * v * w * J;
            }
        }
        return std::sqrt(val);
    }
};

}  // namespace ads

#endif  // ADS_SIMULATION_BASIC_SIMULATION_3D_HPP


/*Copyright (c) 2021 James Gayvert

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

/*
 * transforms.cpp
 *
 */

#include <math.h>
#include <algorithm>
#include <array>
#include <Eigen/Dense>
#include <vector>

#include "BasisSet.h"
#include "gto_ordering.h"
#include "Shell.h"
#include "utils.h"

void uniform_cart_norm(Eigen::MatrixXd &my_mat, BasisSet &bs)
{
	unsigned int bf_idx = 0;
	for(auto&shell:bs.basis)
	{
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(shell.l);
		for(unsigned int i=0;i<shell.num_carts();i++)
		{
			std::array<size_t,3> cart = order[i];
			double scale = sqrt(fact2(2*shell.l-1)/fact2(2*cart[0]-1)
					/fact2(2*cart[1]-1)/fact2(2*cart[2]-1));
			my_mat.col(bf_idx+i) = my_mat.col(bf_idx+i)*scale;
			my_mat.row(bf_idx+i) = my_mat.row(bf_idx+i)*scale;
		}
		bf_idx += shell.num_carts();
	}
}

//DOI: 10.1002/qua.560540202
//Last part of eqn 15, for M<0 we want imaginary part, for M>0 we want real part
double term4(int M, int exp_num)
{
	if(M<0)
	{
		//integer powers will be real
		if(exp_num%2==0)
			return 0;
		else
			return (exp_num-1)%4==0 ? 1:-1;
	}
	else
	{
		//half-integer powers will be imaginary
		if(exp_num%2!=0)
			return 0;
		else
			return exp_num%4==0 ? 1:-1;
	}
}

//DOI: 10.1002/qua.560540202
//Equation 15, currently assumes fully normalized cartesians. Will add non-fully normalized carts soon enough...
double get_coeff(int L, int m, int lx, int ly, int lz)
{
      auto abs_m = std::abs(m);
      if ((lx + ly - abs_m)%2)
        return 0.0;
      auto j = (lx + ly - abs_m)/2;
      if (j < 0)
        return 0.0;
      auto term1 = sqrt( fact(2*lx)*fact(2*ly)*fact(2*lz)*fact(L)*fact(L-abs_m)
    		  	  	  	  /fact(2*L)/fact(lx)/fact(ly)/fact(lz)/fact(L+abs_m));
      term1/=fact(L);
      term1/=pow(2,L);
      auto term2 = 0;
      for(int i=0;i<=(L-abs_m)/2;i++)
      {
    	  term2+=binom(L,i)*binom(i,j)
    			  *parity(i) * fact(2*(L-i))
				  /fact(L-abs_m-2*i);
      }
      double term3=0;
      for (int k=0;k<=j;k++)
    	  term3+=binom(j,k)*binom(abs_m,lx-2*k)*term4(m,abs_m - lx +2*k);
      // for m!=0, real solid harmonics are linear combinations of complex ones
      // R+(l,m) = ( Y(l,m) + Y(l,-m) )/ sqrt(2) ;  R-(l,m) = ( Y(l,m) - Y(l,-m) )/ sqrt(-2)
      double result = (m == 0) ? term1*term2*term3 : M_SQRT2*term1*term2*term3;
      return result;
}

Eigen::MatrixXd get_trans_mat(Shell &shell)
{
	std::vector<std::array<size_t,3>> cart_order = opencap_carts_ordering(shell.l);
	std::vector<int> sph_order = opencap_harmonic_ordering(shell.l);
	Eigen::MatrixXd trans_mat(shell.num_bf(),shell.num_carts());
	for(size_t i=0;i<shell.num_bf();i++)
	{
		int M = sph_order[i];
		for(size_t j=0;j<shell.num_carts();j++)
		{
			std::array<size_t,3> cart = cart_order[j];
			trans_mat(i,j) = get_coeff(shell.l,M,cart[0],cart[1],cart[2]);
		}
	}
	return trans_mat;
}

Eigen::MatrixXd transform_block(Shell &shell1, Shell &shell2, Eigen::MatrixXd cart_block)
{

	if(!shell1.pure && !shell2.pure)
		return cart_block;
	else if(shell1.pure && !shell2.pure)
		return get_trans_mat(shell1)*cart_block;
	else if(shell1.pure && shell2.pure)
		return get_trans_mat(shell1)*cart_block*get_trans_mat(shell2).transpose();
	else
		return cart_block*get_trans_mat(shell2).transpose();
}

void cart2spherical(Eigen::MatrixXd &cart_ints, Eigen::MatrixXd &spherical_ints, BasisSet &bs)
{
	//indices for first basis function for cart and spherical matrices
	unsigned int cart_row_idx = 0;
	unsigned int sph_row_idx = 0;
	for(auto shell1:bs.basis)
	{
		//indices for 2nd basis function for cart and spherical matrices
		unsigned int cart_col_idx = 0;
		unsigned int sph_col_idx = 0;
		for(auto shell2:bs.basis)
		{
			Eigen::MatrixXd cart_block = cart_ints.block(cart_row_idx,cart_col_idx, shell1.num_carts(),shell2.num_carts());
			spherical_ints.block(sph_row_idx,sph_col_idx,shell1.num_bf(),shell2.num_bf())
							= transform_block(shell1,shell2,cart_block);
			cart_col_idx+=shell2.num_carts();
			sph_col_idx+=shell2.num_bf();
		}
		cart_row_idx+=shell1.num_carts();
		sph_row_idx+=shell1.num_bf();
	}
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "bbw.h"
#include "mosek_quadprog.h"
#include "../harmonic.h"
#include "../slice_into.h"
#include <Eigen/Sparse>
#include <iostream>
#include <cstdio>

template <
  typename DerivedV,
  typename DerivedEle,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::mosek::bbw(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedEle> & Ele,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  igl::BBWData & data,
  igl::mosek::MosekData & mosek_data,
  Eigen::PlainObjectBase<DerivedW> & W
  )
{
  using namespace std;
  using namespace Eigen;
  assert(!data.partition_unity && "partition_unity not implemented yet");
  // number of domain vertices
  int n = V.rows();
  // number of handles
  int m = bc.cols();
  // Build biharmonic operator
  Eigen::SparseMatrix<typename DerivedV::Scalar> Q;
  harmonic(V,Ele,2,Q);
  W.derived().resize(n,m);
  // No linear terms
  VectorXd c = VectorXd::Zero(n);
  // No linear constraints
  SparseMatrix<typename DerivedW::Scalar> A(0,n);
  VectorXd uc(0,1),lc(0,1);
  // Upper and lower box constraints (Constant bounds)
  VectorXd ux = VectorXd::Ones(n);
  VectorXd lx = VectorXd::Zero(n);
  // Loop over handles
  for(int i = 0;i<m;i++)
  {
    if(data.verbosity >= 1)
    {
      cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
        "."<<endl;
    }
    VectorXd bci = bc.col(i);
    VectorXd Wi;
    // impose boundary conditions via bounds
    slice_into(bci,b,ux);
    slice_into(bci,b,lx);
    bool r = mosek_quadprog(Q,c,0,A,lc,uc,lx,ux,mosek_data,Wi);
    if(!r)
    {
      return false;
    }
    W.col(i) = Wi;
  }
#ifndef NDEBUG
    const double min_rowsum = W.rowwise().sum().array().abs().minCoeff();
    if(min_rowsum < 0.1)
    {
      cerr<<"bbw.cpp: Warning, minimum row sum is very low. Consider more "
        "active set iterations or enforcing partition of unity."<<endl;
    }
#endif

  return true;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template bool igl::mosek::bbw<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, igl::BBWData&, igl::mosek::MosekData&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


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

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

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_EDWILLIAMS_AVFORM_INTERMEDIATE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_EDWILLIAMS_AVFORM_INTERMEDIATE_HPP

#include <random>
#include <cmath>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

template
<
    typename Point,
    typename DomainGeometry
>
struct edwilliams_avform_intermediate
{
    edwilliams_avform_intermediate(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                edwilliams_avform_intermediate const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }

    // The following implementation is adapted from
    // https://www.edwilliams.org/avform.htm#Intermediate
    template
    <
        typename LengthType,
        typename PointIn
    >
    static Point map(PointIn const& p1, PointIn const& p2, LengthType const& f)
    {
        Point out;
        const auto lat1 = get_as_radian<1>(p1);
        const auto lon1 = get_as_radian<0>(p1);
        const auto lat2 = get_as_radian<1>(p2);
        const auto lon2 = get_as_radian<0>(p2);
        LengthType const d = std::acos(
              std::sin(lat1) * std::sin(lat2)
            + std::cos(lat1) * std::cos(lat2) * std::cos(lon1 - lon2));
        LengthType const A = std::sin( ( 1 - f ) * d ) / std::sin(d);
        LengthType const B = std::sin( f * d ) / std::sin( d );
        LengthType const x = A * std::cos(lat1) * std::cos(lon1)
                           + B * std::cos(lat2) * std::cos(lon2);
        LengthType const y = A * std::cos(lat1) * std::sin(lon1)
                           + B * std::cos(lat2) * std::sin(lon2);
        LengthType const z = A * std::sin(lat1) + B * std::sin(lat2);
        LengthType const lat = std::atan2(z, std::sqrt(x * x + y * y));
        LengthType const lon = std::atan2(y, x);
        set_from_radian<1>(out, lat);
        set_from_radian<0>(out, lon);
        return out;
    }

    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        typedef typename select_most_precise
            <
                typename coordinate_type<DomainGeometry>::type,
                double
            >::type sample_type;
        std::uniform_real_distribution<sample_type> real_dist(0, 1);
        return map<sample_type>(d, real_dist(g));
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry
>
struct default_strategy
<
    Point,
    DomainGeometry,
    segment_tag,
    single_tag,
    2,
    spherical_tag
> : public edwilliams_avform_intermediate<Point, DomainGeometry> {
    typedef edwilliams_avform_intermediate<Point, DomainGeometry> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_EDWILLIAMS_AVFORM_INTERMEDIATE_HPP