#include <algorithm>
#include <boost/bind.hpp>
#include <chrono>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <gazebo/gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <iostream>
#include <vector>

#define L 0.04 // distance between body center and wheel center
#define r 0.01905 // wheel radius

#define pi30 0.1047197551196597705355242034774843062905347323976457118988037109375 // long double thirty = 30; long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne) / thirty);
#define pi12 1.5707963267948965579989817342720925807952880859375 // long double two = 2; long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne) / two);
#define pi 3.141592653589793115997963468544185161590576171875 // long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne));
#define pi2 6.28318530717958623199592693708837032318115234375 // long double two = 2; long double mOne = -1; printf("%1.70Lf\n", (long double) two * acos(mOne));
#define sqrt3 1.732050807568877193176604123436845839023590087890625 // long double three = 3; printf("%1.70Lf\n", (long double) sqrt(three));
#define sqrt32 0.8660254037844385965883020617184229195117950439453125 // long double two = 2; long double three = 3; printf("%1.70Lf\n", (sqrt(three) / two));
#define Vmax 1.0 // set by experiments (m/s)
#define omegamax 50.0 // set by experiments (rad/s)
#define P 5.0 // PID
#define I 0.0005 // PID
#define stopDistance 0.001
#define stopAngle pi30

double Vx;
double Vy;
double Vxm;
double Vym;
double Vxw;
double Vyw;
double VxAbs;
double VyAbs;
double VxmTarget;
double VymTarget;
double VxwTarget;
double VywTarget;
double omegap;
double Vback;
double Vleft;
double Vright;
double VbackTarget;
double VleftTarget;
double VrightTarget;
double omegapL;
double omegapLVxm2;
double sqrtVym2;
double Vxm2;
double Vback3;
double Vleft3;
double Vright3;
double VxTarget;
double VyTarget;
double omegapTarget;
double x = 0;
double y = 0;
double xm = 0;
double ym = 0;
double xOffset = 0;
double yOffset = 0;
double xTarget = 0;
double yTarget = 0;
double xw = 0;
double yw = 0;
double xError;
double yError;
double xErrorI;
double yErrorI;
double theta = 0;
double thetaError;
double thetaErrorI;
double thetaOffset = 0;
double thetaTarget = 0;
double timeElapsed = 0;
double timeLast;
double timeNow = 0;
double backAngleElapsed;
double backAngleLast;
double backAngleNow = 0;
double leftAngleElapsed;
double leftAngleLast;
double leftAngleNow = 0;
double rightAngleElapsed;
double rightAngleLast;
double rightAngleNow = 0;
double phi;
double Vlow;
double Vhigh;
double scale;

class Point {

    public: double x;

    public: double y;

    private: static double w;

    public: void offset(double x, double y) {
        this->x += x;
        this->y += y;
    }

    public: void rotate(double theta) {
              w = (std::cos(theta) * this->y) + (std::sin(theta) * this->x);
        this->x = (std::cos(theta) * this->x) - (std::sin(theta) * this->y);
        this->y = w;
    }

    public: Point() {}

    public: Point(const Point& p) {
        x = p.x;
        y = p.y;
    }

    public: Point(double x, double y) {
        this->x = x;
        this->y = y;
    }

    public: Point(Point p, double xOffset, double yOffset) {
        x = p.x + xOffset;
        y = p.y + yOffset;
    }
};

std::vector<Point> points;
std::vector<double> angles;

enum Movements {
    MOVEMENT_DIRECT,
    MOVEMENT_DIRECT_M,
    MOVEMENT_DIRECT_W,
    MOVEMENT_NONE,
    MOVEMENT_ABSOLUTE_M,
    MOVEMENT_ABSOLUTE_W,
    MOVEMENT_BEZIER_W
};
Movements movement = MOVEMENT_NONE;

long long binomialCoefficient_c;
long long binomialCoefficient_i;

/**
 * source:
 * https://en.wikipedia.org/wiki/Binomial_coefficient
 * @param n the number that goes above
 * @param k the number that goes below
 * @return
 */
long long binomialCoefficient(long long n, long long k) {
    if ((k < 0) || (k > n))
        return 0;
    if ((k == 0) || (k == n))
        return 1;
    k = std::min(k, n - k);// take advantage of symmetry
    binomialCoefficient_c = 1;
    for (long binomialCoefficient_i = 0;
     binomialCoefficient_i < k; binomialCoefficient_i++)
        binomialCoefficient_c = binomialCoefficient_c
         * (n - binomialCoefficient_i) / (binomialCoefficient_i + 1);
    return binomialCoefficient_c;
}

double Bezier_t;
double Bezier_step;
long long Bezier_nT;
long long Bezier_nR;
long long Bezier_i;
double Bezier_B;
double Bezier_pX;
double Bezier_pY;
double Bezier_pTheta;

double Bezier_B_(double Bezier_n) {
    return ((double) binomialCoefficient(Bezier_n, Bezier_i))
         * std::pow((double) Bezier_t, (double) Bezier_i)
         * std::pow(1.0 - Bezier_t, (double) (Bezier_n - Bezier_i));
}

void Bezier() {
    Bezier_pX = 0;
    Bezier_pY = 0;
    for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
        Bezier_B = Bezier_B_(Bezier_nT);
        Bezier_pX += points[Bezier_i].x * Bezier_B;
        Bezier_pY += points[Bezier_i].y * Bezier_B;
    }
    Bezier_pTheta = 0;
    for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
        Bezier_pTheta += angles[Bezier_i] * Bezier_B_(Bezier_nR);
    }
}

/**
 * Resets the error values used by the PID controller.
 *
 * Should be used after a movement command if another one is fired
 * before or immediately after the previous one
 * and a continuous movement is not desired.
 */
void resetErrors() {
    xErrorI = 0;
    yErrorI = 0;
    thetaErrorI = 0;
}

/**
 * Calculates the mobile platform's velocities
 * relative to the mobile platform's frame
 * from the wheels velocities.
 */
void forwardKinematicsMobile() {
    Vleft3 = Vleft / 3.0;
    Vback3 = Vback / 3.0;
    Vright3 = Vright / 3.0;
    Vxm = (2 * Vback3) - Vleft3 - Vright3;
    Vym = (sqrt3 * Vright3) - (sqrt3 * Vleft3);
    omegap = (Vleft3 + Vback3 + Vright3) / L;
}

/**
 * Calculates the mobile platform's velocities
 * relative to the world's frame
 * from the wheels velocities.
 *
 * Also runs forwardKinematicsMobile().
 */
void forwardKinematicsWorld() {
    forwardKinematicsMobile();
    Vxw = (std::cos(theta) * Vxm) - (std::sin(theta) * Vym);
    Vyw = (std::cos(theta) * Vym) + (std::sin(theta) * Vxm);
}

/**
 * Generates a pseudo random number between 0 and maximumValue, inclusive.
 * @param maximumValue
 * @return
 */
double getRandom(double maximumValue) {
    return (((double) std::rand()) / ((double) RAND_MAX)) * maximumValue;
}

/**
 * Calculates the wheels velocities
 * from the mobile platform's velocities
 * relative to the mobile platform's frame.
 */
void inverseKinematicsMobile() {
    omegapL = omegap * L;
    sqrtVym2 = sqrt32 * Vym;
    Vxm2 = Vxm / 2.0;
    omegapLVxm2 = omegapL - Vxm2;
    Vleft  = omegapLVxm2 - sqrtVym2;
    Vback  = omegapL + Vxm;
    Vright = omegapLVxm2 + sqrtVym2;
}

/**
 * Calculates the wheels velocities
 * from the mobile platform's velocities
 * relative to the mobile platform's frame.
 *
 * Also runs inverseKinematicsMobile().
 */
void inverseKinematicsWorld() {
    Vxm = (std::cos(theta) * Vxw) + (std::sin(theta) * Vyw);
    Vym = (std::cos(theta) * Vyw) - (std::sin(theta) * Vxw);
    inverseKinematicsMobile();
}

bool isStopPose() {
    return (std::abs(xError) < stopDistance)
     && (std::abs(yError) < stopDistance)
     && (std::abs(thetaError) < stopAngle);
}

double normalizeRadian(double radian) {
    radian = fmod(radian, pi2);
    if (radian < 0) radian += pi2;
    return radian;
}

namespace gazebo {

    class OmniPlatformPlugin : public ModelPlugin {

        private: physics::JointPtr backJoint;

        private: physics::JointPtr leftJoint;

        // Pointer to the model
        private: physics::ModelPtr model;

        private: physics::JointPtr rightJoint;

        // Pointer to the update event connection
        private: event::ConnectionPtr updateConnection;

        private: physics::WorldPtr world;

