/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef GCOPTER_HPP
#define GCOPTER_HPP

#include "gcopter/minco.hpp"
#include "gcopter/flatness.hpp"
#include "gcopter/lbfgs.hpp"

#include <Eigen/Eigen>

#include <cmath>
#include <cfloat>
#include <iostream>
#include <vector>

namespace gcopter
{

    class GCOPTER_PolytopeSFC
    {
    public:
        typedef Eigen::Matrix3Xd PolyhedronV;
        typedef Eigen::MatrixX4d PolyhedronH;
        typedef std::vector<PolyhedronV> PolyhedraV;
        typedef std::vector<PolyhedronH> PolyhedraH;

    private:
        minco::MINCO_S3NU minco;
        flatness::FlatnessMap flatmap;

        double rho;
        Eigen::Matrix3d headPVA;
        Eigen::Matrix3d tailPVA;

        PolyhedraV vPolytopes;
        PolyhedraH hPolytopes;
        Eigen::Matrix3Xd shortPath;

        Eigen::VectorXi pieceIdx;
        Eigen::VectorXi vPolyIdx;
        Eigen::VectorXi hPolyIdx;

        int polyN;
        int pieceN;

        int spatialDim;
        int temporalDim;

        double smoothEps;
        int integralRes;
        Eigen::VectorXd magnitudeBd;
        Eigen::VectorXd penaltyWt;
        Eigen::VectorXd physicalPm;
        double allocSpeed;

        lbfgs::lbfgs_parameter_t lbfgs_params;

        Eigen::Matrix3Xd points;
        Eigen::VectorXd times;
        Eigen::Matrix3Xd gradByPoints;
        Eigen::VectorXd gradByTimes;
        Eigen::MatrixX3d partialGradByCoeffs;
        Eigen::VectorXd partialGradByTimes;

    private:
        static inline void forwardT(const Eigen::VectorXd &tau,
                                    Eigen::VectorXd &T)
        {
            const int sizeTau = tau.size();
            T.resize(sizeTau);
            for (int i = 0; i < sizeTau; i++)
            {
                T(i) = tau(i) > 0.0
                           ? ((0.5 * tau(i) + 1.0) * tau(i) + 1.0)
                           : 1.0 / ((0.5 * tau(i) - 1.0) * tau(i) + 1.0);
            }
            return;
        }

        template <typename EIGENVEC>
        static inline void backwardT(const Eigen::VectorXd &T,
                                     EIGENVEC &tau)
        {
            const int sizeT = T.size();
            tau.resize(sizeT);
            for (int i = 0; i < sizeT; i++)
            {
                tau(i) = T(i) > 1.0
                             ? (sqrt(2.0 * T(i) - 1.0) - 1.0)
                             : (1.0 - sqrt(2.0 / T(i) - 1.0));
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void backwardGradT(const Eigen::VectorXd &tau,
                                         const Eigen::VectorXd &gradT,
                                         EIGENVEC &gradTau)
        {
            const int sizeTau = tau.size();
            gradTau.resize(sizeTau);
            double denSqrt;
            for (int i = 0; i < sizeTau; i++)
            {
                if (tau(i) > 0)
                {
                    gradTau(i) = gradT(i) * (tau(i) + 1.0);
                }
                else
                {
                    denSqrt = (0.5 * tau(i) - 1.0) * tau(i) + 1.0;
                    gradTau(i) = gradT(i) * (1.0 - tau(i)) / (denSqrt * denSqrt);
                }
            }

            return;
        }

        static inline void forwardP(const Eigen::VectorXd &xi,
                                    const Eigen::VectorXi &vIdx,
                                    const PolyhedraV &vPolys,
                                    Eigen::Matrix3Xd &P)
        {
            const int sizeP = vIdx.size();
            P.resize(3, sizeP);
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k).normalized().head(k - 1);
                P.col(i) = vPolys[l].rightCols(k - 1) * q.cwiseProduct(q) +
                           vPolys[l].col(0);
            }
            return;
        }

        static inline double costTinyNLS(void *ptr,
                                         const Eigen::VectorXd &xi,
                                         Eigen::VectorXd &gradXi)
        {
            const int n = xi.size();
            const Eigen::Matrix3Xd &ovPoly = *(Eigen::Matrix3Xd *)ptr;

            const double sqrNormXi = xi.squaredNorm();
            const double invNormXi = 1.0 / sqrt(sqrNormXi);
            const Eigen::VectorXd unitXi = xi * invNormXi;
            const Eigen::VectorXd r = unitXi.head(n - 1);
            const Eigen::Vector3d delta = ovPoly.rightCols(n - 1) * r.cwiseProduct(r) +
                                          ovPoly.col(1) - ovPoly.col(0);

            double cost = delta.squaredNorm();
            gradXi.head(n - 1) = (ovPoly.rightCols(n - 1).transpose() * (2 * delta)).array() *
                                 r.array() * 2.0;
            gradXi(n - 1) = 0.0;
            gradXi = (gradXi - unitXi.dot(gradXi) * unitXi).eval() * invNormXi;

            const double sqrNormViolation = sqrNormXi - 1.0;
            if (sqrNormViolation > 0.0)
            {
                double c = sqrNormViolation * sqrNormViolation;
                const double dc = 3.0 * c;
                c *= sqrNormViolation;
                cost += c;
                gradXi += dc * 2.0 * xi;
            }

            return cost;
        }

        template <typename EIGENVEC>
        static inline void backwardP(const Eigen::Matrix3Xd &P,
                                     const Eigen::VectorXi &vIdx,
                                     const PolyhedraV &vPolys,
                                     EIGENVEC &xi)
        {
            const int sizeP = P.cols();

            double minSqrD;
            lbfgs::lbfgs_parameter_t tiny_nls_params;
            tiny_nls_params.past = 0;
            tiny_nls_params.delta = 1.0e-5;
            tiny_nls_params.g_epsilon = FLT_EPSILON;
            tiny_nls_params.max_iterations = 128;

            Eigen::Matrix3Xd ovPoly;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();

                ovPoly.resize(3, k + 1);
                ovPoly.col(0) = P.col(i);
                ovPoly.rightCols(k) = vPolys[l];
                Eigen::VectorXd x(k);
                x.setConstant(sqrt(1.0 / k));
                lbfgs::lbfgs_optimize(x,
                                      minSqrD,
                                      &GCOPTER_PolytopeSFC::costTinyNLS,
                                      nullptr,
                                      nullptr,
                                      &ovPoly,
                                      tiny_nls_params);

                xi.segment(j, k) = x;
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void backwardGradP(const Eigen::VectorXd &xi,
                                         const Eigen::VectorXi &vIdx,
                                         const PolyhedraV &vPolys,
                                         const Eigen::Matrix3Xd &gradP,
                                         EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double normInv;
            Eigen::VectorXd q, gradQ, unitQ;
            for (int i = 0, j = 0, k, l; i < sizeP; i++, j += k)
            {
                l = vIdx(i);
                k = vPolys[l].cols();
                q = xi.segment(j, k);
                normInv = 1.0 / q.norm();
                unitQ = q * normInv;
                gradQ.resize(k);
                gradQ.head(k - 1) = (vPolys[l].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;
                gradXi.segment(j, k) = (gradQ - unitQ * unitQ.dot(gradQ)) * normInv;
            }

            return;
        }

        template <typename EIGENVEC>
        static inline void normRetrictionLayer(const Eigen::VectorXd &xi,
                                               const Eigen::VectorXi &vIdx,
                                               const PolyhedraV &vPolys,
                                               double &cost,
                                               EIGENVEC &gradXi)
        {
            const int sizeP = vIdx.size();
            gradXi.resize(xi.size());

            double sqrNormQ, sqrNormViolation, c, dc;
            Eigen::VectorXd q;
            for (int i = 0, j = 0, k; i < sizeP; i++, j += k)
            {
                k = vPolys[vIdx(i)].cols();

                q = xi.segment(j, k);
                sqrNormQ = q.squaredNorm();
                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradXi.segment(j, k) += dc * 2.0 * q;
                }
            }

            return;
        }

        static inline bool smoothedL1(const double &x,
                                      const double &mu,
                                      double &f,
                                      double &df)
        {
            if (x < 0.0)
            {
                return false;
            }
            else if (x > mu)
            {
                f = x - 0.5 * mu;
                df = 1.0;
                return true;
            }
            else
            {
                const double xdmu = x / mu;
                const double sqrxdmu = xdmu * xdmu;
                const double mumxd2 = mu - 0.5 * x;
                f = mumxd2 * sqrxdmu * xdmu;
                df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu);
                return true;
            }
        }

        // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
        // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
        // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
        //                   vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
        static inline void attachPenaltyFunctional(const Eigen::VectorXd &T,
                                                   const Eigen::MatrixX3d &coeffs,
                                                   const Eigen::VectorXi &hIdx,
                                                   const PolyhedraH &hPolys,
                                                   const double &smoothFactor,
                                                   const int &integralResolution,
                                                   const Eigen::VectorXd &magnitudeBounds,
                                                   const Eigen::VectorXd &penaltyWeights,
                                                   flatness::FlatnessMap &flatMap,
                                                   double &cost,
                                                   Eigen::VectorXd &gradT,
                                                   Eigen::MatrixX3d &gradC)
        {
            const double velSqrMax = magnitudeBounds(0) * magnitudeBounds(0);
            const double omgSqrMax = magnitudeBounds(1) * magnitudeBounds(1);
            const double thetaMax = magnitudeBounds(2);
            const double thrustMean = 0.5 * (magnitudeBounds(3) + magnitudeBounds(4));
            const double thrustRadi = 0.5 * fabs(magnitudeBounds(4) - magnitudeBounds(3));
            const double thrustSqrRadi = thrustRadi * thrustRadi;

            const double weightPos = penaltyWeights(0);
            const double weightVel = penaltyWeights(1);
            const double weightOmg = penaltyWeights(2);
            const double weightTheta = penaltyWeights(3);
            const double weightThrust = penaltyWeights(4);

            Eigen::Vector3d pos, vel, acc, jer, sna;
            Eigen::Vector3d totalGradPos, totalGradVel, totalGradAcc, totalGradJer;
            double totalGradPsi, totalGradPsiD;
            double thr, cos_theta;
            Eigen::Vector4d quat;
            Eigen::Vector3d omg;
            double gradThr;
            Eigen::Vector4d gradQuat;
            Eigen::Vector3d gradPos, gradVel, gradOmg;

            double step, alpha;
            double s1, s2, s3, s4, s5;
            Eigen::Matrix<double, 6, 1> beta0, beta1, beta2, beta3, beta4;
            Eigen::Vector3d outerNormal;
            int K, L;
            double violaPos, violaVel, violaOmg, violaTheta, violaThrust;
            double violaPosPenaD, violaVelPenaD, violaOmgPenaD, violaThetaPenaD, violaThrustPenaD;
            double violaPosPena, violaVelPena, violaOmgPena, violaThetaPena, violaThrustPena;
            double node, pena;

            const int pieceNum = T.size();
            const double integralFrac = 1.0 / integralResolution;
            for (int i = 0; i < pieceNum; i++)
            {
                const Eigen::Matrix<double, 6, 3> &c = coeffs.block<6, 3>(i * 6, 0);
                step = T(i) * integralFrac;
                for (int j = 0; j <= integralResolution; j++)
                {
                    s1 = j * step;
                    s2 = s1 * s1;
                    s3 = s2 * s1;
                    s4 = s2 * s2;
                    s5 = s4 * s1;
                    beta0(0) = 1.0, beta0(1) = s1, beta0(2) = s2, beta0(3) = s3, beta0(4) = s4, beta0(5) = s5;
                    beta1(0) = 0.0, beta1(1) = 1.0, beta1(2) = 2.0 * s1, beta1(3) = 3.0 * s2, beta1(4) = 4.0 * s3, beta1(5) = 5.0 * s4;
                    beta2(0) = 0.0, beta2(1) = 0.0, beta2(2) = 2.0, beta2(3) = 6.0 * s1, beta2(4) = 12.0 * s2, beta2(5) = 20.0 * s3;
                    beta3(0) = 0.0, beta3(1) = 0.0, beta3(2) = 0.0, beta3(3) = 6.0, beta3(4) = 24.0 * s1, beta3(5) = 60.0 * s2;
                    beta4(0) = 0.0, beta4(1) = 0.0, beta4(2) = 0.0, beta4(3) = 0.0, beta4(4) = 24.0, beta4(5) = 120.0 * s1;
                    pos = c.transpose() * beta0;
                    vel = c.transpose() * beta1;
                    acc = c.transpose() * beta2;
                    jer = c.transpose() * beta3;
                    sna = c.transpose() * beta4;

                    flatMap.forward(vel, acc, jer, 0.0, 0.0, thr, quat, omg);

                    violaVel = vel.squaredNorm() - velSqrMax;
                    violaOmg = omg.squaredNorm() - omgSqrMax;
                    cos_theta = 1.0 - 2.0 * (quat(1) * quat(1) + quat(2) * quat(2));
                    violaTheta = acos(cos_theta) - thetaMax;
                    violaThrust = (thr - thrustMean) * (thr - thrustMean) - thrustSqrRadi;

                    gradThr = 0.0;
                    gradQuat.setZero();
                    gradPos.setZero(), gradVel.setZero(), gradOmg.setZero();
                    pena = 0.0;

                    L = hIdx(i);
                    K = hPolys[L].rows();
                    for (int k = 0; k < K; k++)
                    {
                        outerNormal = hPolys[L].block<1, 3>(k, 0);
                        violaPos = outerNormal.dot(pos) + hPolys[L](k, 3);
                        if (smoothedL1(violaPos, smoothFactor, violaPosPena, violaPosPenaD))
                        {
                            gradPos += weightPos * violaPosPenaD * outerNormal;
                            pena += weightPos * violaPosPena;
                        }
                    }

                    if (smoothedL1(violaVel, smoothFactor, violaVelPena, violaVelPenaD))
                    {
                        gradVel += weightVel * violaVelPenaD * 2.0 * vel;
                        pena += weightVel * violaVelPena;
                    }

                    if (smoothedL1(violaOmg, smoothFactor, violaOmgPena, violaOmgPenaD))
                    {
                        gradOmg += weightOmg * violaOmgPenaD * 2.0 * omg;
                        pena += weightOmg * violaOmgPena;
                    }

                    if (smoothedL1(violaTheta, smoothFactor, violaThetaPena, violaThetaPenaD))
                    {
                        gradQuat += weightTheta * violaThetaPenaD /
                                    sqrt(1.0 - cos_theta * cos_theta) * 4.0 *
                                    Eigen::Vector4d(0.0, quat(1), quat(2), 0.0);
                        pena += weightTheta * violaThetaPena;
                    }

                    if (smoothedL1(violaThrust, smoothFactor, violaThrustPena, violaThrustPenaD))
                    {
                        gradThr += weightThrust * violaThrustPenaD * 2.0 * (thr - thrustMean);
                        pena += weightThrust * violaThrustPena;
                    }

                    flatMap.backward(gradPos, gradVel, gradThr, gradQuat, gradOmg,
                                     totalGradPos, totalGradVel, totalGradAcc, totalGradJer,
                                     totalGradPsi, totalGradPsiD);

                    node = (j == 0 || j == integralResolution) ? 0.5 : 1.0;
                    alpha = j * integralFrac;
                    gradC.block<6, 3>(i * 6, 0) += (beta0 * totalGradPos.transpose() +
                                                    beta1 * totalGradVel.transpose() +
                                                    beta2 * totalGradAcc.transpose() +
                                                    beta3 * totalGradJer.transpose()) *
                                                   node * step;
                    gradT(i) += (totalGradPos.dot(vel) +
                                 totalGradVel.dot(acc) +
                                 totalGradAcc.dot(jer) +
                                 totalGradJer.dot(sna)) *
                                    alpha * node * step +
                                node * integralFrac * pena;
                    cost += node * step * pena;
                }
            }

            return;
        }

        static inline double costFunctional(void *ptr,
                                            const Eigen::VectorXd &x,
                                            Eigen::VectorXd &g)
        {
            GCOPTER_PolytopeSFC &obj = *(GCOPTER_PolytopeSFC *)ptr;
            const int dimTau = obj.temporalDim;
            const int dimXi = obj.spatialDim;
            const double weightT = obj.rho;
            Eigen::Map<const Eigen::VectorXd> tau(x.data(), dimTau);
            Eigen::Map<const Eigen::VectorXd> xi(x.data() + dimTau, dimXi);
            Eigen::Map<Eigen::VectorXd> gradTau(g.data(), dimTau);
            Eigen::Map<Eigen::VectorXd> gradXi(g.data() + dimTau, dimXi);

            forwardT(tau, obj.times);
            forwardP(xi, obj.vPolyIdx, obj.vPolytopes, obj.points);

            double cost;
            obj.minco.setParameters(obj.points, obj.times);
            obj.minco.getEnergy(cost);
            obj.minco.getEnergyPartialGradByCoeffs(obj.partialGradByCoeffs);
            obj.minco.getEnergyPartialGradByTimes(obj.partialGradByTimes);

            attachPenaltyFunctional(obj.times, obj.minco.getCoeffs(),
                                    obj.hPolyIdx, obj.hPolytopes,
                                    obj.smoothEps, obj.integralRes,
                                    obj.magnitudeBd, obj.penaltyWt, obj.flatmap,
                                    cost, obj.partialGradByTimes, obj.partialGradByCoeffs);

            obj.minco.propogateGrad(obj.partialGradByCoeffs, obj.partialGradByTimes,
                                    obj.gradByPoints, obj.gradByTimes);

            cost += weightT * obj.times.sum();
            obj.gradByTimes.array() += weightT;

            backwardGradT(tau, obj.gradByTimes, gradTau);
            backwardGradP(xi, obj.vPolyIdx, obj.vPolytopes, obj.gradByPoints, gradXi);
            normRetrictionLayer(xi, obj.vPolyIdx, obj.vPolytopes, cost, gradXi);

            return cost;
        }

        static inline double costDistance(void *ptr,
                                          const Eigen::VectorXd &xi,
                                          Eigen::VectorXd &gradXi)
        {
            void **dataPtrs = (void **)ptr;
            const double &dEps = *((const double *)(dataPtrs[0]));
            const Eigen::Vector3d &ini = *((const Eigen::Vector3d *)(dataPtrs[1]));
            const Eigen::Vector3d &fin = *((const Eigen::Vector3d *)(dataPtrs[2]));
            const PolyhedraV &vPolys = *((PolyhedraV *)(dataPtrs[3]));

            double cost = 0.0;
            const int overlaps = vPolys.size() / 2;

            Eigen::Matrix3Xd gradP = Eigen::Matrix3Xd::Zero(3, overlaps);
            Eigen::Vector3d a, b, d;
            Eigen::VectorXd r;
            double smoothedDistance;
            for (int i = 0, j = 0, k = 0; i <= overlaps; i++, j += k)
            {
                a = i == 0 ? ini : b;
                if (i < overlaps)
                {
                    k = vPolys[2 * i + 1].cols();
                    Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                    r = q.normalized().head(k - 1);
                    b = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                        vPolys[2 * i + 1].col(0);
                }
                else
                {
                    b = fin;
                }

                d = b - a;
                smoothedDistance = sqrt(d.squaredNorm() + dEps);
                cost += smoothedDistance;

                if (i < overlaps)
                {
                    gradP.col(i) += d / smoothedDistance;
                }
                if (i > 0)
                {
                    gradP.col(i - 1) -= d / smoothedDistance;
                }
            }

            Eigen::VectorXd unitQ;
            double sqrNormQ, invNormQ, sqrNormViolation, c, dc;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                Eigen::Map<Eigen::VectorXd> gradQ(gradXi.data() + j, k);
                sqrNormQ = q.squaredNorm();
                invNormQ = 1.0 / sqrt(sqrNormQ);
                unitQ = q * invNormQ;
                gradQ.head(k - 1) = (vPolys[2 * i + 1].rightCols(k - 1).transpose() * gradP.col(i)).array() *
                                    unitQ.head(k - 1).array() * 2.0;
                gradQ(k - 1) = 0.0;
                gradQ = (gradQ - unitQ * unitQ.dot(gradQ)).eval() * invNormQ;

                sqrNormViolation = sqrNormQ - 1.0;
                if (sqrNormViolation > 0.0)
                {
                    c = sqrNormViolation * sqrNormViolation;
                    dc = 3.0 * c;
                    c *= sqrNormViolation;
                    cost += c;
                    gradQ += dc * 2.0 * q;
                }
            }

            return cost;
        }