        /**
         * Movement for desired pose in mobile platform's frame.
         *
         * Translates the desired pose in the mobile platform's frame
         * to the world's frame and moves according to the world's frame.
         * Keep in mind that how the mobile platform reaches the target theta
         * will affect the platform's position in it's own frame,
         * and that position can't be predicted. Also remember
         * that you can change the value of the platform's pose in either frame.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteM(double x, double y, double theta) {
            // distance to be traversed in the mobile platform's frame
            xTarget = x - xm;
            yTarget = y - ym;
            // distance rotated to the direction
            // to be traversed in the world's frame
            x = (std::cos(theta) * xTarget) - (std::sin(theta) * yTarget);
            y = (std::cos(theta) * yTarget) + (std::sin(theta) * xTarget);
            // rotated distance added to current world's frame position
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Random movement for desired pose in mobile platform's frame
         *
         * At the end of the movement, the mobile platform's x, y and theta
         * will be the same as the parameters. However, the position
         * in the world frame will depend on how theta will vary
         * from it's current value to the target value, and can't be predicted.
         * It's preferred to use fireMovementPoseM(),
         * unless you know what you're doing.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteMRaw(double x, double y, double theta) {
            xTarget = x;
            yTarget = y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_M;
            updateIndicator();
        }

        /**
         * Movement for desired pose in world frame.
         *
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteW(double x, double y, double theta) {
            xTarget = x;
            yTarget = y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Incremental movement based on two Bézier curves
         * relative to the platform's frame.
         *
         * The curve may be translated such that the first control point
         * and angle coincides with the current platform's position and angle.
         *
         * The Bézier function's t variable, by default, iterates
         * between 0 and 1 to generate the curve. t's step value
         * is defined by the user, and influences the time the platform
         * will take to execute the movement, which is equal to the step value
         * divided by the iteration time. If the movement time is too short
         * (which depends on the movement length and complexity), the platform
         * will make it's best effort to execute the movement. Therefore,
         * the longer the movement time (the shorter the step value), the closer
         * the platform will be to the desired movement.
         *
         * The platform's angle is also controlled by a Bézier curve. However,
         * the algorithm has been modified to be one dimensional, which means
         * its input are control values instead of control points.
         * If a constant angle is desired, input a vector with a single value
         * equal to the angle.
         *
         * @param step Bézier funcion's t variable's increment value.
         * @param points control points for the translation movement
         * @param angles control angles for the rotation movement
         * @param offsetT whether to offset the control points to the starting platform's position
         * @param offsetR whether to offset the control angles to the starting platform's angle
         */
        public: void fireMovementBezierM(std::vector<Point> * points,
                std::vector<double> * angles, double step,
                bool offsetT, bool offsetR) {
            if (points->empty() || angles->empty()) return;
            ::points.clear();
            ::angles.clear();
            Bezier_nT = points->size() - 1;
            Bezier_nR = angles->size() - 1;
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points.push_back(Point(points->at(Bezier_i)));
                ::points[Bezier_i].offset(-xw, -yw);
                ::points[Bezier_i].rotate(theta);
                ::points[Bezier_i].offset(xw, yw);
            }
            if (offsetT) {
                xOffset = xw - ::points[0].x;
                yOffset = yw - ::points[0].y;
            } else {
                xOffset = 0;
                yOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points[Bezier_i].offset(xOffset, yOffset);
            }
            if (offsetR) {
                thetaOffset = pi - normalizeRadian((*angles)[0] + pi - theta);
            } else {
                thetaOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
                ::angles.push_back(angles->at(Bezier_i) + thetaOffset);
            }
            Bezier_t = 0;
            Bezier_step = step;
            movement = MOVEMENT_BEZIER_W;
            updateIndicator();
        }

        /**
         * Incremental movement based on two Bézier curves relative to the world's frame.
         *
         * The curve may be translated such that the first control point
         * and angle coincides with the current platform's position and angle.
         *
         * The Bézier function's t variable, by default, iterates
         * between 0 and 1 to generate the curve. t's step value
         * is defined by the user, and influences the time the platform
         * will take to execute the movement, which is equal to the step value
         * divided by the iteration time. If the movement time is too short
         * (which depends on the movement length and complexity), the platform
         * will make it's best effort to execute the movement. Therefore,
         * the longer the movement time (the shorter the step value), the closer
         * the platform will be to the desired movement.
         *
         * The platform's angle is also controlled by a Bézier curve. However,
         * the algorithm has been modified to be one dimensional, which means
         * its input are control values instead of control points.
         * If a constant angle is desired, input a vector with a single value
         * equal to the angle.
         *
         * @param step Bézier funcion's t variable's increment value.
         * @param points control points for the translation movement
         * @param angles control angles for the rotation movement
         * @param offsetT whether to offset the control points to the starting platform's position
         * @param offsetR whether to offset the control angles to the starting platform's angle
         */
        public: void fireMovementBezierW(std::vector<Point> * points,
                std::vector<double> * angles, double step,
                bool offsetT, bool offsetR) {
            if (points->empty() || angles->empty()) return;
            ::points.clear();
            ::angles.clear();
            Bezier_nT = points->size() - 1;
            Bezier_nR = angles->size() - 1;
            if (offsetT) {
                xOffset = xw - (*points)[0].x;
                yOffset = yw - (*points)[0].y;
            } else {
                xOffset = 0;
                yOffset = 0;
            }
            if (offsetR) {
                thetaOffset = pi - normalizeRadian((*angles)[0] + pi - theta);
            } else {
                thetaOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points.push_back(Point(points->at(Bezier_i), xOffset, yOffset));
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
                ::angles.push_back(angles->at(Bezier_i) + thetaOffset);
            }
            Bezier_t = 0;
            Bezier_step = step;
            movement = MOVEMENT_BEZIER_W;
            updateIndicator();
        }

        /**
         * Moves the platform with fixed translation and rotation speeds
         * relative to the mobile platform's frame but translated
         * to the world's frame. This results in a world's frame movement
         * rotated according to the initial angle between frames.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectHybrid(double Vxm, double Vym, double omegap) {
            VxTarget = (std::cos(theta) * Vxm) - (std::sin(theta) * Vym);
            VyTarget = (std::cos(theta) * Vym) + (std::sin(theta) * Vxm);
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_W;
        }

        /**
         * Moves the platform with fixed translation and rotation speeds,
         * relative to the mobile platform's frame. Does not transform
         * the speeds from the mobile platform's frame to the world frame.
         * This means that random changes in the platform's angle
         * will not change it's movement, as the mobile platform's frame
         * rotates with the platform.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectMobile(double Vxm, double Vym, double omegap) {
            VxTarget = Vxm;
            VyTarget = Vym;
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_M;
        }

        /**
         * Sets the wheels' speeds, in m/s.
         *
         * To be executed by the communications module.
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectWheel(double Vleft, double Vback, double Vright) {
            VleftTarget  = Vleft ;
            VbackTarget  = Vback ;
            VrightTarget = Vright;
            movement = MOVEMENT_DIRECT;
        }

        /**
         * Moves the platform with fixed translation and rotation speeds,
         * relative to the world's frame.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectWorld(double Vxw, double Vyw, double omegap) {
            VxTarget     = Vxw   ;
            VyTarget     = Vyw   ;
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_W;
        }

        /**
         * Movement of a given distance and angle of rotation
         * in the mobile platform's frame
         *
         * Translates the movement in the mobile platform's frame
         * to the world's frame and moves according to the world's frame.
         * Keep in mind that how the mobile platform reaches the target theta
         * will affect the platform's position in it's own frame,
         * and that position can't be predicted. Also remember
         * that you can change the value of the platform's pose in either frame.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeM(double x, double y, double theta) {
            // distance to be traversed in the mobile platform's frame
            xTarget = x;
            yTarget = y;
            // distance rotated to the direction
            // to be traversed in the world's frame
            x = (std::cos(theta) * xTarget) - (std::sin(theta) * yTarget);
            y = (std::cos(theta) * yTarget) + (std::sin(theta) * xTarget);
            // rotated distance added to current world's frame position
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Random movement of a given distance and angle of rotation
         * in the mobile platform's frame
         *
         * At the end of the movement, the mobile platform will have traversed
         * in x and y and rotated by theta, relative to its frame.
         * However, the position in the world frame will depend
         * on how theta will vary from it's current value to the target value,
         * and can't be predicted. It's preferred to use fireMovementPoseM(),
         * unless you know what you're doing.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeMRaw(double x, double y, double theta) {
            xTarget = xm + x;
            yTarget = ym + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_M;
            updateIndicator();
        }

        /**
         * Movement of a given distance and angle of rotation
         * relative to the world frame.
         *
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeW(double x, double y, double theta) {
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {
            // Store the pointer to the model
            this->model = _parent;

            this-> leftJoint = model->GetJoint("left_joint");
            this-> backJoint = model->GetJoint("back_joint");
            this->rightJoint = model->GetJoint("right_joint");

            this->world = _parent->GetWorld();
            //this->indicator = this->world->GetModel("wood_cube_2_5cm");
            //this->indicatorPose.reset(new math::Pose());
            //this->indicator->SetGravityMode(false);
            //this->indicator->Fini();

            std::srand(std::time(0));

            // Listen to the update event. This event is broadcast every
            // simulation iteration.
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                    boost::bind(&OmniPlatformPlugin::OnUpdate, this, _1));
        }

        private: void odometry() {
            timeLast = timeNow;
            timeNow = this->world->GetSimTime().Double();
            timeElapsed = timeNow - timeLast;
            Vleft  = this-> leftJoint->GetVelocity(0) * r;
            Vback  = this-> backJoint->GetVelocity(0) * r;
            Vright = this->rightJoint->GetVelocity(0) * r;
            theta = normalizeRadian(model->GetRelativePose().rot.GetYaw()); // simulation theta
            forwardKinematicsWorld();
            xm += Vxm * timeElapsed;
            ym += Vym * timeElapsed;
            xw += Vxw * timeElapsed;
            //xw = model->GetRelativePose().pos.x; // simulation x
            yw += Vyw * timeElapsed;
            //yw = model->GetRelativePose().pos.y; // simulation y
        }

        // Called by the world update start event
        public: void OnUpdate(const common::UpdateInfo & /*_info*/) {
            odometry();
            switch (movement) {
                case MOVEMENT_ABSOLUTE_M:
                    xError = xTarget - xm;
                    yError = yTarget - ym;
                    // method parameter: theta plus shift to place thetaTarget at pi
                    thetaError = pi - normalizeRadian(theta + pi - thetaTarget);
                    if (isStopPose()) {
                        movement = MOVEMENT_NONE;
                        break;
                    }
                    xErrorI += xError;
                    yErrorI += yError;
                    thetaErrorI += thetaError;
                    Vxm = (xError * P) + (xErrorI * I);
                    Vym = (yError * P) + (yErrorI * I);
                    VxAbs = std::abs(Vxm);
                    VyAbs = std::abs(Vym);
                    if ((VxAbs > Vmax) || (VyAbs > Vmax)) {
                        scale = Vmax / ((VxAbs > VyAbs) ? VxAbs : VyAbs);
                        Vxm *= scale;
                        Vym *= scale;
                    }
                    omegap = (thetaError * P) + (thetaErrorI * I);
                    if (omegap > omegamax) {
                        omegap = omegamax;
                    }
                    inverseKinematicsMobile();
                    break;
                case MOVEMENT_BEZIER_W:
                    if (Bezier_t <= 1) {
                        Bezier();
                        xTarget = Bezier_pX;
                        yTarget = Bezier_pY;
                        thetaTarget = normalizeRadian(Bezier_pTheta);
                        Bezier_t += Bezier_step;
                        updateIndicator();
                    } else {
                        movement = MOVEMENT_NONE;
                    }
                    // break; // fall through intended
                case MOVEMENT_ABSOLUTE_W:
                    xError = xTarget - xw;
                    yError = yTarget - yw;
                    // method parameter: theta plus shift to place thetaTarget at pi
                    thetaError = pi - normalizeRadian(theta + pi - thetaTarget);
                    if (isStopPose() && (movement == MOVEMENT_ABSOLUTE_W)) {
                        movement = MOVEMENT_NONE;
                        break;
                    }
                    xErrorI += xError;
                    yErrorI += yError;
                    thetaErrorI += thetaError;
                    Vxw = (xError * P) + (xErrorI * I);
                    Vyw = (yError * P) + (yErrorI * I);
                    VxAbs = std::abs(Vxw);
                    VyAbs = std::abs(Vyw);
                    if ((VxAbs > Vmax) || (VyAbs > Vmax)) {
                        scale = Vmax / ((VxAbs > VyAbs) ? VxAbs : VyAbs);
                        Vxw *= scale;
                        Vyw *= scale;
                    }
                    omegap = (thetaError * P) + (thetaErrorI * I);
                    if (omegap > omegamax) {
                        omegap = omegamax;
                    }
                    inverseKinematicsWorld();
                    break;
                case MOVEMENT_DIRECT:
                    Vleft  = VleftTarget ;
                    Vback  = VbackTarget ;
                    Vright = VrightTarget;
                    break;
                case MOVEMENT_DIRECT_M:
                    Vxm    = VxTarget    ;
                    Vym    = VyTarget    ;
                    omegap = omegapTarget;
                    inverseKinematicsMobile();
                    break;
                case MOVEMENT_DIRECT_W:
                    Vxw    = VxTarget    ;
                    Vyw    = VyTarget    ;
                    omegap = omegapTarget;
                    inverseKinematicsWorld();
                    break;
                default:
                    xErrorI = 0;
                    yErrorI = 0;
                    thetaErrorI = 0;
                    Vleft = 0;
                    Vback = 0;
                    Vright = 0;
            }
             leftJoint->SetVelocity(0, Vleft / r);
             backJoint->SetVelocity(0, Vback / r);
            rightJoint->SetVelocity(0, Vright / r);
        }

        /**
         * Simulation purposes only.
         */
        private: void updateIndicator() {
//            indicatorPose->Set(xTarget, yTarget, 0.05, 0, 0, thetaTarget);
//            indicator->SetRelativePose(*indicatorPose);
        }
    };

    // Register this plugin with the simulator
    GZ_REGISTER_MODEL_PLUGIN(OmniPlatformPlugin)
}

// C++ bug: when things are declared as 'static' or 'const',
// they lose their references, and it's only possible to read from them
// to restore their references, they must be decladed again like this:
double Point::w;


#include "conex/supernodal_solver.h"
#include "conex/clique_ordering.h"

#include <iostream>
#include <map>

#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Permutation = Eigen::PermutationMatrix<-1>;
using T = TriangularMatrixOperations;
using std::vector;

namespace {

vector<int> Relabel(const vector<int>& x, const vector<int>& labels) {
  vector<int> y;
  for (auto& xi : x) {
    y.push_back(labels.at(xi));
  }
  return y;
}

int GetMax(const std::vector<Clique>& cliques) {
  int max = cliques.at(0).at(0);
  for (const auto& c : cliques) {
    for (const auto ci : c) {
      if (ci > max) {
        max = ci;
      }
    }
  }
  return max;
}

class TriangularMatrixColumnOperations {
 public:
  TriangularMatrixColumnOperations(T::Matrix* mat) : mat_(mat) {
    dense_size = mat_->supernode_size.at(0);
  }
  void NextColumn() {
    supernode_column_++;
    column++;
    dense_size--;
    if (supernode_column_ >= mat_->supernode_size.at(supernode_index_)) {
      supernode_index_++;
      supernode_column_ = 0;
      dense_size = mat_->supernode_size.at(supernode_index_);
    }
  }

  void Rescale(double scale) {
    mat_->supernodes.at(supernode_index_).col(supernode_column_).array() *=
        scale;
    mat_->separator.at(supernode_index_).row(supernode_column_).array() *=
        scale;
  }

  double Diagonal() {
    return mat_->supernodes.at(supernode_index_)(supernode_column_,
                                                 supernode_column_);
  }

  // Applies the operation b = b - L.col(i) * y (i)
  void SubtractWeightedColumn(VectorXd* b, double y) {
    b->segment(column, dense_size) -= y * mat_->supernodes.at(supernode_index_)
                                              .col(supernode_column_)
                                              .tail(dense_size);

    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      (*b)(row) -=
          y * mat_->separator.at(supernode_index_)(supernode_column_, i);
    }
  }

  std::vector<int> NonzeroRows() {
    std::vector<int> y;
    for (int i = 1; i < dense_size; i++) {
      y.push_back(column + i);
    }
    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      y.push_back(row);
    }
    return y;
  }

  // double DotProduct(const Eigen::MatrixXd& x) {
  //  double y = 0;
  //  mat_->supernodes.at(supernode_index_).col(supernode_column_).tail(dense_size).dot(
  //      x.segment(column, dense_size);
  //}

 private:
  int supernode_index_ = 0;
  int dense_size = 0;
  int column = 0;
  int supernode_column_ = 0;
  SparseTriangularMatrix* mat_;
};

template <typename T>
int LookupSuperNode(const T& o, int index, int start) {
  for (int j = static_cast<int>(o.snodes.size()) - 1; j >= 0; j--) {
    if (o.snodes.at(j).size() > 0) {
      if (index >= o.snodes.at(j).at(0)) {
        return j;
      }
    }
  }
  throw "Sparse matrix is malformed: invalid supernode partition.";
}

double Get(const SparseTriangularMatrix& o, int i, int j) {
  if (j > i) {
    return 0;
  }
  // Find the supernode that owns the column.
  int node = LookupSuperNode(o, j, 0);

  // Apply offsets.
  int offset_i = i - o.path.at(node).at(0);
  int offset_j = j - o.path.at(node).at(0);
  if ((offset_i < o.supernode_size.at(node)) &&
      (offset_j < o.supernode_size.at(node))) {
    return o.supernodes.at(node)(offset_i, offset_j);
  }
  for (size_t k = o.supernode_size.at(node); k < o.path.at(node).size(); k++) {
    if (i == o.path.at(node).at(k)) {
      return o.separator.at(node)(offset_j, k - o.supernode_size.at(node));
    }
  }
  return 0;
}

class LowerTriangularSuperNodal {
  // We partition matrix into
  //
  // SN1  R1
  // R1   SN2  R2
  // R1   R2   SN3
  //
  // Where R1 is sparse.
 public:
  LowerTriangularSuperNodal(SparseTriangularMatrix* mat)
      : supernodes_(mat->supernodes),
        supernode_size_(mat->supernode_size),
        path_(mat->path),
        separator_(mat->separator) {
    Init();
  }

  void Init() {
    // for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
    //   supernodes_.at(j).resize(supernode_size_.at(j), supernode_size_.at(j));
    //   separator_.at(j).resize(supernode_size_.at(j),  path_.at(j).size() -
    //   supernode_size_.at(j));
    // }
  }

  // The smallest super-node less than i.
  int LookupSuperNode(int index, int start) const {
    for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
      if (index >= path_.at(j).at(0)) {
        return j;
      }
    }
    throw "Sparse matrix is malformed: invalid supernode partition.";
  }

  double Get(int i, int j) const {
    if (j > i) {
      return 0;
    }
    // Find the supernode that owns the column.
    int node = LookupSuperNode(j, 0);

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      return supernodes_.at(node)(offset_i, offset_j);
    }
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        return separator_.at(node)(offset_j, k - supernode_size_.at(node));
      }
    }
    return 0;
  }

  void Increment(double val, int i, int j, int supernode_index = -1) {
    if (val == 0) {
      return;
    }
    assert(j <= i);

    int node = supernode_index;
    // Find the supernode that owns storage for column j.
    if (node == -1) {
      node = LookupSuperNode(j, 0);
    }

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);

    // i is also in the supernode.
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      supernodes_.at(node)(offset_i, offset_j) += val;
      return;
    }

    // i is in a separator.
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        separator_.at(node)(offset_j, k - supernode_size_.at(node)) += val;
        return;
      }
    }

    throw "Specified entry of sparse matrix is not accesible.";
  }

 public:
  using MapT = Eigen::Map<Eigen::MatrixXd>;
  std::vector<MapT>& supernodes_;
  std::vector<int>& supernode_size_;
  std::vector<Clique>& path_;
  std::vector<MapT>& separator_;
};

std::vector<int> ResidualSize(std::vector<Clique>& path) {
  std::vector<int> y;
  for (size_t j = 0; j < path.size() - 1; j++) {
    std::vector<int> temp;
    IntersectionOfSorted(path.at(j), path.at(j + 1), &temp);
    y.push_back(path.at(j).size() - temp.size());
  }
  y.push_back(path.back().size());
  return y;
}

}  // namespace

void IntersectionOfSorted(const std::vector<int>& v1,
                          const std::vector<int>& v2, std::vector<int>* v3) {
  v3->clear();
  std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(),
                        back_inserter(*v3));
}

SparseTriangularMatrix GetFillInPattern(
    int N, const std::vector<Clique>& cliques_input) {
  auto mat = MakeSparseTriangularMatrix(N, cliques_input);

  for (int j = static_cast<int>(mat.path.size()) - 1; j >= 0; j--) {
    // Initialize columns of super nodes.
    mat.supernodes.at(j).setConstant(1);
    mat.separator.at(j).setConstant(1);

    // Update other columns: the (seperator, seperator) components.
    int index = 0;
    auto s_s = mat.workspace_.seperator_diagonal.at(j);
    int n = mat.path.at(j).size();
    for (int i = mat.supernode_size.at(j); i < n; i++) {
      for (int k = i; k < n; k++) {
        *s_s.at(index++) += 1;
      }
    }
  }
  return mat;
}