        static inline void getShortestPath(const Eigen::Vector3d &ini,
                                           const Eigen::Vector3d &fin,
                                           const PolyhedraV &vPolys,
                                           const double &smoothD,
                                           Eigen::Matrix3Xd &path)
        {
            const int overlaps = vPolys.size() / 2;
            Eigen::VectorXi vSizes(overlaps);
            for (int i = 0; i < overlaps; i++)
            {
                vSizes(i) = vPolys[2 * i + 1].cols();
            }
            Eigen::VectorXd xi(vSizes.sum());
            for (int i = 0, j = 0; i < overlaps; i++)
            {
                xi.segment(j, vSizes(i)).setConstant(sqrt(1.0 / vSizes(i)));
                j += vSizes(i);
            }

            double minDistance;
            void *dataPtrs[4];
            dataPtrs[0] = (void *)(&smoothD);
            dataPtrs[1] = (void *)(&ini);
            dataPtrs[2] = (void *)(&fin);
            dataPtrs[3] = (void *)(&vPolys);
            lbfgs::lbfgs_parameter_t shortest_path_params;
            shortest_path_params.past = 3;
            shortest_path_params.delta = 1.0e-3;
            shortest_path_params.g_epsilon = 1.0e-5;

            lbfgs::lbfgs_optimize(xi,
                                  minDistance,
                                  &GCOPTER_PolytopeSFC::costDistance,
                                  nullptr,
                                  nullptr,
                                  dataPtrs,
                                  shortest_path_params);

            path.resize(3, overlaps + 2);
            path.leftCols<1>() = ini;
            path.rightCols<1>() = fin;
            Eigen::VectorXd r;
            for (int i = 0, j = 0, k; i < overlaps; i++, j += k)
            {
                k = vPolys[2 * i + 1].cols();
                Eigen::Map<const Eigen::VectorXd> q(xi.data() + j, k);
                r = q.normalized().head(k - 1);
                path.col(i + 1) = vPolys[2 * i + 1].rightCols(k - 1) * r.cwiseProduct(r) +
                                  vPolys[2 * i + 1].col(0);
            }

            return;
        }

        static inline bool processCorridor(const PolyhedraH &hPs,
                                           PolyhedraV &vPs)
        {
            const int sizeCorridor = hPs.size() - 1;

            vPs.clear();
            vPs.reserve(2 * sizeCorridor + 1);

            int nv;
            PolyhedronH curIH;
            PolyhedronV curIV, curIOB;
            for (int i = 0; i < sizeCorridor; i++)
            {
                if (!geo_utils::enumerateVs(hPs[i], curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);

                curIH.resize(hPs[i].rows() + hPs[i + 1].rows(), 4);
                curIH.topRows(hPs[i].rows()) = hPs[i];
                curIH.bottomRows(hPs[i + 1].rows()) = hPs[i + 1];
                if (!geo_utils::enumerateVs(curIH, curIV))
                {
                    return false;
                }
                nv = curIV.cols();
                curIOB.resize(3, nv);
                curIOB.col(0) = curIV.col(0);
                curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
                vPs.push_back(curIOB);
            }

            if (!geo_utils::enumerateVs(hPs.back(), curIV))
            {
                return false;
            }
            nv = curIV.cols();
            curIOB.resize(3, nv);
            curIOB.col(0) = curIV.col(0);
            curIOB.rightCols(nv - 1) = curIV.rightCols(nv - 1).colwise() - curIV.col(0);
            vPs.push_back(curIOB);

            return true;
        }

        static inline void setInitial(const Eigen::Matrix3Xd &path,
                                      const double &speed,
                                      const Eigen::VectorXi &intervalNs,
                                      Eigen::Matrix3Xd &innerPoints,
                                      Eigen::VectorXd &timeAlloc)
        {
            const int sizeM = intervalNs.size();
            const int sizeN = intervalNs.sum();
            innerPoints.resize(3, sizeN - 1);
            timeAlloc.resize(sizeN);

            Eigen::Vector3d a, b, c;
            for (int i = 0, j = 0, k = 0, l; i < sizeM; i++)
            {
                l = intervalNs(i);
                a = path.col(i);
                b = path.col(i + 1);
                c = (b - a) / l;
                timeAlloc.segment(j, l).setConstant(c.norm() / speed);
                j += l;
                for (int m = 0; m < l; m++)
                {
                    if (i > 0 || m > 0)
                    {
                        innerPoints.col(k++) = a + c * m;
                    }
                }
            }
        }

    public:
        // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
        // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
        // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
        //                   vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
        inline bool setup(const double &timeWeight,
                          const Eigen::Matrix3d &initialPVA,
                          const Eigen::Matrix3d &terminalPVA,
                          const PolyhedraH &safeCorridor,
                          const double &lengthPerPiece,
                          const double &smoothingFactor,
                          const int &integralResolution,
                          const Eigen::VectorXd &magnitudeBounds,
                          const Eigen::VectorXd &penaltyWeights,
                          const Eigen::VectorXd &physicalParams)
        {
            rho = timeWeight;
            headPVA = initialPVA;
            tailPVA = terminalPVA;

            hPolytopes = safeCorridor;
            for (size_t i = 0; i < hPolytopes.size(); i++)
            {
                const Eigen::ArrayXd norms =
                    hPolytopes[i].leftCols<3>().rowwise().norm();
                hPolytopes[i].array().colwise() /= norms;
            }
            if (!processCorridor(hPolytopes, vPolytopes))
            {
                return false;
            }

            polyN = hPolytopes.size();
            smoothEps = smoothingFactor;
            integralRes = integralResolution;
            magnitudeBd = magnitudeBounds;
            penaltyWt = penaltyWeights;
            physicalPm = physicalParams;
            allocSpeed = magnitudeBd(0) * 3.0;

            getShortestPath(headPVA.col(0), tailPVA.col(0),
                            vPolytopes, smoothEps, shortPath);
            const Eigen::Matrix3Xd deltas = shortPath.rightCols(polyN) - shortPath.leftCols(polyN);
            pieceIdx = (deltas.colwise().norm() / lengthPerPiece).cast<int>().transpose();
            pieceIdx.array() += 1;
            pieceN = pieceIdx.sum();

            temporalDim = pieceN;
            spatialDim = 0;
            vPolyIdx.resize(pieceN - 1);
            hPolyIdx.resize(pieceN);
            for (int i = 0, j = 0, k; i < polyN; i++)
            {
                k = pieceIdx(i);
                for (int l = 0; l < k; l++, j++)
                {
                    if (l < k - 1)
                    {
                        vPolyIdx(j) = 2 * i;
                        spatialDim += vPolytopes[2 * i].cols();
                    }
                    else if (i < polyN - 1)
                    {
                        vPolyIdx(j) = 2 * i + 1;
                        spatialDim += vPolytopes[2 * i + 1].cols();
                    }
                    hPolyIdx(j) = i;
                }
            }

            // Setup for MINCO_S3NU, FlatnessMap, and L-BFGS solver
            minco.setConditions(headPVA, tailPVA, pieceN);
            flatmap.reset(physicalPm(0), physicalPm(1), physicalPm(2),
                          physicalPm(3), physicalPm(4), physicalPm(5));

            // Allocate temp variables
            points.resize(3, pieceN - 1);
            times.resize(pieceN);
            gradByPoints.resize(3, pieceN - 1);
            gradByTimes.resize(pieceN);
            partialGradByCoeffs.resize(6 * pieceN, 3);
            partialGradByTimes.resize(pieceN);

            return true;
        }

        inline double optimize(Trajectory<5> &traj,
                               const double &relCostTol)
        {
            Eigen::VectorXd x(temporalDim + spatialDim);
            Eigen::Map<Eigen::VectorXd> tau(x.data(), temporalDim);
            Eigen::Map<Eigen::VectorXd> xi(x.data() + temporalDim, spatialDim);

            setInitial(shortPath, allocSpeed, pieceIdx, points, times);
            backwardT(times, tau);
            backwardP(points, vPolyIdx, vPolytopes, xi);

            double minCostFunctional;
            lbfgs_params.mem_size = 256;
            lbfgs_params.past = 3;
            lbfgs_params.min_step = 1.0e-32;
            lbfgs_params.g_epsilon = 0.0;
            lbfgs_params.delta = relCostTol;

            int ret = lbfgs::lbfgs_optimize(x,
                                            minCostFunctional,
                                            &GCOPTER_PolytopeSFC::costFunctional,
                                            nullptr,
                                            nullptr,
                                            this,
                                            lbfgs_params);

            if (ret >= 0)
            {
                forwardT(tau, times);
                forwardP(xi, vPolyIdx, vPolytopes, points);
                minco.setParameters(points, times);
                minco.getTrajectory(traj);
            }
            else
            {
                traj.clear();
                minCostFunctional = INFINITY;
                std::cout << "Optimization Failed: "
                          << lbfgs::lbfgs_strerror(ret)
                          << std::endl;
            }

            return minCostFunctional;
        }
    };

}

#endif


/**
 * @file 	SteinerTreeHeuristic.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <sstream>
#include <boost/heap/d_ary_heap.hpp>
#include "SteinerTreeHeuristic.h"

using std::pair;

/*
 * Data that is stored in one node of the heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority. The int is
 * used for the vertex index and the double for its key during the algorithm
 */
struct heap_data {

	int index;
	weight_type key;

    heap_data(int i, weight_type k): index(i), key(k) {
    }

    bool operator<(heap_data const & rhs) const {
        return key > rhs.key;
    }
};

/*
 * Implementation of the improved Shortest-Path-Heuristic. A 4-ary-heap from
 * boost is used and keys are never decreased, but instead new nodes are pushed
 * whenever a better path is found. The resulting tree is stored implicitly
 */