std::vector<Clique> Permute(std::vector<Clique>& path,
                            std::vector<int>& permutation) {
  auto y = path;
  for (size_t i = 0; i < path.size(); i++) {
    for (size_t j = 0; j < path.at(i).size(); j++) {
      y.at(i).at(j) = permutation.at(path.at(i).at(j));
    }
  }
  return y;
}

void Sort(std::vector<Clique>* path) {
  for (size_t i = 0; i < path->size(); i++) {
    std::sort(path->at(i).begin(), path->at(i).end());
  }
}

// Want A B  C  D  E
//   Add A_i \cap A_k
//    to all A_j for i < j < k.
//
void RunningIntersectionClosure(std::vector<Clique>* path) {
  if (path->size() < 2) {
    return;
  }
  int n = path->size();
  for (int i = 0; i < n - 2; i++) {
    for (int j = n - 1; j > i + 1; j--) {
      std::vector<int> temp;
      IntersectionOfSorted(path->at(i), path->at(j), &temp);
      if (temp.size() == 0) {
        continue;
      }
      for (int k = j - 1; k > i; k--) {
        path->at(k) = UnionOfSorted(path->at(k), temp);
      }
    }
  }
}

Eigen::MatrixXd TriangularMatrixOperations::ToDense(
    const SparseTriangularMatrix& mat) {
  MatrixXd y(mat.N, mat.N);
  for (int i = 0; i < mat.N; i++) {
    for (int j = 0; j < mat.N; j++) {
      y(i, j) = Get(mat, i, j);
    }
  }
  return y;
}

void TriangularMatrixOperations::SetConstant(SparseTriangularMatrix* mat,
                                             double val) {
  for (auto& n : mat->supernodes) {
    n.array() = val;
  }
  for (auto& n : mat->separator) {
    n.array() = val;
  }
}

SparseTriangularMatrix MakeSparseTriangularMatrix(
    int N, const std::vector<Clique>& path_) {
  auto path = path_;
  Sort(&path);
  RunningIntersectionClosure(&path);
  auto supernode_size = ResidualSize(path);
  return SparseTriangularMatrix(N, path, supernode_size);
}

void T::CholeskyInPlace(SparseTriangularMatrix* C) {
  TriangularMatrixColumnOperations col(C);
  LowerTriangularSuperNodal mat(C);
  double sqrt_d = std::sqrt(col.Diagonal());
  col.Rescale(1.0 / sqrt_d);

  // i: a supernode
  for (int i = 0; i < C->N - 1; i++) {
    // Substract col(2:n) c(2:n)^T
    auto indices = col.NonzeroRows();
    for (size_t k = 0; k < indices.size(); k++) {
      double weight = -mat.Get(indices.at(k), i);
      // Decrement column k
      for (size_t j = k; j < indices.size(); j++) {
        mat.Increment(weight * mat.Get(indices.at(j), i), indices.at(j),
                      indices.at(k));
      }
    }
    col.NextColumn();
    double sqrt_d = std::sqrt(col.Diagonal());
    col.Rescale(1.0 / sqrt_d);
  }
}

// Apply L  inverse.
// 1
// 1  1
// 1  1  1

// L
// B in
VectorXd T::ApplyInverseOfTranspose(SparseTriangularMatrix* mat,
                                    const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  y(n - 1) = res(n - 1) / L.Get(n - 1, n - 1);
  for (int i = n - 2; i >= 0; i--) {
    for (int j = 0; j < i + 1; j++) {
      res(j) = res(j) - L.Get(i + 1, j) * y(i + 1);
    }
    y(i) = res(i) / L.Get(i, i);
  }
  return y;
}

VectorXd T::ApplyInverse(SparseTriangularMatrix* mat, const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  TriangularMatrixColumnOperations col(mat);
  y(0) = res(0) / col.Diagonal();
  for (int i = 1; i < n; i++) {
    // Iterate over non-zero entries of column.
    // for (int j = i; j < n; j++) {
    //   res(j) = res(j) - L.Get(j, i - 1) * y(i - 1);
    // }
    col.SubtractWeightedColumn(&res, y(i - 1));

    col.NextColumn();
    y(i) = res(i) / col.Diagonal();
  }
  return y;
}

std::vector<int> UnionOfSorted(const std::vector<int>& x1,
                               const std::vector<int>& x2) {
  std::vector<int> y;
  set_union(x1.begin(), x1.end(), x2.begin(), x2.end(), inserter(y, y.end()));
  return y;
}

MatrixData GetData(const vector<vector<int>>& cliques, int init) {
  vector<vector<int>> separators;
  vector<vector<int>> supernodes;
  MatrixData d;
  d.cliques = cliques;
  Sort(&d.cliques);
  auto& order = d.clique_order;

  PickCliqueOrder(d.cliques, init, &order, &supernodes, &separators);

  d.permutation.resize(GetMax(cliques) + 1);
  d.permutation_inverse.resize(GetMax(cliques) + 1);
  int i = 0;
  for (auto& e : order) {
    for (auto& sn_ii : supernodes.at(e)) {
      d.permutation_inverse.at(i) = sn_ii;
      d.permutation.at(sn_ii) = i;
      i++;
    }
  }

  auto& separators_ = d.separators;
  auto& supernodes_ = d.supernodes;

  supernodes_.resize(order.size());
  separators_.resize(order.size());

  i = 0;

  for (auto e : order) {
    supernodes_.at(i) = Relabel(supernodes.at(e), d.permutation);
    separators_.at(i) = Relabel(separators.at(e), d.permutation);
    i++;
  }

  Sort(&separators_);
  Sort(&supernodes_);

  int cnt = 0;
  auto& supernode_size = d.supernode_size;
  supernode_size.resize(order.size());
  for (auto& si : supernode_size) {
    si = supernodes_.at(cnt).size();
    cnt++;
  }
  d.N = std::accumulate(supernode_size.begin(), supernode_size.end(), 0);
  for (size_t i = 0; i < d.cliques.size(); i++) {
    d.cliques.at(i) = UnionOfSorted(supernodes_.at(i), separators_.at(i));
  }
  return d;
}

}  // namespace conex


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_CLUSTERING_HH
#define GRAPH_CLUSTERING_HH

#include "config.h"

#include <unordered_set>
#include <boost/mpl/if.hpp>

#ifdef HAVE_SPARSEHASH
#include SPARSEHASH_INCLUDE(dense_hash_set)
#endif

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

namespace graph_tool
{
using namespace boost;

#ifdef HAVE_SPARSEHASH
using google::dense_hash_set;
#else
using std::unordered_set;
#endif

// calculates the number of triangles to which v belongs
template <class Graph>
pair<int,int>
get_triangles(typename graph_traits<Graph>::vertex_descriptor v, const Graph &g)
{
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

#ifdef HAVE_SPARSEHASH
    typedef dense_hash_set<vertex_t, std::hash<vertex_t>> set_t;
#else
    typedef unordered_set<vertex_t> set_t;
#endif

    set_t neighbour_set;

#ifdef HAVE_SPARSEHASH
     neighbour_set.set_empty_key(numeric_limits<vertex_t>::max());
     neighbour_set.resize(out_degree(v, g));
#endif

    size_t triangles = 0;

    typename graph_traits<Graph>::adjacency_iterator n, n_end;
    for (tie(n, n_end) = adjacent_vertices(v, g); n != n_end; ++n)
    {
        if (*n == v) // no self-loops
            continue;
        neighbour_set.insert(*n);
    }

    for (tie(n, n_end) = adjacent_vertices(v, g); n != n_end; ++n)
    {
        typename graph_traits<Graph>::adjacency_iterator n2, n2_end;
        for (tie(n2, n2_end) = adjacent_vertices(*n, g); n2 != n2_end; ++n2)
        {
            if (*n2 == *n) // no self-loops
                continue;
            if (neighbour_set.find(*n2) != neighbour_set.end())
                ++triangles;
        }
    }

    size_t k = out_degree(v, g);
    return make_pair(triangles/2,(k*(k-1))/2);
}

// retrieves the global clustering coefficient
struct get_global_clustering
{
    template <class Graph>
    void operator()(const Graph& g, double& c, double& c_err) const
    {
        size_t triangles = 0, n = 0;
        pair<size_t, size_t> temp;

        int i, N = num_vertices(g);

        #pragma omp parallel for default(shared) private(i,temp) \
            schedule(runtime) if (N > 100) reduction(+:triangles, n)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            temp = get_triangles(v, g);
            triangles += temp.first;
            n += temp.second;
        }
        c = double(triangles) / n;

        // "jackknife" variance
        c_err = 0.0;
        double cerr = 0.0;

        #pragma omp parallel for default(shared) private(i,temp) \
            schedule(runtime) if (N > 100) reduction(+:cerr)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            temp = get_triangles(v, g);
            double cl = double(triangles - temp.first) / (n - temp.second);

            cerr += power(c - cl, 2);
        }
        c_err = sqrt(cerr);
    }
};

// sets the local clustering coefficient to a property
struct set_clustering_to_property
{
    template <class Graph, class ClustMap>
    void operator()(const Graph& g, ClustMap clust_map) const
    {
        typedef typename property_traits<ClustMap>::value_type c_type;
        typename get_undirected_graph<Graph>::type ug(g);
        int i, N = num_vertices(g);

        #pragma omp parallel for default(shared) private(i) schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            pair<size_t,size_t> triangles = get_triangles(v,ug); // get from ug
            double clustering = (triangles.second > 0) ?
                double(triangles.first)/triangles.second :
                0.0;

            clust_map[v] = c_type(clustering);
        }
    }

    template <class Graph>
    struct get_undirected_graph
    {
        typedef typename mpl::if_
           <std::is_convertible<typename graph_traits<Graph>::directed_category,
                                directed_tag>,
            const UndirectedAdaptor<Graph>,
            const Graph& >::type type;
    };
};

} //graph-tool namespace

#endif // GRAPH_CLUSTERING_HH


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIK_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIK_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/eps_related.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/real.hpp>

#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/tan.hpp>
#include <nt2/include/functions/atan.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/average.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (arithmetic_<A0>)(arithmetic_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return ellik(type(a0), type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (double_<A0>)(double_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::double_,tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      if (is_eqz(a1))  return type(a0);
      return boost::math::ellint_1(nt2::sqrt(a1), a0);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (float_<A0>)(float_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::float_,tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      if (is_eqz(a1))  return a0;
      type phi = nt2::abs(a0);
      type m = a1;
      type a = 1.0;
      type b = oneminus(m);
      if( is_eqz(b) )   return nt2::log(nt2::tan(nt2::average(Pio_2<type>(),phi)));
      b = nt2::sqrt(b);
      type c = nt2::sqrt(m);
      int d = 1;
      type t = nt2::tan(phi);
      int mod = (phi + Pio_2<type>())/Pi<type>();
      while( nt2::abs(c) > nt2::abs(a)*Eps<type>() )
      {
        type temp = b/a;
        phi += nt2::atan(t*temp) + mod*Pi<type>();
        mod = (phi + Pio_2<type>())/Pi<type>();
        t = t*oneplus(temp)/( oneminus(temp*t*t));
        c = average(a,-b);
        temp = nt2::sqrt(a*b);
        a = average(a,b);
        b = temp;
        d += d;
      }
      type temp = (atan(t) + mod * Pi<type>())/(d * a);
      if( is_ltz(a0) )  temp = -temp;
      return temp;
    }
  };
} }

#endif
// modified by jt the 26/12/2010


#include <iostream>
#include <fstream>
#include <algorithm>
#include <boost/tuple/tuple.hpp>

#include <CGAL/Cartesian.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/function_objects.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>

#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Polyhedron_3.h>

#include <CGAL/Timer.h>

#include <CGAL/Random.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/algorithm.h>

const std::size_t elements = 50000;
const int runs = 10;

template<typename Tree>
struct FilterP {
  const Tree* t;

  template<typename T>
  bool operator()(const T& tt) { return !t->do_intersect(tt); }
};

template<typename ForwardIterator, typename Tree>
std::size_t intersect(ForwardIterator b, ForwardIterator e, const Tree& tree, long& counter) {
      typedef
        typename Tree::AABB_traits::template Intersection_and_primitive_id<typename ForwardIterator::value_type>::Type
        Obj_type;

  std::vector<Obj_type> v;
  // bad educated guess
  v.reserve(elements);
  for(; b != e; ++b) {
    tree.all_intersections(*b, std::back_inserter(v));
    boost::optional<Obj_type> o = tree.any_intersection(*b);
    if(o)
      ++counter;
  }

  return v.size();
}

template<typename K>
boost::tuple<std::size_t, std::size_t, std::size_t, long> test(const char* name) {
  typedef typename K::FT FT;
  typedef typename K::Ray_3 Ray;
  typedef typename K::Line_3 Line;
  typedef typename K::Point_3 Point;
  typedef typename K::Segment_3 Segment;
  typedef CGAL::Polyhedron_3<K> Polyhedron;

  typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
  typedef CGAL::AABB_traits<K, Primitive> Traits;
  typedef CGAL::AABB_tree<Traits> Tree;

  std::ifstream ifs(name);

  Polyhedron polyhedron;
  ifs >> polyhedron;

  // Random seeded to 23, cube size equal to the magic number 2
  CGAL::Random r(23);
  CGAL::Random_points_in_cube_3<Point, CGAL::Creator_uniform_3<FT, Point> > g( 2., r);

  std::vector<Point> points;
  points.reserve(elements * 2);
  std::copy_n(g, elements * 2, std::back_inserter(points));

  // generate a bunch of happy random primitives
  std::vector<Line> lines;
  lines.reserve(elements);

  // forward
  for(std::size_t i = 0; i < points.size(); i += 2)
  {
    lines.push_back(Line(points[i], points[i + 1]));
  }

  std::vector<Ray> rays;
  rays.reserve(elements);

  // backwards
  for(std::size_t i = points.size(); i != 0; i -= 2)
  {
    rays.push_back(Ray(points[i - 1], points[i - 2]));
  }

  std::vector<Segment> segments;
  segments.reserve(elements);
  // from both sides
  for(std::size_t i = 0, j = points.size() - 1; i < j; ++i, --j)
  {
    segments.push_back(Segment(points[i], points[j]));
  }

  Tree tree(faces(polyhedron).first, faces(polyhedron).second, polyhedron);

  // filter all primitives that do not intersect

  FilterP<Tree> p = { &tree };

  lines.erase(std::remove_if(lines.begin(), lines.end(), p), lines.end());

  rays.erase(std::remove_if(rays.begin(), rays.end(), p), rays.end());

  segments.erase(std::remove_if(segments.begin(), segments.end(), p), segments.end());

  boost::tuple<std::size_t, std::size_t, std::size_t, long> tu;

    {
      CGAL::Timer t;
      t.start();

      for(int i = 0; i < runs; ++i)
      {
        long counter = 0L;
        tu = boost::make_tuple(intersect(lines.begin(), lines.end(), tree, counter),
                               intersect(rays.begin(), rays.end(), tree, counter),
                               intersect(segments.begin(), segments.end(), tree, counter),
                               // cant use counter here
                               0);
        boost::get<3>(tu) = counter;
      }
      std::cout << t.time();
    }

  return tu;
}

int main()
{
  const char* filename = "./data/finger.off";

  std::cout << "| Simple cartesian float kernel | ";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t1 = test<CGAL::Simple_cartesian<float> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Cartesian float kernel | ";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t2 = test<CGAL::Cartesian<float> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Simple cartesian double kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t3 = test<CGAL::Simple_cartesian<double> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Cartesian double kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t4 = test<CGAL::Cartesian<double> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Epic kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t5 = test<CGAL::Exact_predicates_inexact_constructions_kernel>(filename);
  std::cout << " | " << std::endl;

  std::size_t a, b, c;
  long d;

  boost::tie(a, b, c, d) = t5;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t4;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t3;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t2;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t1;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;
  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <ql/experimental/models/mcgaussian1dnonstandardswaptionengine.hpp>
#include <ql/rebatedexercise.hpp>

// #include <boost/math/special_functions/laguerre.hpp>

namespace QuantLib {

namespace {
Real basis0(const Real x) { return 1; }
Real basis1(const Real x) { return x; }
Real basis2(const Real x) { return x * x; }
}

Gaussian1dNonstandardSwaptionPathPricer::
    Gaussian1dNonstandardSwaptionPathPricer(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const NonstandardSwaption::arguments *arguments,
        const Handle<YieldTermStructure> &discount, const Handle<Quote> &oas)
    : model_(model), arguments_(arguments), discount_(discount), oas_(oas) {

    // basis functions for ls regression
    basis_.push_back(boost::function1<Real, Real>(&basis0));
    basis_.push_back(boost::function1<Real, Real>(&basis1));
    basis_.push_back(boost::function1<Real, Real>(&basis2));

    // minimum alive exercise index
    Date today = Settings::instance().evaluationDate();
    minIdxAlive_ =
        std::upper_bound(arguments_->exercise->dates().begin(),
                         arguments_->exercise->dates().end(), today) -
        arguments_->exercise->dates().begin();
}

void Gaussian1dNonstandardSwaptionPathPricer::initExerciseIndices(
    const Path &path) const {
    // initialize the indices corresponding to the exercise dates
    if (exerciseIdx_.size() == 0) {
        for (Size i = 0, j = minIdxAlive_;
             i < path.length() && j < arguments_->exercise->dates().size();
             ++i) {
            if (close(
                    model_->stateProcess()->time(arguments_->exercise->date(j)),
                    path.time(i))) {
                exerciseIdx_.push_back(i);
                ++j;
            }
        }
        QL_REQUIRE(exerciseIdx_.size() ==
                       arguments_->exercise->dates().size() - minIdxAlive_,
                   "did not find all future exercise dates ("
                       << arguments_->exercise->dates().size() - minIdxAlive_
                       << ") in path times grid, only matched "
                       << exerciseIdx_.size() << " dates and grid times.");
    }
}

Real Gaussian1dNonstandardSwaptionPathPricer::state(const Path &path,
                                                    Size t) const {
    initExerciseIndices(path);
    return path[exerciseIdx_[t - 1]];
}

std::vector<boost::function1<Real, Real> >
Gaussian1dNonstandardSwaptionPathPricer::basisSystem() const {
    return basis_;
}

Real Gaussian1dNonstandardSwaptionPathPricer::operator()(const Path &path,
                                                         Size t) const {

    initExerciseIndices(path);

    // in the following we have to use a standardized state
    Real state = (path[exerciseIdx_[t - 1]] -
                  model_->stateProcess()->expectation(
                      0.0, 0.0, path.time(exerciseIdx_[t - 1]))) /
                 model_->stateProcess()->stdDeviation(
                     0.0, 0.0, path.time(exerciseIdx_[t - 1]));

    // price all cashflows that belong to the exercise into right
    // and return the deflated NPV
    boost::shared_ptr<RebatedExercise> rebatedExercise =
        boost::dynamic_pointer_cast<RebatedExercise>(arguments_->exercise);
    Date exDate = arguments_->exercise->date(minIdxAlive_ + (t - 1));
    boost::shared_ptr<NonstandardSwap> swap = arguments_->swap;
    Schedule fixedSchedule = swap->fixedSchedule();
    Schedule floatingSchedule = swap->floatingSchedule();
    Size j1 = std::upper_bound(fixedSchedule.dates().begin(),
                               fixedSchedule.dates().end(), exDate - 1) -
              fixedSchedule.dates().begin();
    Size k1 = std::upper_bound(floatingSchedule.dates().begin(),
                               floatingSchedule.dates().end(), exDate - 1) -
              floatingSchedule.dates().begin();

    // this is more or less copied from gaussian1dnonstandardswaptionengine.cpp
    Real floatingLegNpv = 0.0;
    for (Size l = k1; l < arguments_->floatingCoupons.size(); l++) {
        Real zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, arguments_->floatingPayDates[l])));
        Real amount;
        if (arguments_->floatingIsRedemptionFlow[l])
            amount = arguments_->floatingCoupons[l];
        else
            amount = arguments_->floatingNominal[l] *
                     arguments_->floatingAccrualTimes[l] *
                     (arguments_->floatingGearings[l] *
                          model_->forwardRate(
                              arguments_->floatingFixingDates[l], exDate, state,
                              arguments_->swap->iborIndex()) +
                      arguments_->floatingSpreads[l]);
        floatingLegNpv +=
            amount *
            model_->deflatedZerobond(arguments_->floatingPayDates[l], exDate,
                                     state, discount_, discount_) *
            zSpreadDf;
    }
    Real fixedLegNpv = 0.0;
    for (Size l = j1; l < arguments_->fixedCoupons.size(); l++) {
        Real zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, arguments_->fixedPayDates[l])));
        fixedLegNpv +=
            arguments_->fixedCoupons[l] *
            model_->deflatedZerobond(arguments_->fixedPayDates[l], exDate,
                                     state, discount_, discount_) *
            zSpreadDf;
    }
    Real rebate = 0.0;
    Real zSpreadDf = 1.0;
    Date rebateDate = exDate;
    if (rebatedExercise != NULL) {
        rebate = rebatedExercise->rebate(minIdxAlive_ + (t - 1));
        rebateDate = rebatedExercise->rebatePaymentDate(minIdxAlive_ + (t - 1));
        zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, rebateDate)));
    }
    Real exerciseValue =
        std::max(((arguments_->type == VanillaSwap::Payer ? 1.0 : -1.0) *
                      (floatingLegNpv - fixedLegNpv) +
                  rebate *
                      model_->deflatedZerobond(rebateDate, exDate, state,
                                               discount_, discount_) *
                      zSpreadDf),
                 0.0);

    return exerciseValue;
}

} // namespace QuantLib