double SteinerTreeHeuristic::compute_steiner_tree(const CSR_Graph& g, int num_vertices,
		int root, vector<int>& tree_preds, const vector<int>& terminals) {

	assert((int) tree_preds.size() == num_vertices);

	// terminals are scanned
	int num_terminals = terminals.size();
	vector<bool> is_terminal(num_vertices, false);
	for (int i = 0; i < num_terminals; ++i) {
		assert(terminals[i] < num_vertices);
		is_terminal[terminals[i]] = true;
	}

	assert(is_terminal[root]);

	heap::d_ary_heap<heap_data, heap::arity<4>> heap;

	// this vector is used to store the info about predecessor and the respective
	// edge together in order to avoid cache misses
	vector<pair<int,weight_type>> preds(num_vertices);
	preds[root] = std::make_pair(root, 0);

	// we expect the tree vector to be filled with -1 already
	for (int i = 0; i < num_vertices; ++i) {
		assert(tree_preds[i] == -1);
	}

	// initialization of distances. all but the root vertex have infinity
	vector<weight_type> dist(num_vertices, std::numeric_limits<weight_type>::infinity());
	dist[root] = 0;
	tree_preds[root] = root;

	heap.push(heap_data(root, 0));

	// this is just boost syntax so that we can access weights and indices later
	property_map<CSR_Graph, edge_weight_t>::const_type weights = get(edge_weight, g);
	property_map<CSR_Graph, vertex_index_t>::const_type index = get(vertex_index, g);

	int connected_terminals = 1;
	weight_type tree_cost = 0;

	// here the actual algorithm starts
	while (connected_terminals < num_terminals) {

		assert(!heap.empty());	//heap should never be empty before tree contains all terminals

		int min_idx = heap.top().index;
		weight_type min_key = heap.top().key;

		heap.pop();

		if (dist[min_idx] < min_key) continue;

		// if we scan a terminal, all vertices on shortest path to subtree
		// are added to heap with weight 0 and included into the tree
		if (is_terminal[min_idx] && tree_preds[min_idx] == -1) {

			++connected_terminals;

			min_key = 0;
			int next_vertex = preds[min_idx].first;
			tree_preds[min_idx] = next_vertex;

			tree_cost += preds[min_idx].second;

			// here all vertices on path from new terminal to tree are added again with key=0
			while (tree_preds[next_vertex] == -1) {

				heap.push(heap_data(next_vertex, 0));
				dist[next_vertex] = 0;
				tree_preds[next_vertex] = preds[next_vertex].first;

				tree_cost += preds[next_vertex].second;

				next_vertex = preds[next_vertex].first;
			}
		}

		typename graph_traits<CSR_Graph>::out_edge_iterator it, it_end;

		// this is basically dijkstra with pushes instead of decreasekey operations
		for (tie(it,it_end) = out_edges(*(vertices(g).first+min_idx),g); it != it_end; ++it) {

			int v_target = index[target(*it, g)];

			// we only consider vertices which have not been added to the tree yet
			if (tree_preds[v_target] != -1) continue;

			if (min_key + weights[*it] < dist[v_target]) {
				// distance is updated
				dist[v_target] = min_key + weights[*it];
				preds[v_target].first = min_idx;
				preds[v_target].second = weights[*it];
				heap.push(heap_data(v_target, dist[v_target]));
			}
		}
	}

	return tree_cost;
}

/* note: this method only works if tree_preds actually represents a tree.
 * If not sure, use function testTree(...)
 */
string SteinerTreeHeuristic::print_tree(const CSR_Graph& g, int num_vertices, int root,
		vector<int>& tree_preds, vector<int>& terminals) {

	std::stringstream stream;
	vector<bool> visited(num_vertices, false);

	// we iterate through the tree going from each terminal until we arrive at root
	for (unsigned int i = 0; i < terminals.size(); ++i) {
		int curr = terminals[i];
		int edge_count = 0;
		while (curr != root && !visited[curr]) {
			++edge_count;

			// every 50 lines a linebreak is added for the sake of readability
			if (edge_count % 50 == 0) stream << std::endl;
			stream << "(" << curr << "," << tree_preds[curr] << ") ";

			visited[curr] = true;
			curr = tree_preds[curr];
		}
	}

	return stream.str();
}

bool SteinerTreeHeuristic::test_tree(const CSR_Graph& g, int num_vertices, int root,
	vector<int>& tree_preds, vector<int>& terminals) {

	vector<bool> visited(num_vertices, false);

	// we iterate through the tree going from each terminal until we arrive at
	// the root or a vertex that we already visited
	for (unsigned int i = 0; i < terminals.size(); ++i) {
		int curr = terminals[i];
		int vertex_count = 1;

		while (curr != root && !visited[curr] && vertex_count != num_vertices) {
			// if one terminal is not connected to root, result is false
			if (curr == -1) return false;
			visited[curr] = true;
			curr = tree_preds[curr];
			++vertex_count;
		}

		// if a cycle is found, result is false
		if (vertex_count == num_vertices || curr == -1){
			return false;
		}
	}

	return true;
}


#include <VoronoiTests.h>

#include <SimilarityGraph.h>
#include <Heuristics.h>

#pragma warning(push)
#pragma warning(disable: 4996)

#include <boost/geometry.hpp>

#pragma warning(pop)

#include <set>

TEST_F(VoronoiTests, Build_TriangleConfiguration_1)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
        | \
        2 - 3
    */
    edges.insert({ 0, 2 });
    edges.insert({ 0, 3 });
    edges.insert({ 2, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getTriangleConfiguration1Answer(), output.str());
}

TEST_F(VoronoiTests, Build_TriangleConfiguration_2)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0 - 1
        | /
        2   3
    */
    edges.insert({ 0, 1 });
    edges.insert({ 0, 2 });
    edges.insert({ 1, 2 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getTriangleConfiguration2Answer(), output.str());
}

TEST_F(VoronoiTests, Build_TriangleConfiguration_3)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0 - 1
          \ |
        2   3
    */
    edges.insert({ 0, 1 });
    edges.insert({ 0, 3 });
    edges.insert({ 1, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getTriangleConfiguration3Answer(), output.str());
}

TEST_F(VoronoiTests, Build_TriangleConfiguration_4)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
          / |
        2 - 3
    */
    edges.insert({ 1, 2 });
    edges.insert({ 1, 3 });
    edges.insert({ 2, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getTriangleConfiguration4Answer(), output.str());
}

TEST_F(VoronoiTests, Build_DiagonalConfiguration_1)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
          /
        2   3
    */

    edges.insert({ 2, 1 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDiagonalConfiguration1Answer(), output.str());
}