/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */

#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <algorithm>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include "../components/ConnectedComponents.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(Graph& G,const count k, const count CB,const double epsilon) : G(G),k(k),CB(CB),epsilon(epsilon){
     if (G.isDirected()) throw std::runtime_error("Graph is directed graphs!");
     ConnectedComponents cc(G);
     cc.run();
     if (cc.getPartition().numberOfSubsets() > 1) throw std::runtime_error("Graph has more then one component!");
     if(k>=G.numberOfNodes()) throw std::runtime_error("Size of Group greater then number of nodes!");

     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;

     numberOfCoarsedNodes=0;
     S.resize(k);
     n=G.numberOfNodes();

     end = std::chrono::high_resolution_clock::now();
     diff = end-start;
     std::cout << "Constructor finished in " << diff.count() << "(s)" << "\n\n";
           std::cout << "Number of Nodes=" << G.numberOfNodes() << "\n";
   }

   void CurrentFlowGroupCloseness::run() {
     bool coarse;
     count ID;
     count minDegree;
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;
     std::vector<std::tuple<count,count,count>> c_indices;
     arma::Mat<double> L;
     ID=0;
     if(CB>0){
       ID++;
       minDegree=1;
       mergePeripheralNodes();
       ID++;
       minDegree=updateMinDegree();
       coarse=true;
       while((minDegree<CB)&&(coarse)){
         c_indices=coarsingIndices(minDegree, false);
         coarseGraph(c_indices,ID);
         ID++;
         minDegree=updateMinDegree();

         if(!(c_indices.size()>1)||!(G.numberOfNodes()>k)){
           coarse=false;
         }
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Coarsening " << diff.count() << "(s)" << "\n\n";
      std::cout << "Laplacian size afvoidter coarsening" << L.n_rows << "\n";
      start = std::chrono::high_resolution_clock::now();
      /*changed
      //ERD.computeFromLaplacian(vecOfNodes,L);
      */
      L=computePinvOfLaplacian();
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Initial EffectiveResistanceDistanceMatrix finished in " << diff.count() << "(s)" << "\n\n";
      //greedy();
      //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
      start = std::chrono::high_resolution_clock::now();
      /*
      if(CB>1){
        ID=LevelList[LevelList.size()-1].getID();
        std::cout<<"ID="<<ID<<"\n";
        while(ID>1){
          uncoarseEfffectiveResistanceDistanceMatrix(ID);
          //greedy();
          //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
          ID--;
        }
      }
      */
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Uncoarsening in " << diff.count() << "(s)" << "\n\n";
      start = std::chrono::high_resolution_clock::now();
      greedy(L);
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Greedy in " << diff.count() << "(s)" << "\n\n";
    }
    std::vector<node> CurrentFlowGroupCloseness::getNodesofGroup(){
      return S;
    }
    double CurrentFlowGroupCloseness::getCFGCC() {
      return CFGCC;
    }
    void CurrentFlowGroupCloseness::greedy(arma::Mat<double> L){
      count sampleSize;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, zeroVec, marginalGain;

      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }

      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(vecOfPeripheralNodes[v])
          vecOfPeriphs.push_back(v);
        else{
          vecOfSamples.push_back(v);
        }
      }
      sampleSize=(count)(log(vecOfSamples.size())/(2*epsilon*epsilon));
      if(sampleSize>vecOfSamples.size()){
        sampleSize=vecOfSamples.size();
      }
      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      V.resize(G.numberOfNodes(),true);
      mindst.resize(G.numberOfNodes(),n*n);
      zeroVec.resize(G.numberOfNodes(),0.);
      marginalGain.resize(G.numberOfNodes(),CFGCC);
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          if(V[v] && (bestMarginalGain<marginalGain[v])){
            dst=mindst;
            centrality = 0.;
            for (count l = 0; l < sampleSize; l++) {
              w=vecOfSamples[l];
              distance=L(reverse[v],reverse[v])+L(reverse[w],reverse[w])-2*L(reverse[v],reverse[w]);
              if (distance< mindst[w]){
                dst[w]=distance;
              }
              centrality +=dst[w];
            }
            centrality*=((double)(vecOfSamples.size()+numberOfCoarsedNodes)/(double)(sampleSize));
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              distance=L(reverse[v],reverse[v])+L(reverse[w],reverse[w])-2*L(reverse[v],reverse[w]);
              if (distance< mindst[w]){
                dst[w]=distance;
              }
              centrality += dst[w];
            }
            marginalGain[v]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v];
            }
            //std::cout<<"c("<<v<<")"<<centrality<<"\n";
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    count CurrentFlowGroupCloseness::updateMinDegree(){
      bool search;
      count min;
      count i;
      std::vector<node> vecOfNodes;
      node v;
      search=true;
      min=n;
      i=0;
      vecOfNodes=G.nodes();
      while((i<vecOfNodes.size())&&(search)){
        v=vecOfNodes[i];
        if((G.degree(v)<min) && (G.degree(v)>1)){
          min=G.degree(v);
          if(min==2){
            search=false;
          }
        }
        i++;
      }
      return min;
    }
    std::vector<std::tuple<count,count,count>> CurrentFlowGroupCloseness::coarsingIndices(count courseningDegree, bool Random){
        bool search;
        count c_i,s_i,w_i,l;
        node c,s,w;
        std::vector<bool> vecOfFreeNodes;
        std::vector<node> vecOfNodes, vecOfCoarseNodes, vecOfNeighbours;
        std::vector<std::tuple<count,count,count>> indices;

        vecOfCoarseNodes.resize(0);
        vecOfFreeNodes.resize(G.upperNodeIdBound(),true);

        vecOfNodes=G.nodes();
        for(count i=0;i<vecOfNodes.size();i++){
          c=vecOfNodes[i];
          if(G.degree(c)==courseningDegree){
            vecOfCoarseNodes.push_back(i);
          }
        }
        if(Random){
          std::random_shuffle (vecOfCoarseNodes.begin(), vecOfCoarseNodes.end());
        }
        for(count i=0;i<vecOfCoarseNodes.size();i++){
          c=vecOfCoarseNodes[i];
          if(vecOfFreeNodes[c]){
            search=true;
            l=0;
            vecOfNeighbours=G.neighbors(c);
            while((search)&&(l<vecOfNeighbours.size())){
              s=vecOfNeighbours[l];
              if(vecOfFreeNodes[s]){
                l++;
                while((search)&&(l<vecOfNeighbours.size())){
                  w=vecOfNeighbours[l];
                  if(vecOfFreeNodes[w]){
                    vecOfFreeNodes[s]=false;
                    vecOfFreeNodes[c]=false;
                    vecOfFreeNodes[w]=false;
                    search=false;
                    indices.push_back(std::make_tuple(c,s,w));
                  }
                  else{
                    l++;
                  }
                }
              }
              else{
                l++;
              }
            }//End while
          }
        }
        return indices;
      }
    /**************************************************************************
    void CurrentFlowGroupCloseness::uncoarseEfffectiveResistanceDistanceMatrix(count ID){
      bool search;
      count l,j;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<std::tuple<node,node,count,double,double,double>> vecOfTriangles;
      std::tuple<node,node,count,double,double,double> triangle;

      vecOfTriangles=LevelList[ID-1].getVecOfTriangles();
      for(count i=0;i<vecOfTriangles.size();i++){
        j=vecOfTriangles.size()-1-i;
        triangle=vecOfTriangles[j];
        ERD.uncoarseTriangle(vecOfNodes,triangle);
        c=std::get<0>(triangle);
        s=std::get<1>(triangle);
        w=std::get<2>(triangle);
        edgeWeightcs=std::get<3>(triangle);
        edgeWeightcw=std::get<4>(triangle);
        edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
        edgeWeightsw=1./edgeWeightsw;
        //vecOfNodes.push_back(c);
        //Adj[s].push_back (c);
        //Adj[c].push_back(s);
        if(std::get<4>(triangle)>0){
          //Adj[c].push_back (w);
          //Adj[w].push_back(c);
        }
        if(!(std::get<5>(triangle)==edgeWeightsw)){
          search = true;
          l= 0;
          while((l<Adj[s].size())&&(search)){
            if(Adj[s][l]==w){
              Adj[s].erase(Adj[s].begin()+l);
              search=false;
            }
            else{
              l++;
            }
          }//end while
          search = true;
          l= 0;
          while((l<Adj[w].size())&&(search)){
            if(Adj[w][l]==s){
                Adj[w].erase(Adj[w].begin()+l);
                search=false;
              }
          else{
            l++;
          }
          }//end while
        }//end if
        numberOfCoarsedNodes--;
      }//end for
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::mergePeripheralNodes(){
      bool search;
      count l;
      node c,s,w;
      double edgeWeightcs,edgeWeightss,edgeWeightsw;
      std::vector<node> vecOfNodes,vecOfSupernodes;
      std::vector<std::pair<node,node>> mapping;
      std::vector<std::tuple<node,node,node,double,double,double>> vecOfTriangles;

      vecOfNodes=G.nodes();
      vecOfSupernodes.resize(0);
      mapping.resize(0);
      vecOfTriangles.resize(0);
      for(count c_i=0;c_i<vecOfNodes.size();c_i++){
        c=vecOfNodes[c_i];
        if(G.degree(c)==1){
          s=G.randomNeighbor(c);
          if(std::find(vecOfSupernodes.begin(), vecOfSupernodes.end(), s) == vecOfSupernodes.end()){
            vecOfSupernodes.push_back(s);
            mapping.push_back (std::make_pair(s,c));
            vecOfPeripheralNodes[c]=true;
          }
          else{
            search = true;
            l= 0;
            while(search){
              if(vecOfSupernodes[l]==s){
                w=mapping[l].second;
                search=false;
              }
              l++;
            }//end while
            edgeWeightcs=G.weight(c,s);
            edgeWeightsw=G.weight(s,w);
            edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
            edgeWeightsw=1./edgeWeightsw;
            G.setWeight(s,w,edgeWeightsw);
            G.removeNode(c);
            vecOfTriangles.push_back(std::make_tuple(c,s,w,edgeWeightcs,0.,edgeWeightsw));
          }//end else
        }
      }
      ERDLevel Level(1,vecOfTriangles);
      LevelList.push_back(Level);
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseGraph(std::vector<std::tuple<count,count,count>> matchings,count ID){
      bool search;
      count a,l,c_i,s_i,w_i;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<node> vecOfNodes;
      std::vector<std::tuple<node,node,node,double,double,double>> vecOfTriangles;

      vecOfNodes=G.nodes();
      vecOfTriangles.resize(0);

      for(count i=0;i<matchings.size();i++){
        search=false;
        c=std::get<0>(matchings[i]);
        s=std::get<1>(matchings[i]);
        w_i=std::get<2>(matchings[i]);
        w=vecOfNodes[w_i];
        edgeWeightcs=G.weight(c,s);
        edgeWeightcw=G.weight(c,w);
        edgeWeightsw=1./edgeWeightcs+1/edgeWeightcw;
        edgeWeightsw=1./edgeWeightsw;
        edgeWeightsw+=G.weight(s,w);
        G.setWeight(s,w,edgeWeightsw);
        vecOfTriangles.push_back(std::make_tuple(c,s,w,edgeWeightcs,edgeWeightcw,edgeWeightsw));
        G.removeNode(c);
        numberOfCoarsedNodes++;
      }
      /*
      arma::uvec indices(vecOfNodes.size()-matchings.size());
      a=0;
      l=0;
      for(count i=0;i<vecOfNodes.size();i++){
        if(std::get<0>(matchings[l])==i){
          if(l<matchings.size()-1)
            l++;
        }
        else{
          indices(a)=i;
          a++;
        }
      }
      for(count i=0;i<matchings.size();i++){
        vecOfNodes.erase(vecOfNodes.begin()+std::get<0>(matchings[i])-i);
      }
      L=L.submat(indices, indices);
      */
      ERDLevel Level(ID,vecOfTriangles);
      LevelList.push_back(Level);
    }

    arma::Mat<double> CurrentFlowGroupCloseness::computePinvOfLaplacian(){
      node v;
      node w;
      count n;
      double factor;
      std::vector<node> vecOfNodes;
      vecOfNodes=G.nodes();
      n=vecOfNodes.size();
      arma::Mat<double> L(n,n);
      L.zeros();
      for(count i=0;i<n;i++){
        v=vecOfNodes[i];
        for(count j=i+1;j<n;j++){
          w=vecOfNodes[j];
          if(G.hasEdge(v,w)){
            L(i,j)=-G.weight(v,w);
            L(j,i)=-G.weight(v,w);
            L(i,i)+=G.weight(v,w);
            L(j,j)+=G.weight(v,w);
          }
        }
      }
      arma::Mat<double> J(n,n);
      factor=1./n;
      J.fill(factor);
      L= L+J;
      L=arma::inv_sympd(L);
      L= L-J;
      return L;
    }
} /* namespace NetworKit*/


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/SizeParam.h"
#include "latbuilder/Util.h"
#include "latticetester/Util.h"
#include "latticetester/IntFactor.h"
#include <NTL/GF2XFactoring.h>

namespace LatBuilder {

//===============================================================================================================
template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::SizeParam(uInteger primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>>(primeBase == 0 ? 0 : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if (primeBase >= 2 and LatticeTester::IntFactor<std::int64_t>::isPrime(primeBase, 0) == LatticeTester::COMPOSITE)
      throw std::invalid_argument("SizeParam: primeBase is not prime");
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::SizeParam(Polynomial primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>>(IsZero(primeBase) ? Polynomial(0) : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if ( !IterIrredTest(primeBase))
      throw std::invalid_argument("SizeParam: primeBase is not prime");

}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::SizeParam(uInteger primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>>(primeBase == 0 ? 0 : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if (primeBase >= 2 and LatticeTester::IntFactor<std::int64_t>::isPrime(primeBase, 0) == LatticeTester::COMPOSITE)
      throw std::invalid_argument("SizeParam: primeBase is not prime");
}

//===================================================================================================================

template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::SizeParam(uInteger numPoints):
   BasicSizeParam<SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>>(numPoints)
{
   if (numPoints == 0) {
      m_base = 0;
      m_maxLevel = 0;
   }
   else if (numPoints == 1) {
      m_base = 1;
      m_maxLevel = 0;
   }
   else {
      const auto factors = primeFactorsMap(numPoints);
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.first;
      m_maxLevel = factor.second;
   }
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::SizeParam(Polynomial modulus):
   BasicSizeParam<SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>>(modulus)
{
   if (IsZero(modulus)) {
      m_base = Polynomial(0);
      m_maxLevel = 0;
   }
   else {
      NTL::vector< NTL::Pair< Polynomial, long > > factors ;
      CanZass(factors, modulus); // calls "Cantor/Zassenhaus" algorithm from <NTL/GF2XFactoring.h>
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.a; // = factor.first
      m_maxLevel = factor.b; // = factor.second
   }
}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::SizeParam(uInteger numPoints):
   BasicSizeParam<SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>>(numPoints)
{
   if (numPoints == 0) {
      m_base = 0;
      m_maxLevel = 0;
   }
   else if (numPoints == 1) {
      m_base = 1;
      m_maxLevel = 0;
   }
   else {
      const auto factors = primeFactorsMap(numPoints);
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.first;
      m_maxLevel = factor.second;
   }
}

//=======================================================================================================================

template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return base() == 0 ? 0 : intPow(base(), level);
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return IsZero(base())  ? 0 : intPow( 2, deg(base())*level );
}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return base() == 0 ? 0 : intPow(base(), level);
}
//========================================================================================================================

template<>
size_t
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::totient() const
{ return base() == 0 ? 0 : (base() - 1) * this->numPoints() / base(); }

template<>
size_t
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::totient() const
{ return IsZero(base()) == 0 ? 0 : (intPow(2,deg(base())) - 1) * this->numPoints() / intPow(2,deg(base())); }

//=========================================================================================================================

template<LatticeType LR>
void
SizeParam<LR,EmbeddingType::MULTILEVEL>::normalize(Real& merit) const
{ merit /= this->numPoints(); }

template<LatticeType LR>
void
SizeParam<LR,EmbeddingType::MULTILEVEL>::normalize(RealVector& merit) const
{
   if (merit.size() != maxLevel() + 1)
      throw std::logic_error("merit vector size and maximum level do not match");
   for (Level level = 0; level < merit.size(); level++)
      merit[level] /= numPointsOnLevel(level);
}
//===========================================================================================================================
template<LatticeType LR>
std::ostream&
SizeParam<LR,EmbeddingType::MULTILEVEL>::format(std::ostream& os) const
{ os << base(); if (maxLevel() != 1) os << "^" << maxLevel(); return os; }
//===========================================================================================================================

template class SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>;
template class SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>;
template class SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>;

}


#include <iostream>
#include <armadillo>
#include <complex>
#include <cstdlib>
#include <ctime>
using namespace std;
using namespace arma;

int main() {
	// Input: sizeCells of lattice in z direction and # of k-points in the direction of kPointVec
	int sizeCells = 5;
	int k_points_max=1+19*1;
	rowvec kPointVec(3); // Defined below

	// Parameters for the Hamiltonian and data structures (Don't need to change those)
	double tHopping = 1;
	double deltatHopping = 0.4;
	complex<double> spinOrbitCoupling(0,1);

	// Misc
	double delta = 0.01; // small number for numerical comparisons
	int index =0;
	const complex<double> ii(0, 1);

	// Geometry
	double latVecNorm=1/sqrt(2); // cubic cell size a = 1
	double sublatVecNorm=sqrt(3)/4;
	int nSites=3*sizeCells;

	rowvec dummyVec(3), dummyVec2(3), dummyVec3(3), dummyVec4(3);
	cx_mat dummyMat(2,2);
	mat sublatOne(nSites, 4); // Sites at origin of a unit cell
	mat sublatTwo(nSites, 4); // Sites at sublatVec in a unit cell
	rowvec sublatVec(3); // 2nd lattice site within a unit cell
	rowvec latVec1(3), latVec2(3), latVec3(3), latVec4(3), latVec5(3); // lattice vectors
	rowvec bulklatVec1(3), bulklatVec2(3), bulklatVec3(3), GVec1(3), GVec2(3);
	sublatVec << 0.25 << 0.25 << 0.25;
	bulklatVec1 << 0 << 0.5 << 0.5;
	bulklatVec2 << 0.5 << 0 << 0.5;
	bulklatVec3 << 0.5 << 0.5 << 0;

	// Variables below are needed for periodic b.c.s
	// Unit cell
	latVec1 << 0.5 << -0.5 << 0;
	latVec2 << 0 << 0.5 << -0.5;
	latVec3 << 1 << 1 << 1;
	// Two more atoms from sublattice One within the primary unit cell
	latVec4 << 0 << 0.5 << 0.5;
	latVec5 << 0.5 << 1 << 0.5;
	// Reciprocal lattice vectors for slab
	GVec1 << 4*M_PI/3 << -2*M_PI/3 << -2*M_PI/3;
	GVec2 << 2*M_PI/3 << 2*M_PI/3 << -4*M_PI/3;

	// Define Pauli matrices
	cx_mat unity(2,2), sigma_x(2,2), sigma_y(2,2), sigma_z(2,2); // Pauli matrices
	mat dummy_unity(2,2), dummy_sigma_x(2,2), dummy_sigma_y(2,2), dummy_sigma_z(2,2);
	dummy_unity(0,0)=1; dummy_unity(1,1)=1; dummy_sigma_x(1,0)=1; dummy_sigma_x(0,1)=1;
	dummy_sigma_y(0,1)=-1; dummy_sigma_y(1,0)=1; dummy_sigma_z(0,0)=1; dummy_sigma_z(1,1)=-1;
	unity.set_real(dummy_unity);
	sigma_x.set_real(dummy_sigma_x);
	sigma_y.set_imag(dummy_sigma_y);
	sigma_z.set_real(dummy_sigma_z);

	// Set up data structures for the slab
	cx_mat Hamiltonian(4*nSites,4*nSites);
	Hamiltonian.zeros();
	vec eigvals(4*nSites);
	cx_mat eigvecs(4*nSites,4*nSites);
	mat bandstructure(k_points_max,4*nSites);

	// Set up primary unit cell of the slab
	for (int k=0; k<sizeCells; k++) {
		dummyVec=k*latVec3;
		sublatOne(index,0)=99; sublatOne(index+1,0)=99; sublatOne(index+2,0)=99;
		sublatOne(index,span(1,3))=dummyVec;
		sublatOne(index+1,span(1,3))=(dummyVec+latVec4);
		sublatOne(index+2,span(1,3))=(dummyVec+latVec5);
		sublatTwo(index,0)=99; sublatTwo(index+1,0)=99; sublatTwo(index+2,0)=99;
		sublatTwo(index,span(1,3))=(dummyVec+sublatVec);
		sublatTwo(index+1,span(1,3))=(dummyVec+latVec4+sublatVec);
		sublatTwo(index+2,span(1,3))=(dummyVec+latVec5+sublatVec);
		index=index+3;
	}

	// Loop over k-points
	for (int m=0; m<k_points_max; m++ ){
		kPointVec=GVec1*m/20;

		// Set up the Hamiltonian in a different way (results equivalent)
		for (int n=0; n<nSites; n++){
			// Go over unit cells in width (i,j; periodic), height (k) and across sites in each cell (l)
			for (int i=-2; i<3; i++) {
				for (int j=-2; j<3; j++) {
					for (int k=0; k<sizeCells; k++) {
						for (int l=0; l<3; l++) {
							switch(l) {
								case 0:
									dummyVec=i*latVec1+j*latVec2+k*latVec3-sublatOne(n,span(1,3));
									break;
								case 1:
									dummyVec=i*latVec1+j*latVec2+k*latVec3+latVec4-sublatOne(n,span(1,3));
									break;
								case 2:
									dummyVec=i*latVec1+j*latVec2+k*latVec3+latVec5-sublatOne(n,span(1,3));
									break;
								default:
									cout << "Error, l should be 0,1,2" << endl;
							}
							dummyVec2=dummyVec+sublatVec; // used to find hoppings between sublattices
							index=3*k+l; // used when have a site within primary cell
							dummyVec3=i*latVec1+j*latVec2; // used if have an atom outside primary cell
							dummyVec4=dummyVec+sublatOne(n,span(1,3))-sublatTwo(n,span(1,3));

							// Set up nearest neighbour hoppings (between sublattices)
							// Set up hoppings into sublatOne
							if (dot(dummyVec2, dummyVec2) < sublatVecNorm*sublatVecNorm+delta) {
								// hopping to the n-th site from along [111] within the primary cell
								if (dot(dummyVec, dummyVec) < delta) {
									Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=(tHopping+deltatHopping)*unity;
									if (n != index || i!=0 || j!=0) {cout << "Error for deltatHopping" << endl; }
									// hopping in other directions
								} else {
									if (i==0 && j==0) {
										// Hoppings withing the primary cell
										Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=tHopping*unity;
										if (i!=0 || j!=0 || n==index) {cout << "Error for tHopping within primary cell" << endl; }
										// hopping to sublatOne from outside the primary cell
									} else {
										// hoppings from sublatTwo of index to sublatOne of n
										Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=tHopping*exp(ii*dot(dummyVec3,kPointVec))*unity;
										if (n == index) {cout << "Error for tHopping" << endl; }
									}
								}
							}

							// Set up hoppings into sublatTwo
							if (dot(dummyVec4, dummyVec4) < sublatVecNorm*sublatVecNorm+delta) {
								// hopping to the n-th site from along [111] within the primary cell
								if (dot(dummyVec4+sublatVec,dummyVec4+sublatVec) < delta) {
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=(tHopping+deltatHopping)*unity;
									if (n != index || i!=0 || j!=0) {cout << "Error for deltatHopping" << endl; }
									// hopping in other directions
								} else {
									if (i==0 && j==0) {
										// Hoppings withing the primary cell
										Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=tHopping*unity;
										if (i!=0 || j!=0 || n==index) {cout << "Error for tHopping within primary cell" << endl; }
										// hopping to sublatTwo from outside the primary cell
									} else {
										// hoppings from sublatOne of index to sublatTwo of n
										Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=tHopping*exp(ii*dot(dummyVec3,kPointVec))*unity;
										if (n == index) {cout << "Error for tHopping" << endl; }
									}
								}
							}
							if ( (dot(dummyVec,dummyVec) < latVecNorm*latVecNorm+delta)  &&
									(dot(dummyVec,dummyVec) > sublatVecNorm*sublatVecNorm+delta) ) {
								// Matrix elements are direction-dependent
								if ( (dummyVec(0) > 0 && dummyVec(1) > 0) || (dummyVec(0) > 0 &&
											dummyVec(2) > 0) || (dummyVec(1) > 0 && dummyVec(2) > 0) ) {
									dummyVec = cross(sublatVec, dummyVec-sublatVec);
								} else if ( (dummyVec(1) < 0 && dummyVec(2) < 0) || (dummyVec(0) > 0 &&
											dummyVec(1) < 0) || (dummyVec(0) > 0 && dummyVec(2) < 0) ) {
									dummyVec = cross(-bulklatVec1+sublatVec, dummyVec+bulklatVec1-sublatVec);
								} else if ( (dummyVec(0) < 0 && dummyVec(1) > 0) || (dummyVec(0) < 0 &&
											dummyVec(2) < 0) || (dummyVec(1) > 0 && dummyVec(2) < 0) ) {
									dummyVec = cross(-bulklatVec2+sublatVec, dummyVec+bulklatVec2-sublatVec);
								} else if ( (dummyVec(0) < 0 && dummyVec(2) > 0) || (dummyVec(1) < 0 &&
											dummyVec(2) > 0) || (dummyVec(0) < 0 && dummyVec(1) < 0) ) {
									dummyVec = cross(-bulklatVec3+sublatVec, dummyVec+bulklatVec3-sublatVec);
								} else {cout << "Error in NNN hopping" << endl; }
								dummyMat=dummyVec(0)*sigma_x+dummyVec(1)*sigma_y+dummyVec(2)*sigma_z;
								if (i==0 && j==0) {
									Hamiltonian(span(4*n,4*n+1),span(4*index,4*index+1))+=spinOrbitCoupling*dummyMat;
									// Minus sign because for sublatTwo dummyVec is minus that of sublatOne
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+2,4*index+3))+= -1.0*spinOrbitCoupling*dummyMat;
								} else {
									// Hoppings within sublatOne
									Hamiltonian(span(4*n,4*n+1),span(4*index,4*index+1))+=exp(ii*dot(dummyVec3, kPointVec))*spinOrbitCoupling*dummyMat;
									// Hoppings within sublatTwo
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+2,4*index+3))+= -1.0*exp(ii*dot(dummyVec3,kPointVec))*spinOrbitCoupling*dummyMat;
								}

							}
						}
					}
				}
			}
		}

		// abs returns a matrix, max returns a vector, 2nd max - largest value
		if (max(max(abs(Hamiltonian-Hamiltonian.t()))) > delta ) {
			cout << "Error, Hamiltonian is not Hermitian by at least " << delta << endl;
		}

		eig_sym(eigvals, eigvecs, Hamiltonian);
		bandstructure.row(m)=eigvals.t();
		cout << endl;
	//	Hamiltonian.save("Hamiltonian.txt", arma_ascii);
		cout << "Calculated k-point";
		kPointVec.print();
		Hamiltonian.zeros();
	}

	bandstructure.save("output_bs.txt", raw_ascii);
	//sublatOne.save("output.txt",raw_ascii);
	//sublatTwo.save("output2.txt",raw_ascii);
}