TEST_F(VoronoiTests, Build_DiagonalConfiguration_2)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
          \
        2   3
    */

    edges.insert({ 0, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDiagonalConfiguration2Answer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_1)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
        |   |
        2   3
    */
    edges.insert({ 0, 2 });
    edges.insert({ 1, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_2)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0 - 1

        2 - 3
    */
    edges.insert({ 0, 1 });
    edges.insert({ 2, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_3)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
        |
        2   3
    */
    edges.insert({ 0, 2 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_4)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1
            |
        2   3
    */
    edges.insert({ 1, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_5)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0 - 1

        2   3
    */
    edges.insert({ 0, 1 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Build_DefaultConfiguration_6)
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    /*
        0   1

        2 - 3
    */
    edges.insert({ 2, 3 });

    VoronoiDiagram voronoi{ std::make_tuple(2, 2) };
    voronoi.build(edges);

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    EXPECT_EQ(getDefaultConfigurationAnswer(), output.str());
}

TEST_F(VoronoiTests, Weld)
{
    using namespace dpa::image;
    using namespace dpa::graph;

    Image<RGB, stbi_uc> image{ m_curve };

    SimilarityGraph simGraph;
    simGraph.build(image);

    auto imageDims = std::make_tuple(image.getWidth(), image.getHeight());

    simGraph.applyHeuristic(heuristics::DissimilarPixels{});
    simGraph.applyHeuristic(heuristics::Curves{ imageDims });
    simGraph.applyHeuristic(heuristics::Islands{ imageDims });
    simGraph.applyHeuristic(heuristics::SparsePixels{ imageDims });

    VoronoiDiagram voronoi{ imageDims };
    voronoi.build(simGraph.getEdges());

    std::ostringstream output;
    voronoi.printVertices(std::ref(output));

    std::ifstream input{ m_curves_weld_solution };
    std::string solution{ std::istreambuf_iterator<char>(input), std::istreambuf_iterator<char>() };

    EXPECT_EQ(solution, output.str());
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ABS_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <cstdlib>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      using utype = dispatch::as_unsigned_t<T>;

      utype mask = a >> (sizeof(T)*8 - 1);
      return (a + mask) ^ mask;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::single_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      #ifdef BOOST_SIMD_HAS_FABSF
       return ::fabsf(a);
      #else
       return (a > 0) ? a : -a;
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::double_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return ::fabs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::unsigned_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::bool_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( std_tag const&, T a) const BOOST_NOEXCEPT
    {
      return std::fabs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_<bd::unsigned_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( std_tag const&, T a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( std_tag const&,T a) const BOOST_NOEXCEPT
    {
      return std::abs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_<bd::signed_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T const& a0) const BOOST_NOEXCEPT
    {
      return (a0==Valmin<T>())?Valmax<T>():bs::abs(a0);
    }
  };
} } }

#include <boost/simd/arch/common/scalar/function/abs_s.hpp>

#endif


#pragma once
#include <Eigen/Dense>

struct InternalBoundingBox
{
	/// The position of the box's corner that has the least coordinate value in all axes.
	Eigen::Vector3i pos;
	/// The extents of the cube.
	Eigen::Vector3i span;

	static bool IsPointInCube(const InternalBoundingBox& cube, Eigen::Vector3i point)
	{
		bool xIntersects = cube.pos.x() <= point.x() && cube.pos.x() + cube.span.x() > point.x();
		bool yIntersects = cube.pos.y() <= point.y() && cube.pos.y() + cube.span.y() > point.y();
		bool zIntersects = cube.pos.z() <= point.z() && cube.pos.z() + cube.span.z() > point.z();
		return xIntersects && yIntersects && zIntersects;
	}

	static bool CubesIntersect(const InternalBoundingBox& cube1, const InternalBoundingBox& cube2)
	{
		bool xIntersects = !(cube2.pos.x() + cube2.span.x() <= cube1.pos.x() || cube1.pos.x() + cube1.span.x() <= cube2.pos.x());
		bool yIntersects = !(cube2.pos.y() + cube2.span.y() <= cube1.pos.y() || cube1.pos.y() + cube1.span.y() <= cube2.pos.y());
		bool zIntersects = !(cube2.pos.z() + cube2.span.z() <= cube1.pos.z() || cube1.pos.z() + cube1.span.z() <= cube2.pos.z());
		return xIntersects && yIntersects && zIntersects;
	}

	static void SplitCube(const InternalBoundingBox& cube, InternalBoundingBox children[8])
	{
		Eigen::Vector3i newSpan = cube.span / 2;

		children[0].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() };
		children[0].span = newSpan;

		children[1].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[1].span = newSpan;

		children[2].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[2].span = newSpan;

		children[3].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[3].span = newSpan;

		children[4].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() };
		children[4].span = newSpan;

		children[5].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[5].span = newSpan;

		children[6].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[6].span = newSpan;

		children[7].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[7].span = newSpan;
	}

	friend std::ostream& operator<<(std::ostream& os, const InternalBoundingBox& cube)
	{
		os << '[' << cube.pos.x() << ", " << cube.pos.y() << ", " << cube.pos.z() << "] -> "
			<< '[' << cube.pos.x() + cube.span.x() - 1 << ", " << cube.pos.y() + cube.span.y() - 1 <<
			", " << cube.pos.z() + cube.span.z() - 1 << ']';
		return os;
	}
};


#include "coredsp/filter.h"
#include "coredsp/noise.h"
#include "coreutil/cpu.h"
#include <boost/preprocessor.hpp>
#include <functional>
#include <chrono>
#include <vector>

typedef std::chrono::steady_clock Clock;
typedef Clock::time_point TimePoint;
typedef Clock::duration Duration;

static unsigned iteration_count = 64 * 1024;

struct Benchmark {
  const char *name {};
  std::function<float()> fn;
  double run() const;
};

double Benchmark::run() const {
  TimePoint tstart = Clock::now();
  this->fn();
  Duration dur = Clock::now() - tstart;
  auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(dur);
  return ns.count() * 1e-9;
}

#define IIR_NCOEF_MIN 2
#define IIR_NCOEF_LIMIT 256

static constexpr unsigned niir = IIR_NCOEF_LIMIT - IIR_NCOEF_MIN;

#define DECL_IIR_FLT(z, n, t)                               \
  static coredsp::IIR<n, coreutil::simd_t<float>> iirf##n;  \
  static coredsp::IIR<n, coreutil::simd_t<double>> iird##n;
#define DECL_IIRG_FLT(z, n, t)                                 \
  static coredsp::IIRg<coreutil::simd_t<float>> iirgf##n(n);   \
  static coredsp::IIRg<coreutil::simd_t<double>> iirgd##n(n);
BOOST_PP_REPEAT_FROM_TO(IIR_NCOEF_MIN, IIR_NCOEF_LIMIT, DECL_IIR_FLT,);
BOOST_PP_REPEAT_FROM_TO(IIR_NCOEF_MIN, IIR_NCOEF_LIMIT, DECL_IIRG_FLT,);

static Benchmark biirf_simd[niir], biirf_scalar[niir];
static Benchmark biirgf_simd[niir], biirgf_scalar[niir];
static Benchmark biird_simd[niir], biird_scalar[niir];
static Benchmark biirgd_simd[niir], biirgd_scalar[niir];

template <class T> std::function<float()> create_iir_simd_fn(T &filter) {
  return [&filter]() -> float {
    float out {};
    coredsp::WhiteNoise source;
    for (unsigned i = 0; i < iteration_count; ++i)
      out = filter.impl_simd(source.tick());
    return out;
  };
}

template <class T> std::function<float()> create_iir_scalar_fn(T &filter) {
  return [&filter]() -> float {
    float out {};
    coredsp::WhiteNoise source;
    for (unsigned i = 0; i < iteration_count; ++i)
      out = filter.impl_scalar(source.tick());
    return out;
  };
}

int main() {
  coreutil::disable_denormals();

#define DEF_IIR_BENCHMARK(z, n, t)                                      \
  biirf_simd[n - IIR_NCOEF_MIN] = Benchmark{"IIR<" #n "> simd float", create_iir_simd_fn(iirf##n)}; \
  biirf_scalar[n - IIR_NCOEF_MIN] = Benchmark{"IIR<" #n "> scalar float", create_iir_scalar_fn(iirf##n)}; \
  biirgf_simd[n - IIR_NCOEF_MIN] = Benchmark{"IIRg<" #n "> simd float", create_iir_simd_fn(iirgf##n)}; \
  biirgf_scalar[n - IIR_NCOEF_MIN] = Benchmark{"IIRg<" #n "> scalar float", create_iir_scalar_fn(iirgf##n)}; \
  biird_simd[n - IIR_NCOEF_MIN] = Benchmark{"IIR<" #n "> simd double", create_iir_simd_fn(iird##n)}; \
  biird_scalar[n - IIR_NCOEF_MIN] = Benchmark{"IIR<" #n "> scalar double", create_iir_scalar_fn(iird##n)}; \
  biirgd_simd[n - IIR_NCOEF_MIN] = Benchmark{"IIRg<" #n "> simd double", create_iir_simd_fn(iirgd##n)}; \
  biirgd_scalar[n - IIR_NCOEF_MIN] = Benchmark{"IIRg<" #n "> scalar double", create_iir_scalar_fn(iirgd##n)};

  BOOST_PP_REPEAT_FROM_TO(IIR_NCOEF_MIN, IIR_NCOEF_LIMIT, DEF_IIR_BENCHMARK,);

  setlinebuf(stdout);

  for (unsigned i = 0; i < niir; ++i) {
    double tsimdf = biirf_simd[i].run();
    double tscalarf = biirf_scalar[i].run();
    double tgsimdf = biirgf_simd[i].run();
    double tgscalarf = biirgf_scalar[i].run();
    double tsimdd = biird_simd[i].run();
    double tscalard = biird_scalar[i].run();
    double tgsimdd = biirgd_simd[i].run();
    double tgscalard = biirgd_scalar[i].run();

    printf("%u %f %f %f %f %f %f %f %f\n", i + IIR_NCOEF_MIN,
           tsimdf, tscalarf, tgsimdf, tgscalarf,
           tsimdd, tscalard, tgsimdd, tgscalard);
  }

  return 0;
}


//
//  Copyright Karl Meerbergen, 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include "../../blas/test/random.hpp"

#include <boost/numeric/bindings/lapack/driver/ptsv.hpp>
#include <boost/numeric/bindings/lapack/computational/pttrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pttrs.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

struct apply_real
{
  template< typename VectorD, typename VectorE, typename MatrixB >
  static inline std::ptrdiff_t pttrs(const char uplo, const VectorD& d,
                                     const VectorE& e, MatrixB& b)
  {
    return lapack::pttrs(d, e, b);
  }
};

struct apply_complex
{
  template< typename VectorD, typename VectorE, typename MatrixB >
  static inline std::ptrdiff_t pttrs(const char uplo, const VectorD& d,
                                     const VectorE& e, MatrixB& b)
  {
    return lapack::pttrs(uplo, d, e, b);
  }
};

template <typename B, typename X>
bool check_residual(B const& b, X const& x)
{
  typedef typename B::value_type value_type ;

  ublas::matrix<value_type, ublas::column_major> res(b) ;
  row(res,0).minus_assign(value_type(2.0) * row(x,0) - row(x,1)) ;
  for(int i=1; i<res.size1()-1; ++i)
  {
    row(res,i).minus_assign(value_type(2.0) * row(x,i) - row(x,i+1) - row(x,i-1)) ;
  }
  row(res,res.size1()-1).minus_assign(value_type(2.0) * row(x,res.size1()-1) - row(x,res.size1()-2)) ;

  return norm_frobenius(res)<norm_frobenius(b)*1.e-5 ;
} // check_residual()

template <typename T>
int do_value_type()
{
  typedef typename boost::mpl::if_<boost::is_complex<T>, apply_complex, apply_real>::type apply_t;
  const int n = 8 ;
  typedef typename bindings::remove_imaginary<T>::type real_type ;

  typedef ublas::matrix<T, ublas::column_major>     matrix_type ;

  // Set matrix
  int const nrhs = 1 ;
  matrix_type b(n, nrhs);
  ublas::vector< real_type > d(n);
  ublas::vector<T>           e(n-1);

  std::fill(d.begin(), d.end(), 2.0) ;
  std::fill(e.begin(), e.end(), -1.0) ;

  for(int i=0; i<b.size1(); ++i) b(i,0) = random_value<T>() ;

  // Factorize and solve
  matrix_type x(b);
  if(lapack::ptsv(d, e, x)) return -1 ;
  if(!check_residual(b,x)) return 1 ;

  // Restart computations
  std::fill(d.begin(), d.end(), 2.0) ;
  std::fill(e.begin(), e.end(), -1.0) ;

  // Compute factorization.
  if(lapack::pttrf(d, e)) return -1 ;

  // Compute solve
  x.assign(b) ;
  if(apply_t::pttrs('U', d, e, x)) return -2 ;

  if(!check_residual(b,x)) return 1 ;

  x.assign(b) ;
  if(apply_t::pttrs('L', d, e, x)) return -3 ;

  if(!check_residual(b,x)) return 2 ;

  return 0 ;
} // do_value_type()

int main()
{
  // Run tests for different value_types
  std::cout << "double\n" ;
  if(do_value_type< double >()) return 255;

  std::cout << "float\n" ;
  if(do_value_type< float >()) return 255;

  std::cout << "complex<double>\n" ;
  if(do_value_type< std::complex<double> >()) return 255;

  std::cout << "complex<float>\n" ;
  if(do_value_type< std::complex<float> >()) return 255;

  std::cout << "Regression test succeeded\n" ;
  return 0;
}


#define TINYPLY_IMPLEMENTATION

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

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

#include "PLY/tinyply.h"

#include "PLY/PLY.hpp"

using namespace stereo_utils;

void stereo_utils::write_ply_with_color(const std::string& fn, const cv::Mat& disp, const cv::Mat& color,
        const Eigen::MatrixXf &Q, bool flip, const float farLimit, bool binary) {
    // Check the the dimensions of the input images.
    const int rows     = disp.rows;
    const int cols     = disp.cols;
    const int nPixels  = rows * cols;
    const int channels = disp.channels();

    if ( rows != color.rows || cols != color.cols )
    {
        std::stringstream ss;
        ss << "Dimensions of disp[" << rows << ", " << cols
           << "] and color[" << color.rows << "," << color.rows << "] are not compatible.";
        throw std::runtime_error(ss.str());
    }

    if ( 1 != channels || 3 != color.channels() )
    {
        std::stringstream ss;
        ss << "Channel of disp[" << channels << "] or color[" << color.channels() << "] is wrong.";
        throw std::runtime_error(ss.str());
    }

    if ( 4 != Q.rows() || 4 != Q.cols() )
    {
        std::stringstream ss;
        ss << "Q.rows = " << Q.rows() << ", Q.cols = " << Q.cols() << ". ";
        throw std::runtime_error(ss.str());
    }

    // Good to go.

    struct float3_t { float x, y, z; };
    struct uchar3_t { unsigned char r, g, b; };

    std::vector<float3_t> vertexCoor;
    std::vector<uchar3_t> vertexColor;

    const float* pDisp = nullptr;
    const unsigned char* pColor = nullptr;

    cv::Mat colorRGB;
    cv::cvtColor(color.clone(), colorRGB, cv::COLOR_BGR2RGB);

    // const auto eQ = Eigen::Map<
    //         const Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >,
    //         Eigen::Unaligned,
    //         Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>
    // >( Q, 4, 4, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>(4, 1) );

    // const Eigen::Matrix<
    //     float,
    //     Eigen::Dynamic, Eigen::Dynamic,
    //     Eigen::RowMajor > eQ = Q;

//    std::cout << "eQ = " << std::endl << eQ << std::endl;

    // Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > eQ_NC = eQ;
    Eigen::MatrixXf eQ_NC = Q;

    float factor = 1.0;

    if ( flip )
    {
        eQ_NC(1,1) *= -1;
        eQ_NC(1,3) *= -1;
        eQ_NC(2,3) *= -1;

        factor = -1;
    }

    Eigen::MatrixXf eCoor(4, nPixels);

    int idx = 0;
    float d;

    for ( int i = 0; i < rows; ++i )
    {
        pDisp = disp.ptr<float>(i);

        for ( int j = 0; j < cols; ++j )
        {
            d = pDisp[j];

            if ( d > 0 )
            {
                eCoor(0, idx) = j;
                eCoor(1, idx) = i;
                eCoor(2, idx) = d;
                eCoor(3, idx) = 1.0;
            }
            else
            {
                eCoor(0, idx) =  0.0;
                eCoor(1, idx) =  0.0;
                eCoor(2, idx) = -1.0;
                eCoor(3, idx) =  1.0;
            }

            idx++;
        }
    }

    Eigen::MatrixXf eCoorWorld(4, nPixels);

    eCoorWorld = eQ_NC * eCoor;

    float dOverB;
    int r, c;

    for ( int i = 0; i < nPixels; ++i )
    {
        if ( eCoorWorld(2, i) * factor > 0 )
        {
            dOverB = eCoorWorld(3, i);

            if ( std::fabs( eCoorWorld(2, i) / dOverB ) < farLimit )
            {
                vertexCoor.push_back({
                    eCoorWorld(0, i) / dOverB,
                    eCoorWorld(1, i) / dOverB,
                    eCoorWorld(2, i) / dOverB });

                r = i / cols;
                c = i % cols;

                pColor = colorRGB.ptr<unsigned char>(r) + c*3;

                vertexColor.push_back({
                    pColor[0], pColor[1], pColor[2]
                });
            }
        }
    }

//    // Debug.
//    std::cout << "Size of coordinates = " << vertexCoor.size() << "." << std::endl;
//    std::cout << "Size of colors = " << vertexColor.size() << "." << std::endl;

    // Create a buffer.
    std::filebuf binBuffer;
    if ( binary )
    {
        binBuffer.open( fn, std::ios::out | std::ios::binary );
    }
    else
    {
        binBuffer.open( fn, std::ios::out );
    }

    // Create the ostream.
    std::ostream ofs(&binBuffer);
    if ( ofs.fail() )
    {
        throw std::runtime_error("Fail to open " + fn + " for output.");
    }

    // Create the PlyFile object.
    tinyply::PlyFile plyFile;

    plyFile.add_properties_to_element("vertex", { "x", "y", "z" },
            tinyply::Type::FLOAT32,
            vertexCoor.size(),
            reinterpret_cast<uint8_t*>(vertexCoor.data()),
            tinyply::Type::INVALID, 0);

    plyFile.add_properties_to_element("vertex", { "red", "green", "blue" },
            tinyply::Type::UINT8,
            vertexColor.size(),
            reinterpret_cast<uint8_t*>(vertexColor.data()),
            tinyply::Type::INVALID, 0);

    plyFile.get_comments().emplace_back("generated by tinyply 2.2");

    plyFile.write( ofs, binary );
}


/*  $Id: conecutter.cpp 681 2011-03-23 21:17:32Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "conecutter.h"
#include "compositecutter.h" // for offsetCutter()
#include "numeric.h"

namespace ocl
{

ConeCutter::ConeCutter() {
    assert(0);
}

ConeCutter::ConeCutter(double d, double a, double l) {
    diameter = d;
    radius = d/2.0;
    angle = a;
    length = radius/tan(angle) + l;
    center_height = radius/tan(angle);
    xy_normal_length = radius;
    normal_length = 0.0;
}

double ConeCutter::height(double r) const {
    assert( tan(angle) > 0.0 ); // guard against division by zero
    return r/tan(angle);
}

double ConeCutter::width(double h) const {
    // grows from zero up to radius
    // above that (cutter shaft) return radius
    return (h<center_height) ? h*tan(angle) : radius ;
}

// ?? Ball-Cone-Bull ??
MillingCutter* ConeCutter::offsetCutter(double d) const {
    return new BallConeCutter(2*d,  diameter+2*d, angle) ;
}

// because this checks for contact with both the tip and the circular edge it is hard to move to the base-class
// we either hit the tip, when the slope of the plane is smaller than angle
// or when the slope is steep, the circular edge between the cone and the cylindrical shaft
bool ConeCutter::facetDrop(CLPoint &cl, const Triangle &t) const {
    bool result = false;
    Point normal = t.upNormal(); // facet surface normal
    if ( isZero_tol( normal.z ) )  // vertical surface
        return false;  //can't drop against vertical surface

    if ( (isZero_tol(normal.x)) && (isZero_tol(normal.y)) ) {  // horizontal plane special case
        CCPoint cc_tmp( cl.x, cl.y, t.p[0].z, FACET_TIP );  // so any vertex is at the correct height
        return cl.liftZ_if_inFacet(cc_tmp.z, cc_tmp, t);
    } else {
        // define plane containing facet
        // a*x + b*y + c*z + d = 0, so
        // d = -a*x - b*y - c*z, where  (a,b,c) = surface normal
        double a = normal.x;
        double b = normal.y;
        double c = normal.z;
        double d = - normal.dot(t.p[0]);
        normal.xyNormalize(); // make xy length of normal == 1.0
        // cylindrical contact point case
        // find the xy-coordinates of the cc-point
        CCPoint cyl_cc_tmp =  cl - radius*normal;
        cyl_cc_tmp.z = (1.0/c)*(-d-a*cyl_cc_tmp.x-b*cyl_cc_tmp.y);
        double cyl_cl_z = cyl_cc_tmp.z - length; // tip positioned here
        cyl_cc_tmp.type = FACET_CYL;

        // tip contact with facet
        CCPoint tip_cc_tmp(cl.x,cl.y,0.0);
        tip_cc_tmp.z = (1.0/c)*(-d-a*tip_cc_tmp.x-b*tip_cc_tmp.y);
        double tip_cl_z = tip_cc_tmp.z;
        tip_cc_tmp.type = FACET_TIP;

        result = result || cl.liftZ_if_inFacet( tip_cl_z, tip_cc_tmp, t);
        result = result || cl.liftZ_if_inFacet( cyl_cl_z, cyl_cc_tmp, t);
        return result;
    }
}

// cone sliced with vertical plane results in a hyperbola as the intersection curve
// find point where hyperbola and line slopes match
CC_CLZ_Pair ConeCutter::singleEdgeDropCanonical( const Point& u1, const Point& u2) const {
    double d = u1.y;
    double m = (u2.z-u1.z) / (u2.x-u1.x); // slope of edge
    // the outermost point on the cutter is at   xu = sqrt( R^2 - d^2 )
    double xu = sqrt( square(radius) - square(u1.y) );                  assert( xu <= radius );
    // max slope at xu is mu = (L/(R-R2)) * xu /(sqrt( xu^2 + d^2 ))
    double mu = (center_height/radius ) * xu / sqrt( square(xu) + square(d) ) ;
    bool hyperbola_case = (fabs(m) <= fabs(mu));
    // find contact point where slopes match, there are two cases:
    // 1) if abs(m) <= abs(mu)  we contact the curve at xp = sign(m) * sqrt( R^2 m^2 d^2 / (h^2 - R^2 m^2) )
    // 2) if abs(m) > abs(mu) there is contact with the circular edge at +/- xu
    double ccu;
    if ( hyperbola_case ) {
        ccu = sign(m) * sqrt( square(radius)*square(m)*square(d) / (square(length) -square(radius)*square(m) ) );
    } else {
        ccu = sign(m)*xu;
    }
    Point cc_tmp( ccu, d, 0.0); // cc-point in the XY plane
    cc_tmp.z_projectOntoEdge(u1,u2);
    double cl_z;
    if ( hyperbola_case ) {  // 1) zc = zp - Lc + (R - sqrt(xp^2 + d^2)) / tan(beta2)
        cl_z = cc_tmp.z - center_height + (radius-sqrt(square(ccu) + square(d)))/ tan(angle);
    } else {  // 2) zc = zp - Lc
        cl_z = cc_tmp.z - center_height; // case where we hit the edge of the cone
    }
    return CC_CLZ_Pair( ccu , cl_z);
}

bool ConeCutter::facetPush(const Fiber& fib, Interval& i,  const Triangle& t) const {
    // push two objects: tip, and base-circle
    bool result = false;
    if ( generalFacetPush( 0, 0, 0, fib, i, t) ) // TIP
        result = true;
    if ( generalFacetPush( 0, this->center_height, this->xy_normal_length , fib, i ,t) ) // BASE
        result = true;

    return result;
}

// cone is pushed along Fiber f into contact with edge p1-p2
bool ConeCutter::generalEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    bool result = false;

    if ( isZero_tol(p2.z-p1.z) ) // guard agains horizontal edge
        return result;
    assert( (p2.z-p1.z) != 0.0 );
    // idea: as the ITO-cone slides along the edge it will pierce a z-plane at the height of the fiber
    // the shaped of the pierced area is either a circle if the edge is steep
    // or a 'half-circle' + cone shape if the edge is shallow (ice-cream cone...)
    // we can now intersect this 2D shape with the fiber and get the CL-points.
    // how to get the CC-point? (point on edge closest to z-axis of cutter? closest to CL?)

    // this is where the ITO cone pierces the plane
    // edge-line: p1+t*(p2-p1) = zheight
    // => t = (zheight - p1)/ (p2-p1)
    double t_tip = (f.p1.z - p1.z) / (p2.z-p1.z);
    if (t_tip < 0.0 )
        t_tip = 0.0;
    Point p_tip = p1 + t_tip*(p2-p1);
    assert( isZero_tol( abs(p_tip.z-f.p1.z) ) ); // p_tip should be in plane of fiber

    // this is where the ITO cone base exits the plane
    double t_base = (f.p1.z+center_height - p1.z) / (p2.z-p1.z);
    Point p_base = p1 + t_base*(p2-p1);
    p_base.z = f.p1.z; // project to plane of fiber
    //std::cout << "(t0, t1) (" << t0 << " , " << t1 << ") \n";
    double L = (p_base-p_tip).xyNorm();

    if ( L <= radius ) { // this is where the ITO-slice is a circle
        // find intersection points, if any, between the fiber and the circle
        // fiber is f.p1 - f.p2
        // circle is centered at p_base and radius
        double d = p_base.xyDistanceToLine(f.p1, f.p2);
        if ( d <= radius ) {
            // we know there is an intersection point.
            // http://mathworld.wolfram.com/Circle-LineIntersection.html

            // subtract circle center, math is for circle centered at (0,0)
            double dx = f.p2.x - f.p1.x;
            double dy = f.p2.y - f.p1.y;
            double dr = sqrt( square(dx) + square(dy) );
            double det = (f.p1.x-p_base.x) * (f.p2.y-p_base.y) - (f.p2.x-p_base.x) * (f.p1.y-p_base.y);

            // intersection given by:
            //  x = det*dy +/- sign(dy) * dx * sqrt( r^2 dr^2 - det^2 )   / dr^2
            //  y = -det*dx +/- abs(dy)  * sqrt( r^2 dr^2 - det^2 )   / dr^2

            double discr = square(radius) * square(dr) - square(det);
            assert( discr > 0.0 ); // this means we have an intersection
            if ( discr == 0.0 ) { // tangent case
                double x_tang =  ( det*dy  )/ square(dr);
                double y_tang = -( det*dx  )/ square(dr);
                Point p_tang(x_tang+p_base.x, y_tang+p_base.y); // translate back from (0,0) system!
                double t_tang = f.tval( p_tang );
                if ( circle_CC( t_tang, p1, p2, f, i) )
                    result = true;
            } else {
                // two intersection points
                double x_pos = (  det*dy + sign(dy)* dx * sqrt( discr ) ) / square(dr);
                double y_pos = ( -det*dx + abs(dy)  * sqrt( discr ) ) / square(dr);
                Point p_pos(x_pos+p_base.x, y_pos+p_base.y);
                double t_pos = f.tval( p_pos );
                // the same with "-" sign:
                double x_neg = (  det*dy - sign(dy) * dx * sqrt( discr ) ) / square(dr);
                double y_neg = ( -det*dx - abs(dy)  * sqrt( discr ) ) / square(dr);
                Point p_neg(x_neg+p_base.x, y_neg+p_base.y);
                double t_neg = f.tval( p_neg );
                if ( circle_CC( t_pos, p1, p2, f, i) )
                    result = true;
                if ( circle_CC( t_neg, p1, p2, f, i) )
                    result = true;
            }
        }
        return result;
    } else {
        // ITO-slice is cone + half-circle
        // lines from p_tip to tangent points
        assert( L > radius );
        // http://mathworld.wolfram.com/CircleTangentLine.html
        // circle centered at x0, y0, radius a
        // tangent through (0,0)
        // t = +/- acos(  -a*x0 +/- y0*sqrt(x0^2+y0^2-a^2) / (x0^2+y0^2) )
        // translate so p_mid is at (0,0)
        //Point c = p_base - p_mid;
        //double cos1 = (-radius*c.x + c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );
        //double cos2 = (-radius*c.x - c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );

        return result;
    }
}

// t is a position along the fiber
// p1-p2 is the edge
// Interval& i is updated
bool ConeCutter::circle_CC( double t, const Point& p1, const Point& p2, const Fiber& f, Interval& i) const {
    // cone base circle is center_height above fiber
    double t_cc = (f.p1.z+center_height - p1.z) / (p2.z-p1.z);
    CCPoint cc_tmp = p1 + t_cc*(p2-p1); // cc-point on the edge
    cc_tmp.type = EDGE_CONE;
    return i.update_ifCCinEdgeAndTrue( t, cc_tmp, p1, p2, (true) );
}

std::string ConeCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, ConeCutter c) {
  stream << "ConeCutter (d=" << c.diameter << ", angle=" << c.angle << ", L=" << c.length << ")";
  return stream;
}

} // end namespace
// end file conecutter.cpp


#include "kernel_poly6.hpp"

#include "kernel_test.hpp"

#include <boost/test/unit_test.hpp>

using namespace GooBalls;
using namespace d2;
using namespace Physics;

namespace utf = boost::unit_test;

using KKernel = Poly6;

BOOST_AUTO_TEST_CASE(poly6_zero_border_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testZeroBorder1d(k);
}

BOOST_AUTO_TEST_CASE(poly6_normalization_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testNormalization1d(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_zero_border_gradient_1d, *utf::tolerance(0.01)){
    KKernel k;
    testZeroBorderGradient1d(k);
}

BOOST_AUTO_TEST_CASE(poly6_grad_finite_diff_1d, *utf::tolerance(0.001)){
    KKernel k;
    testGradientFiniteDifference1d(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_lap_grad_finite_diff_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testLaplacianFromGradientFiniteDifferences1d(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_lap_finite_diff_1d, *utf::tolerance(0.00001)){
    KKernel k;
    testLaplacianFiniteDifference1d(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_zero_border, *utf::tolerance(0.0001)){
    KKernel k;
    testZeroBorder(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_zero_border_gradient, *utf::tolerance(0.01)){
    KKernel k;
    testZeroBorderGradient(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_normalization, *utf::tolerance(0.0001)){
    KKernel k;
    testNormalization(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_monotonicity, *utf::tolerance(0.0001)){
    KKernel k;
    testMonotonicity(k, 10);
}

BOOST_AUTO_TEST_CASE(poly6_radial_symmetric){
    KKernel k;
    testRadialSymmetry(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_grad_finite_diff, *utf::tolerance(0.001)){
    KKernel k;
    testGradientFiniteDifference(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_lap_grad_finite_diff, *utf::tolerance(0.005)){
    KKernel k;
    testLaplacianFromGradientFiniteDifferences(k, 100);
}

BOOST_AUTO_TEST_CASE(poly6_lap_finite_diff, *utf::tolerance(0.001)){
    KKernel k;
    testLaplacianFiniteDifference(k, 100);
}


#include <efanna.hpp>
#include <iostream>
#include <fstream>
#include <ctime>
#include <malloc.h>
#include <boost/timer/timer.hpp>

using namespace efanna;
using namespace std;
void load_data(char* filename, float*& data, size_t& num,int& dim){// load data with sift10K pattern
  ifstream in(filename, ios::binary);
  if(!in.is_open()){cout<<"open file error"<<endl;exit(-1);}
  in.read((char*)&dim,4);
  cout<<"data dimension: "<<dim<<endl;
  in.seekg(0,ios::end);
  ios::pos_type ss = in.tellg();
  size_t fsize = (size_t)ss;
  num = fsize / (dim+1) / 4;
  int cols = (dim + 7)/8*8;
  data = (float*)memalign(KGRAPH_MATRIX_ALIGN, num * cols * sizeof(float));
if(dim!=cols)cout<<"data align to dimension "<<cols<<" for avx2 inst"<<endl;

  in.seekg(0,ios::beg);
  for(size_t i = 0; i < num; i++){
    in.seekg(4,ios::cur);
    in.read((char*)(data+i*cols),dim*4);
  }
  in.close();
}
int main(int argc, char** argv){
  if(argc!=10){cout<< argv[0] << " data_file save_graph_file trees level epoch L K kNN S" <<endl; exit(-1);}

  float* data_load = NULL;
  //float* query_load = NULL;
  size_t points_num;
  int dim;
  load_data(argv[1], data_load, points_num,dim);
  //size_t q_num;
  //int qdim;
  //load_data(argv[3], query_load, q_num,qdim);
  Matrix<float> dataset(points_num,dim,data_load);
  //Matrix<float> query(q_num,qdim,query_load);

  unsigned int trees = atoi(argv[3]);
  int mlevel = atoi(argv[4]);
  unsigned int epochs = atoi(argv[5]);
  int L = atoi(argv[6]);
  int checkK = atoi(argv[7]);
  int kNN = atoi(argv[8]);
  int S = atoi(argv[9]);

  //srand(time(NULL));
  FIndex<float> index(dataset, new L2DistanceAVX<float>(), efanna::KDTreeUbIndexParams(true, trees ,mlevel ,epochs,checkK,L, kNN, trees, S));
boost::timer::auto_cpu_timer timer;

  index.buildIndex();
cout<<timer.elapsed().wall / 1e9<<endl;
  index.saveGraph(argv[2]);
  return 0;
}


/**
 * SemiCluster Algorithm, written by zhigang wang.
 */

#include <string>
#include <vector>
#include <set>
#include <fstream>
#include <algorithm>
#include <graphlab.hpp>

#include <boost/range/iterator_range.hpp>

struct cluster {
	std::set<int> vertices; // set of vertices, no multiple elements
	double score; // score of current semi_cluster
	double innerScore; // inner score
	double boundaryScore; // boundary score

	cluster() {
		score = 1.0;
		innerScore = 0.0;
		boundaryScore = 0.0;
	}

	cluster(const cluster& c) {
		for (std::set<int>::const_iterator iter = c.vertices.begin();
				iter != c.vertices.end(); ++iter) {
			vertices.insert(*iter);
		}
		score = c.score;
		innerScore = c.innerScore;
		boundaryScore = c.boundaryScore;
	}

	~cluster() {
		vertices.clear();
		std::set<int>().swap(vertices);
	}

	void addVertex(const int sid,
			const std::map<int, double>& edges, const double scoreFactor) {
		if (vertices.insert(sid).second) {
			if (size() == 1) {
				for (std::map<int, double>::const_iterator iter = edges.begin();
						iter != edges.end(); ++iter) {
					boundaryScore += iter->second;
				}
				score = 0.0;
			} else {
				for (std::map<int, double>::const_iterator iter = edges.begin();
						iter != edges.end(); ++iter) {
					if (vertices.find(iter->first) != vertices.end()) {
						innerScore += iter->second;
						boundaryScore -= iter->second;
					} else {
						boundaryScore += iter->second;
					}
				}
				score = (innerScore-scoreFactor*boundaryScore)/(size()*(size()-1)/2);
			}
		} // insert successfully, i.e. new vertex id
	}

	int size() {
		return vertices.size();
	}

	void save(graphlab::oarchive& oarc) const {
	    oarc << vertices << score << innerScore << boundaryScore;
	}

	void load(graphlab::iarchive& iarc) {
	    iarc >> vertices >> score >> innerScore >> boundaryScore;
	}

	// override the < operator, used for less order
	bool operator<(const cluster& other) const {
		return (score<other.score);
	}

	// override the > operator, used for greater order
	bool operator>(const cluster& other) const {
		return (score>other.score);
	}
};

struct vertex_data {
	std::vector<cluster> cluster_set;

	vertex_data() {
	}

	/** only maintain the top-maxClusters clusters based on scores */
	void update(int maxClusters) {
		std::sort(cluster_set.begin(), cluster_set.end(), std::greater<cluster>()); // sort, greater
		int delNum = cluster_set.size() - maxClusters;
		if (delNum > 0) {
			for (int i = 0; i < delNum; ++i) {
				cluster_set.pop_back(); // delete the cluster with smaller scores
			}
		}
	}

	/** return the string of vertex data */
	std::string toString() {
		std::stringstream strm;
		for (std::vector<cluster>::const_iterator cIter = cluster_set.begin();
				cIter != cluster_set.end(); ++cIter) {
			cluster c = *cIter;
			strm << "  (";
			for (std::set<int>::const_iterator vIter = c.vertices.begin();
					vIter != c.vertices.end(); ++vIter) {
				strm << *vIter << ",";
			}
			strm << c.score << ")";
		}
		return strm.str();
	}

	void save(graphlab::oarchive& oarc) const {
		oarc << cluster_set;
	}
	void load(graphlab::iarchive& iarc) {
		iarc >> cluster_set;
	}
};

struct gather_data {
	std::map<int, double> edges;
	std::vector<vertex_data> cluster_sets;

	gather_data() {
	}

	~gather_data() {
		edges.clear();
		std::map<int, double>().swap(edges);
		cluster_sets.clear();
		std::vector<vertex_data>().swap(cluster_sets);
	}

	gather_data& operator+=(const gather_data& other) {
		for (std::map<int, double>::const_iterator iter = other.edges.begin();
				iter != other.edges.end(); ++iter) {
			edges[iter->first] = iter->second;
		}

		for (std::vector<vertex_data>::const_iterator iter = other.cluster_sets.begin();
				iter != other.cluster_sets.end(); ++iter) {
			cluster_sets.push_back(*iter);
		}

    return *this;
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << edges << cluster_sets;
  }

  void load(graphlab::iarchive& iarc) {
    iarc >> edges >> cluster_sets;
  }
};

size_t ITERATIONS = 0;
int MaxClusters = 2;
int MaxCapacity = 2;
double ScoreFactor = 0.5;
double EdgeWeight = 0.01;

typedef vertex_data vertex_data_type;
typedef graphlab::empty edge_data_type;
typedef gather_data gather_data_type;

// The graph type is determined by the vertex and edge data types
typedef graphlab::distributed_graph<vertex_data_type, edge_data_type> graph_type;

void initialize_vertex(graph_type::vertex_type& v) {
	cluster c;
	std::map<int, double> edges;
	c.addVertex(v.id(), edges, ScoreFactor);
	v.data().cluster_set.push_back(c);
} // now, we don't have the edge info, just use the null edges

class semicluster :
	public graphlab::ivertex_program<graph_type, gather_data_type>,
	public graphlab::IS_POD_TYPE {

	public:
		edge_dir_type gather_edges(icontext_type& context,
				const vertex_type& vertex) const {
			return graphlab::ALL_EDGES;
		}

		gather_data_type gather(icontext_type& context,
				const vertex_type& vertex, edge_type& edge) const {
			// figure out which data to get from the edge.
			bool isEdgeSource = (vertex.id()==edge.source().id());
			gather_data_type result;
			if (isEdgeSource) {
				std::pair<int, double> p(edge.target().id(), EdgeWeight);
				result.edges.insert(p);
				result.cluster_sets.push_back(edge.target().data());
			} else {
				std::pair<int, double> p(edge.source().id(), EdgeWeight);
				result.edges.insert(p);
				result.cluster_sets.push_back(edge.source().data());
			}

			return result;
		}

		void apply(icontext_type& context, vertex_type& vertex,
				const gather_type& total) {
			bool changed = true;

			for (std::vector<vertex_data>::const_iterator iter = total.cluster_sets.begin();
					iter != total.cluster_sets.end(); ++iter) {
				vertex_data neighbor = *iter;
				int len = neighbor.cluster_set.size();
				for (int i = 0; i < len; ++i) {
					cluster c = neighbor.cluster_set[i];
					bool isContained = (c.vertices.find(vertex.id()) != c.vertices.end());
					if (!isContained && c.size()<MaxCapacity) {
						cluster newC(c);
						newC.addVertex(vertex.id(), total.edges, ScoreFactor);
						vertex.data().cluster_set.push_back(newC);
					} else if (isContained) {
						vertex.data().cluster_set.push_back(c);
					}
				}
			}
			vertex.data().update(MaxClusters);

			context.setUpdateFlag(changed);
			context.signal(vertex);
			//vertex.num_in_edges();
			//std::cout << vertex.id() << "\t" << vertex.data().toString() << "\n";
			//std::cout << vertex.in_edges() << std::endl; // interrupted, no implemented!
		}

		edge_dir_type scatter_edges(icontext_type& context,
				const vertex_type& vertex) const {
			//return graphlab::ALL_EDGES;
			return graphlab::NO_EDGES;
		}

		void scatter(icontext_type& context,
				const vertex_type& vertex, edge_type& edge) const {
			bool isEdgeSource = (vertex.id()==edge.source().id());
			context.signal(isEdgeSource ? edge.target() : edge.source());
		}
	};

struct semicluster_writer {
  std::string save_vertex(graph_type::vertex_type v) {
    std::stringstream strm;
    strm << v.id() << "\t" << v.data().toString() << "\n";
    return strm.str();
  }
  std::string save_edge (graph_type::edge_type e) { return ""; }
};

int main(int argc, char** argv) {
  std::cout << "SemiCluster\n";

  // Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  global_logger().set_log_level(LOG_INFO);

  // Parse command line options -----------------------------------------------
  graphlab::command_line_options clopts("SemiCluster algorithm.");
  std::string graph_dir;
  std::string saveprefix;
  std::string format = "adj";
  std::string execution_type = "synchronous";

  //disk
  size_t ver_block_size = 1;
  size_t ver_buf_block_num = 1;
  size_t edge_block_size = 1;
  size_t useVerDisk = 0;
  size_t useEdgeDisk = 0;

  clopts.attach_option("graph", graph_dir, "The graph file. Required ");
  clopts.add_positional("graph");
  clopts.attach_option("format", format,
                         "The graph file format");
  clopts.attach_option("saveprefix", saveprefix,
                       "If set, will save the resultant label propagation to a "
                       "sequence of files with prefix saveprefix");
  clopts.attach_option("iterations", ITERATIONS,
                         "If set, will force the use of the synchronous engine"
                         "overriding any engine option set by the --engine parameter. "
                         "Runs complete (non-dynamic) label propagation for a fixed "
                         "number of iterations. Also overrides the iterations "
                         "option in the engine");

  clopts.attach_option("ver_block_size", ver_block_size,
  		  "the number of vertices per vertex block");
  clopts.add_positional("ver_block_size");

  clopts.attach_option("ver_buf_block_num", ver_buf_block_num,
		  "the number of blocks in vertex buffer");
  clopts.add_positional("ver_buf_block_num");

  clopts.attach_option("edge_block_size", edge_block_size,
		  "the size of each edge block, i.e. the number of source/target vertices");
  clopts.add_positional("edge_block_size");

  clopts.attach_option("useVerDisk", useVerDisk,
  		  "useVerDisk or not? =0:false, =1:true");
  clopts.add_positional("useVerDisk");

  clopts.attach_option("useEdgeDisk", useEdgeDisk,
  		  "useEdgeDisk or not? =0:false, =1:true");
  clopts.add_positional("useEdgeDisk");

  //! All input parameters cannot be used before .parse() is invoked.
  if(!clopts.parse(argc, argv)) {
    dc.cout() << "Error in parsing command line arguments." << std::endl;
    return EXIT_FAILURE;
  }
  if (graph_dir == "") {
    dc.cout() << "Graph not specified. Cannot continue";
    return EXIT_FAILURE;
  }

  // Enable gather caching in the engine
  clopts.get_engine_args().set_option("use_cache", false);

  if (ITERATIONS) {
      // make sure this is the synchronous engine
      dc.cout() << "--iterations set. Forcing Synchronous engine, and running "
                << "for " << ITERATIONS << " iterations." << std::endl;
      clopts.get_engine_args().set_option("type", "synchronous");
      clopts.get_engine_args().set_option("max_iterations", ITERATIONS);
      clopts.get_engine_args().set_option("sched_allv", true);
  }

  clopts.setVerBlockSize(ver_block_size);
  clopts.setVerBufBlockNum(ver_buf_block_num);
  clopts.setEdgeBlockSize(edge_block_size);
  clopts.setUseVerDisk(useVerDisk);
  clopts.setUseEdgeDisk(useEdgeDisk);

  // Build the graph ----------------------------------------------------------
  graph_type graph(dc, clopts);
  dc.cout() << "Loading graph in format: "<< format << std::endl;
  graphlab::timer load_timer;
  graph.load_format(graph_dir, format);
  // must call finalize before querying the graph
  graph.finalize();
  graph.transform_vertices(initialize_vertex);
  dc.cout() << "Finished loading in " << load_timer.current_time() << std::endl;
  dc.cout() << "global #vertices: " << graph.num_vertices()
		    << "\nglobal #edges:" << graph.num_edges() << std::endl;

  // Run the engine
  graphlab::omni_engine<semicluster> engine(dc, graph, execution_type, clopts);
  engine.signal_all();
  engine.start();
  const float runtime = engine.elapsed_seconds();
  dc.cout() << "Finished running engine in " << runtime << " seconds." << std::endl;

  if (saveprefix != "") {
    graph.save(saveprefix, semicluster_writer(),
       false,  // do not gzip
       true,   // save vertices
       false,  // do not save edges
       1);     // #_of_files per machine
  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
}