#include "pch.h"
#include "ssdsMath.h"
#include "ssdsTypes.h"
#include <Eigen/Eigenvalues>

namespace ssds {

static float4x4 calcRegistrationT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    double3 cs{}, cd{}; // double for precision
    for (size_t i = 0; i < num_points; ++i) {
        cs += s[i];
        cd += d[i];
    }
    return translate(float3((cd - cs) / double(num_points)));
}

static float4x4 calcRegistrationRT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    double3 cs{}, cd{}; // double for precision
    for (size_t i = 0; i < num_points; ++i) {
        cs += s[i];
        cd += d[i];
    }
    cs /= double(num_points);
    cd /= double(num_points);
    if (num_points < 3)
        return translate(cd - cs);

    Eigen::Matrix<double, 4, 4> moment;
    {
        auto sit = s.data();
        auto dit = d.data();
        double sxx = 0, sxy = 0, sxz = 0, syx = 0, syy = 0, syz = 0, szx = 0, szy = 0, szz = 0;
        for (int i = 0; i < num_points; ++i, ++sit, ++dit) {
            sxx += (sit->x - cs.x) * (dit->x - cd.x);
            sxy += (sit->x - cs.x) * (dit->y - cd.y);
            sxz += (sit->x - cs.x) * (dit->z - cd.z);
            syx += (sit->y - cs.y) * (dit->x - cd.x);
            syy += (sit->y - cs.y) * (dit->y - cd.y);
            syz += (sit->y - cs.y) * (dit->z - cd.z);
            szx += (sit->z - cs.z) * (dit->x - cd.x);
            szy += (sit->z - cs.z) * (dit->y - cd.y);
            szz += (sit->z - cs.z) * (dit->z - cd.z);
        }
        moment(0, 0) = sxx + syy + szz;
        moment(0, 1) = syz - szy;        moment(1, 0) = moment(0, 1);
        moment(0, 2) = szx - sxz;        moment(2, 0) = moment(0, 2);
        moment(0, 3) = sxy - syx;        moment(3, 0) = moment(0, 3);
        moment(1, 1) = sxx - syy - szz;
        moment(1, 2) = sxy + syx;        moment(2, 1) = moment(1, 2);
        moment(1, 3) = szx + sxz;        moment(3, 1) = moment(1, 3);
        moment(2, 2) = -sxx + syy - szz;
        moment(2, 3) = syz + szy;        moment(3, 2) = moment(2, 3);
        moment(3, 3) = -sxx - syy + szz;
    }

    float4x4 transform;
    if (moment.norm() > 0) {
        Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> es(moment);
        int maxi = 0;
        for (int i = 1; i < 4; ++i) {
            if (es.eigenvalues()(maxi).real() < es.eigenvalues()(i).real()) {
                maxi = i;
            }
        }

        quatf rot = {
            (float)es.eigenvectors()(0, maxi).real(),
            (float)es.eigenvectors()(1, maxi).real(),
            (float)es.eigenvectors()(2, maxi).real(),
            (float)es.eigenvectors()(3, maxi).real(),
        };
        transform = to_mat4x4(rot);
    }
    float3 cs0 = mul_p(transform, float3(cs));
    (float3&)transform[3] = cd - cs0;
    return transform;
}

#if 0
float4x4 calcRegistrationSRT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    const int num_iterations = 50;
    const double threshold = 1.0e-8;
    const double scaleLowerBound = 1.0e-3;
    float4x4 transform = calcRegistrationRT(s, d);

    float3 scale = float3::one();
    float3 denom = float3::zero();
    {
        auto sit = s.data();
        for (int i = 0; i < num_points; ++i, ++sit) {
            denom[0] += (*sit)[0] * (*sit)[0];
            denom[1] += (*sit)[1] * (*sit)[1];
            denom[2] += (*sit)[2] * (*sit)[2];
        }
    }

    std::vector<float3> sstm(num_points);
    {
        auto sit = s.data();
        for (int i = 0; i < num_points; ++i, ++sit) {
            sstm[i][0] = (*sit)[0];
            sstm[i][1] = (*sit)[1];
            sstm[i][2] = (*sit)[2];
        }
    }

    std::vector<float3> dstm(num_points);
    for (size_t l = 0; l < num_iterations; ++l) {
        float4x4 im = invert(transform);
        auto dit = d.data();
        for (size_t i = 0; i < num_points; ++i, ++dit)
            dstm[i] = mul_p(im, *dit);

        for (size_t d = 0; d < 3; ++d) {
            double ds = 0.0;
            auto sit = s.data();
            for (size_t i = 0; i < num_points; ++i, ++sit) {
                ds += (*sit)[d] * dstm[i][d];
            }
            scale[d] = denom[d] < threshold ? 1.0 : ds / denom[d];
            sit = s.data();
            for (size_t i = 0; i < num_points; ++i, ++sit) {
                sstm[i][d] = scale[d] * (*sit)[d];
            }
        }
        transform = calcRegistrationRT(num_points, sstm.begin(), pd);
    }
    double sv[3] = { scale[0], scale[1], scale[2] };
    transform.setScale(sv, MSpace::kTransform);
    return transform;
}

using RegistrationFunc = float4x4(*)(span<float3> ps, span<float3> pd);

static RegistrationFunc registrationFuncs[3] = {
    calcRegistrationT,
    calcRegistrationRT,
    calcRegistrationSRT
};

void computeSamplePoints(std::vector<float3>& sample, int sid, int joint, const Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int& num_indices = output.num_indices;

    for (int v = 0; v < num_vertices; ++v) {
        sample[v] = input.sample[sid * num_vertices + v];
        const float3& s = input.rest_shape[v];
        for (int i = 0; i < num_indices; ++i) {
            const int jnt = output.skin_indices[v * num_indices + i];
            if (jnt >= 0 && jnt != joint) {
                const double w = output.skin_weights[v * num_indices + i];
                const float4x4& at = output.skinMatrix[sid * input.num_joints + jnt];
                sample[v] -= w * (s * at.asMatrix());
            }
        }
    }
}

void subtractCentroid(std::vector<float3>& model, std::vector<float3>& sample, float3& corModel, float3& corSample, const Eigen::VectorXd& weight, const Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;

    double wsqsum = 0;
    corModel = {};
    corSample = {};
    for (int v = 0; v < num_vertices; ++v) {
        const double w = weight[v];
        corModel += w * w * input.restShape[v];
        corSample += w * sample[v];
        wsqsum += w * w;
    }
    corModel = corModel / wsqsum;
    corSample = corSample / wsqsum;
    for (int v = 0; v < num_vertices; ++v) {
        model[v] = weight[v] * (input.restShape[v] - corModel);
        sample[v] -= weight[v] * corSample;
    }
}

class JointTransformUpdator
{
private:
    Output* output;
    const Input* input;
    const Eigen::VectorXd* weight;
    int transformType;
    int joint;

public:
    JointTransformUpdator(Output* output_, const Input* input_,
        const Eigen::VectorXd* weight_, int joint_, int transformType_)
        : output(output_), input(input_),
        weight(weight_), joint(joint_), transformType(transformType_)
    {
    }

    void operator ()(const tbb::blocked_range<int>& range) const
    {
        std::vector<float3> model(input->num_vertices);
        std::vector<float3> sample(input->num_vertices);
        for (int s = range.begin(); s != range.end(); ++s) {
            if (s == 0) {
                output->skinMatrix[joint] = float4x4::identity;
                continue;
            }
            computeSamplePoints(sample, s, joint, *output, *input);
            float3 corModel(0, 0, 0), corSample(0, 0, 0);
            subtractCentroid(model, sample, corModel, corSample, *weight, *output, *input);
            float4x4 transform = registrationFuncs[transformType](model.size(), model.begin(), sample.begin());
            float3 d = corSample - corModel * transform.asMatrix();
            transform.setTranslation(d + transform.getTranslation(MSpace::kTransform), MSpace::kTransform);
            output->skinMatrix[s * input->num_joints + joint] = transform;
        }
    }
};

void updateJointTransformProc(Output& output, int transformType, const Input& input, int selection = -1)
{
    const int num_vertices = input.sample.front().points.size();
    const int num_samples  = input.sample.size();
    const int num_joints    = output.num_joints;
    const int num_indices  = output.num_indices;

    Eigen::VectorXd weight = Eigen::VectorXd::Zero(num_vertices);
    int begin = selection < 0 ? 0 : selection;
    int end = selection < 0 ? num_joints : selection + 1;
    for (int joint = begin; joint < end; ++joint) {
        for (int v = 0; v < num_vertices; ++v) {
            weight[v] = 0.0;
            for (int i = 0; i < num_indices; ++i) {
                int jnt = output.skin_indices[v * num_indices + i];
                if (jnt == joint) {
                    weight[v] = output.skin_weights[v * num_indices + i];
                    break;
                }
            }
        }
        double wsqsum = weight.dot(weight);
        if (wsqsum > 1.0e-8) {
            tbb::blocked_range<int> blockedRange(0, num_samples);
            JointTransformUpdator transformUpdator(&output, &input, &weight, joint, transformType);
            tbb::parallel_for(blockedRange, transformUpdator);
        }
        else {
            for (int s = 0; s < num_samples; ++s) {
                output.skinMatrix[s * input.num_joints + joint] = float4x4::identity;
            }
            char buf[256];
            printf(buf, "Fixed joint #%d", joint);
        }
    }
}

static PyObject* updateJointTransform(PyObject *self, PyObject *args)
{
    PyObject* pin            = PyTuple_GET_ITEM(args, 0);
    PyObject* pout           = PyTuple_GET_ITEM(args, 1);
    const int transformType = PyInt_AsLong(PyTuple_GET_ITEM(args, 2));
    Input* const input   = reinterpret_cast<Input*>(PyCapsule_GetPointer(pin, "SSDSInput"));
    Output* const output = reinterpret_cast<Output*>(PyCapsule_GetPointer(pout, "SSDSOutput"));
    updateJointTransformProc(*output, transformType, *input);
    return Py_None;
}

// see https://sites.google.com/view/fumiyanarita/project/la_ssdr_mdmc
static void detectNeighborClusters(const Input& input, Output& output)
{
    MGlobal::displayInfo("Detecting neighbor clusters");
    const int& num_indices = output.num_indices;

    std::vector<std::set<int>> clusters(output.num_joints);
    for (int v = 0; v < input.num_vertices; ++v) {
        const int primjid = output.skinIndex[v * num_indices];
        if (input.numRings == 0) {
            for (int j = 0; j < output.num_joints; ++j) {
                for (int v = 0; v < output.num_joints; ++v) {
                    clusters[j].insert(v);
                }
            }
        }
        else {
            clusters[primjid].insert(0);
            clusters[primjid].insert(primjid);
            // one-ring neighbors
            for (auto nvit = input.neighbor[v].begin(); nvit != input.neighbor[v].end(); ++nvit) {
                const int nvid = nvit->first;
                clusters[primjid].insert(output.skinIndex[nvid * num_indices]);
                if (input.numRings == 1) {
                    continue;
                }
                // two-ring neighbors
                for (auto nnvit = input.neighbor[nvid].begin(); nnvit != input.neighbor[nvid].end(); ++nnvit) {
                    const int nnvid = nnvit->first;
                    clusters[primjid].insert(output.skinIndex[nnvid * num_indices]);
                }
            }
        }
    }
    for (int v = 0; v < input.num_vertices; ++v) {
        const int primjid = output.skinIndex[v * num_indices];
        output.vertCluster[v] = std::vector<int>(clusters[primjid].begin(), clusters[primjid].end());
    }
}

int bindVertexToJoint(Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int num_samples  = input.num_samples;
    const int num_indices  = output.num_indices;

    std::vector<int> numJointVertices(output.num_joints, 0);
    for (int v = 0; v < num_vertices; ++v) {
        int bestJoint = 0;
        double minErr = std::numeric_limits<double>::max();
        const float3& restShapePos = input.restShape[v];
        for (int j = 0; j < output.num_joints; ++j) {
            double errsq = 0;
            for (int s = 0; s < num_samples; ++s) {
                float4x4 am = output.skinMatrix[s * input.num_joints + j].asMatrix();
                float3 diff = input.sample[s * num_vertices + v] - restShapePos * am;
                errsq += diff * diff;
            }
            errsq *= (input.restShape[v] - output.restJointPos[j]).length();
            if (errsq < minErr) {
                bestJoint = j;
                minErr = errsq;
            }
        }
        ++numJointVertices[bestJoint];
        output.skinIndex[v * num_indices + 0] = bestJoint;
    }

    std::vector<int>::iterator smallestBoneSize = std::min_element(numJointVertices.begin(), numJointVertices.end());
    while (*smallestBoneSize <= 0) {
        const int smallestBone = static_cast<int>(smallestBoneSize - numJointVertices.begin());
        numJointVertices.erase(numJointVertices.begin() + smallestBone);
        output.restJointPos.erase(output.restJointPos.begin() + smallestBone);
        for (int s = 0; s < input.num_samples; ++s) {
            for (int j = output.num_joints - 2; j >= smallestBone; --j) {
                output.skinMatrix[s * input.num_joints + j] = output.skinMatrix[s * input.num_joints + j + 1];
            }
        }
        for (int v = 0; v < num_vertices; ++v) {
            if (output.skinIndex[v * num_indices + 0] >= smallestBone) {
                --output.skinIndex[v * num_indices + 0];
            }
        }
        --output.num_joints;
        smallestBoneSize = std::min_element(numJointVertices.begin(), numJointVertices.end());
    }
    return static_cast<int>(numJointVertices.size());
}

int findMostStableVertex(const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int num_samples = input.num_samples;
    double minErrorSq = std::numeric_limits<double>::max();
    int mostStableVertex = -1;
    for (int v = 0; v < num_vertices; ++v) {
        double errSq = 0;
        for (int s = 0; s < num_samples; ++s) {
            float3 diff = input.sample[s * num_vertices + v] - input.restShape[v];
            errSq += diff * diff;
        }
        if (errSq < minErrorSq) {
            minErrorSq = errSq;
            mostStableVertex = v;
        }
    }
    return mostStableVertex;
}

int findDistantVertex(const Input& input, const Output& output, const std::set<int>& covered)
{
    const int num_vertices = input.num_vertices;
    const int num_samples  = input.num_samples;
    const int num_indices  = output.num_indices;
    double maxErrorSq      = -std::numeric_limits<double>::max();
    int mostDistantVertex = -1;
    for (int v = 0; v < num_vertices; ++v) {
        if (covered.find(v) != covered.end()) {
            continue;
        }
        int index = output.skinIndex[v * num_indices + 0];
        double errSq = 0;
        for (int s = 0; s < num_samples; ++s) {
            float4x4 bm = output.skinMatrix[s * input.num_joints + index].asMatrix();
            float3 diff = input.sample[s * num_vertices + v] - input.restShape[v] * bm;
            errSq += diff * diff;
        }
        if (errSq > maxErrorSq) {
            maxErrorSq = errSq;
            mostDistantVertex = v;
        }
    }
    return mostDistantVertex;
}

// solving p-center problem
static void clusterVerticesPcenter(Output& output, const Input& input, int transformType)
{
    const int& num_indices = output.num_indices;

    output.num_joints = input.num_joints;
    std::fill(output.skinIndex.begin(), output.skinIndex.end(), -1);
    std::fill(output.skinWeight.begin(), output.skinWeight.end(), 0.0);
    std::fill(output.skinMatrix.begin(), output.skinMatrix.end(), float4x4::identity);
    std::fill(output.restJointPos.begin(), output.restJointPos.end(), float3::origin);

    char buf[512];
    int stable_vertex = findMostStableVertex(input);
    output.weights[stable_vertex * num_indices] = { 0, 1.0f };
    output.restJointPos[0] = input.restShape[stable_vertex];
    std::vector<int> joint_vertices(output.num_joints);
    joint_vertices[0] = stable_vertex;
    sprintf(buf, "Added to Vertex %d (stable)", stable_vertex);

    for (int jid = 1; jid < input.num_joints; ++jid) {
        double max_dist = -1.0;
        int distant_vertex = -1;
        for (int i = 0; i < input.num_vertices; ++i) {
            if (std::find(joint_vertices.begin(), joint_vertices.end(), i) != joint_vertices.end()) {
                continue;
            }
            double mindist = std::numeric_limits<double>::max();
            for (int j = 0; j < jid; ++j) {
                const int joint = joint_vertices[j];
                float3 v = input.restShape[i] - input.restShape[joint];
                if (mindist > v.length()) {
                    mindist = v.length();
                }
            }
            if (mindist > max_dist)
            {
                max_dist = mindist;
                distant_vertex = i;
            }
        }
        output.restJointPos[jid] = input.restShape[distant_vertex];
        joint_vertices[jid] = distant_vertex;
        sprintf(buf, "Added to Vertex %d, %f", distant_vertex, max_dist);
    }

    for (int i = 0; i < input.num_vertices; ++i) {
        double min_dist = std::numeric_limits<double>::max();
        int nearest_joint = -1;
        for (int j = 0; j < output.num_joints; ++j) {
            const int joint = joint_vertices[j];
            float3 v = input.restShape[i] - input.restShape[joint];
            if (v.length() < min_dist) {
                min_dist = v.length();
                nearest_joint = j;
            }
        }
        output.skinIndex[i * num_indices] = nearest_joint;
        output.skinWeight[i * num_indices] = 1.0;
    }
    detectNeighborClusters(input, output);
    updateJointTransformProc(output, transformType, input);
}

static void clusterVerticesAdaptive(Output& output, const Input& input, int transformType)
{
    const int& num_indices = output.num_indices;

    output.num_joints = 0;
    std::fill(output.skinIndex.begin(), output.skinIndex.end(), -1);
    std::fill(output.skinWeight.begin(), output.skinWeight.end(), 0.0);
    std::fill(output.skinMatrix.begin(), output.skinMatrix.end(), float4x4::identity);
    output.restJointPos.clear();

    std::set<int> covered;
    char buf[512];

    int stable_vertex = findMostStableVertex(input);
    output.skinIndex[stable_vertex * num_indices] = 0;
    output.skinWeight[stable_vertex * num_indices] = 1.0;
    output.restJointPos.push_back(input.restShape[stable_vertex]);
    covered.insert(stable_vertex);
    sprintf(buf, "Added to Vertex %d (stable)", stable_vertex);

    for (int i = 0; i < input.neighbor[stable_vertex].size(); ++i) {
        int neighbor = input.neighbor[stable_vertex][i].first;
        if (neighbor < 0) {
            continue;
        }
        output.skinIndex[neighbor * num_indices] = 0;
        output.skinWeight[neighbor * num_indices] = 1.0;
        covered.insert(neighbor);
    }
    updateJointTransformProc(output, transformType, input, 0);
    for (int v = 0; v < input.num_vertices; ++v) {
        output.skinIndex[v * num_indices] = 0;
        output.skinWeight[v * num_indices] = 1.0;
    }
    output.num_joints = 1;

    for (int iteration = 0; iteration < input.num_joints + 10; ++iteration) {
        if (output.num_joints >= input.num_joints || covered.size() >= input.num_vertices) {
            break;
        }
        int distant_vertex = findDistantVertex(input, output, covered);
        output.skinIndex[distant_vertex * num_indices] = output.num_joints;
        output.restJointPos.push_back(input.restShape[distant_vertex]);
        covered.insert(distant_vertex);
        for (int i = 0; i < input.neighbor[distant_vertex].size(); ++i) {
            int neighbor = input.neighbor[distant_vertex][i].first;
            if (neighbor < 0) {
                continue;
            }
            output.skinIndex[neighbor * num_indices] = output.num_joints;
            covered.insert(neighbor);
        }
        updateJointTransformProc(output, transformType, input, output.num_joints);
        ++output.num_joints;
        output.num_joints = bindVertexToJoint(output, input);

        sprintf(buf, "Added to Vertex %d", distant_vertex);
    }
    detectNeighborClusters(input, output);
}

static void initialize(Output& output, Input& input)
{
    PyArrayObject* initPos = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 0));
    PyArrayObject* shapeSample = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 1));
    PyArrayObject* neighborVertices = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 2));
    const long numJoints = PyInt_AsLong(PyTuple_GET_ITEM(args, 3));
    const long numIndices = PyInt_AsLong(PyTuple_GET_ITEM(args, 4));
    const long numRings = PyInt_AsLong(PyTuple_GET_ITEM(args, 5));
    const long numVertices = static_cast<long>(initPos->dimensions[0]);
    const long numSamples = static_cast<long>(shapeSample->dimensions[0]);
    // allocation
    input.numVertices = numVertices;
    input.numSamples = numSamples;
    input.numJoints = numJoints;
    input.numRings = numRings;
    input.rest_shape.points.resize(numVertices);
    input.sample.resize(numSamples);
    input.neighbor.resize(numVertices);

    output.joints.resize(numJoints);
    for (auto& jd : output.joints) {
        jd.weights.resize(numVertices);
        jd.matrix.resize(numSamples);
    }

    output.skinMatrix = std::vector<MTransformationMatrix>(numSamples * numJoints, MTransformationMatrix::identity);
    output.restJointPos = std::vector<MPoint>(numJoints);
    output.vertCluster = std::vector<std::vector<int>>(numVertices);

    // size normalization
    MVector centroid(0, 0, 0);
    MVector minPos(1.0e10, 1.0e10, 1.0e10), maxPos(-1.0e10, -1.0e10, -1.0e10);
    for (int v = 0; v < numVertices; ++v) {
        MVector p(GET_DOUBLE2(initPos, v, 0),
            GET_DOUBLE2(initPos, v, 1),
            GET_DOUBLE2(initPos, v, 2));
        input->restShape[v] = p;
        centroid += p;
        for (int i = 0; i < 3; ++i) {
            minPos[i] = std::min(minPos[i], p[i]);
            maxPos[i] = std::max(maxPos[i], p[i]);
        }
    }
    centroid /= numVertices;
    double scale = std::max(std::max(maxPos.x - minPos.x, maxPos.y - minPos.y), maxPos.z - minPos.z);
    MMatrix m = MMatrix::identity;
    m(0, 0) = m(1, 1) = m(2, 2) = 1.0 / scale;
    m(3, 0) = -centroid.x / scale;
    m(3, 1) = -centroid.y / scale;
    m(3, 2) = -centroid.z / scale;
    input->normalizer = m;
    // normalized samples
    for (int v = 0; v < numVertices; ++v) {
        input->restShape[v] = input->restShape[v] * input->normalizer;
        for (int s = 0; s < numSamples; ++s) {
            MPoint p(GET_DOUBLE3(shapeSample, s, v, 0),
                GET_DOUBLE3(shapeSample, s, v, 1),
                GET_DOUBLE3(shapeSample, s, v, 2));
            input->sample[s * numVertices + v] = p * input->normalizer;
        }
    }

    // one-ring neighbor [Le and Deng 2014]
    for (int v = 0; v < numVertices; ++v) {
        double lv = 0.0;
        for (int i = 0; i < neighborVertices->dimensions[1]; ++i) {
            long n = GET_LONG2(neighborVertices, v, i);
            if (n < 0) {
                continue;
            }
            double len = (input->restShape[v] - input->restShape[n]).length();
            if (len > 0) {
                // [Le and Deng 2014]
                double dsqsum = 0;
                for (int s = 0; s < input->numSamples; ++s) {
                    float diff = (input->restShape[v] - input->restShape[n]).length()
                        - (input->sample[s * numVertices + v] - input->sample[s * numVertices + n]).length();
                    dsqsum += diff * diff;
                }
                dsqsum += 1.0e-10;
                double dvn = 1.0 / std::sqrt(dsqsum / input->numSamples);
                input->neighbor[v].push_back(std::make_pair(n, dvn));
                lv += dvn;
            }
        }
        for (int n = 0; n < input->neighbor[v].size(); ++n) {
            input->neighbor[v][n].second /= -lv;
        }
    }
}
#endif

} // namespace ssds


#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>

#include <iostream>
#include <vector>
#include <algorithm>

#include <Eigen/Core>
#include <Eigen/Geometry>
// From libshell by Etienne Vouga
double angle(const Eigen::Vector3d &v, const Eigen::Vector3d &w, const Eigen::Vector3d &axis,
    Eigen::Matrix<double, 1, 6> *derivative, // v, w
    Eigen::Matrix<double, 6, 6> *hessian
)
{
  const auto crossMatrix = [](Eigen::Vector3d v)->Eigen::Matrix3d
  {
      Eigen::Matrix3d ret;
      ret << 0, -v(2), v(1),
          v(2), 0, -v(0),
          -v(1), v(0), 0;
      return ret;
  };

  // This is a bad idea, if v and w are colinear, this derivatives below still
  // make sense but this will make axis 0 and nuke everything.
  //// This is unnecessary if the caller would promise that axis is unit and
  //// orthogonal to v and w
  const Eigen::Vector3d vcrossw = v.cross(w);
  //const Eigen::Vector3d axis = ((vcrossw.dot(in_axis)>0?1:-1) * vcrossw).eval().normalized();
  //std::cout<<"in: "<<in_axis.transpose()<<std::endl;
  //std::cout<<"vcrossw: "<<vcrossw.transpose()<<std::endl;
  //std::cout<<"ef: "<<axis.transpose()<<std::endl;

    double theta = 2.0 * atan2((vcrossw.dot(axis)), v.dot(w) + v.norm() * w.norm());

    if (derivative)
    {
        derivative->segment(0, 3) = -axis.cross(v) / v.squaredNorm();
        derivative->segment(3, 3) = axis.cross(w) / w.squaredNorm();
    }
    if (hessian)
    {
        hessian->setZero();
        hessian->block(0, 0, 3, 3) += 2.0 * (axis.cross(v))*v.transpose() / v.squaredNorm() / v.squaredNorm();
        hessian->block(3, 3, 3, 3) += -2.0 * (axis.cross(w))*w.transpose() / w.squaredNorm() / w.squaredNorm();
        hessian->block(0, 0, 3, 3) += -crossMatrix(axis) / v.squaredNorm();
        hessian->block(3, 3, 3, 3) += crossMatrix(axis) / w.squaredNorm();

        double sigma = 1.0;
        if (v.cross(w).dot(axis) < 0)
            sigma = -1.0;

        double vwnorm = v.cross(w).norm();
        if (vwnorm > 1e-8)
        {
            Eigen::Matrix3d da = sigma * (1.0 / vwnorm * Eigen::Matrix3d::Identity() - 1.0 / vwnorm / vwnorm / vwnorm * (v.cross(w)) * (v.cross(w)).transpose());
            hessian->block(0, 0, 3, 3) += crossMatrix(v) / v.squaredNorm() * da * -crossMatrix(w);
            hessian->block(3, 0, 3, 3) += crossMatrix(v) / v.squaredNorm() * da * crossMatrix(v);
            hessian->block(0, 3, 3, 3) += -crossMatrix(w) / w.squaredNorm() * da * -crossMatrix(w);
            hessian->block(3, 3, 3, 3) += -crossMatrix(w) / w.squaredNorm() * da * crossMatrix(v);
        }
    }

    return theta;
}

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  Eigen::MatrixXd V,W,A;
  mexErrMsgTxt(nrhs>=2,"nrhs should be == 2");
  parse_rhs_double(prhs+0,V);
  parse_rhs_double(prhs+1,W);
  mexErrMsgTxt(V.cols()==W.cols(),"dims should be the same");
  mexErrMsgTxt(V.rows()==W.rows(),"dims should be the same");
  const int n = V.rows();
  const int dim = V.cols();
  if(dim == 3)
  {
    mexErrMsgTxt(nrhs>=3,"nrhs should be == 3");
    parse_rhs_double(prhs+2,A);
  }else if(dim == 2)
  {
    V.conservativeResize(V.rows(),3);
    W.conservativeResize(W.rows(),3);
    V.col(2).setZero();
    W.col(2).setZero();
    A.resize(n,3);
    A.col(0).setConstant(0);
    A.col(1).setConstant(0);
    A.col(2).setConstant(1);
  }
  Eigen::MatrixXd theta(n,1);
  Eigen::MatrixXd dthetadV(n,dim);
  Eigen::MatrixXd dthetadW(n,dim);
  Eigen::MatrixXd d2thetadV2(n,dim==2?3:6);
  Eigen::MatrixXd d2thetadW2(n,dim==2?3:6);
  Eigen::MatrixXd d2thetadVW(n,dim==2?3:6);
  const Eigen::MatrixXi voigt2 =
    (Eigen::MatrixXi(3,2)<<0,0,1,1,0,1).finished();
  const Eigen::MatrixXi voigt = (dim==2? voigt2 :
    (Eigen::MatrixXi(6,2)<<0,0,1,1,2,2,1,2,0,2,0,1).finished());
  for(int i = 0;i<n;i++)
  {
    Eigen::Matrix<double, 1, 6> derivative;
    Eigen::Matrix<double, 6, 6> hessian;
    theta(i) = angle(
        V.row(i).transpose(),W.row(i).transpose(),A.row(i).transpose(),
        &derivative,&hessian);
    dthetadV.row(i) = derivative.segment(0,dim);
    dthetadW.row(i) = derivative.segment(3,dim);
    for(int b = 0;b<voigt2.rows();b++)
    {
      const int bi = voigt2(b,0);
      const int bj = voigt2(b,1);
      const auto & block = hessian.block(bi*3,bj*3,dim,dim);
      for(int v = 0;v<voigt.rows();v++)
      {
        const int vi = voigt(v,0);
        const int vj = voigt(v,1);
        const double h = block(vi,vj);
        switch(b)
        {
          case 0: d2thetadV2(i,v) = h; break;
          case 1: d2thetadW2(i,v) = h; break;
          case 2: d2thetadVW(i,v) = h; break;
        }
      }
    }
  }

  switch(nlhs)
  {
    case 6:
      prepare_lhs_double(d2thetadVW,plhs+5);
    case 5:
      prepare_lhs_double(d2thetadW2,plhs+4);
    case 4:
      prepare_lhs_double(d2thetadV2,plhs+3);
    case 3:
      prepare_lhs_double(dthetadW,plhs+2);
    case 2:
      prepare_lhs_double(dthetadV,plhs+1);
    case 1:
      prepare_lhs_double(theta,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


// BLAS level 3 -- complex numbers

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

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

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (2, 2);
  a (0, 0) = cmplx_t (1., 0.);
  a (0, 1) = cmplx_t (2., 0.);
  a (1, 0) = cmplx_t (3., 0.);
  a (1, 1) = cmplx_t (4., 0.);
  print_m (a, "A");
  cout << endl;

  m_t b (2, 3);
  b (0, 0) = cmplx_t (1., 0.);
  b (0, 1) = cmplx_t (2., 0.);
  b (0, 2) = cmplx_t (3., 0.);
  b (1, 0) = cmplx_t (1., 0.);
  b (1, 1) = cmplx_t (2., 0.);
  b (1, 2) = cmplx_t (3., 0.);
  print_m (b, "B");
  cout << endl;

  m_t c (2, 3);

  // c = a b
  atlas::gemm (a, b, c);
  print_m (c, "A B");
  cout << endl;

  a (0, 0) = cmplx_t (0., 1.);
  a (0, 1) = cmplx_t (0., 2.);
  a (1, 0) = cmplx_t (0., 3.);
  a (1, 1) = cmplx_t (0., 4.);
  print_m (a, "A");
  cout << endl;

  // c = a b
  atlas::gemm (CblasNoTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  // c = a^T b
  atlas::gemm (CblasTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A^T B");
  cout << endl;

  // c = a^H b
  atlas::gemm (CblasConjTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A^H B");

  cout << endl;

}


#include "model-magick/MeshStatsCalculator.h"

#include <cmath>

#include <Eigen/Geometry>

namespace ModelMagick
{

using namespace std;
using namespace oneapi::tbb;
using namespace oneapi::tbb::flow;

Eigen::VectorXf calculateSolidAnglesForPlanarTriangles(
    const Mesh& mesh,
    const Eigen::RowVector3f& queryPoint)
{
    // https://igl.ethz.ch/projects/winding-number/robust-inside-outside-segmentation-using-generalized-winding-numbers-siggraph-2013-jacobson-et-al.pdf
    // Eq. 6.

    Eigen::VectorXf solidAngles;
    const int m = mesh.numFaces();
    solidAngles.resize(m);
    parallel_for(
        blocked_range<size_t>(0, m),
        [&solidAngles, &mesh, &queryPoint](const blocked_range<size_t>& r) {
            for (size_t i = r.begin(); i != r.end(); ++i) {
                // triangle points in 3x3 matrix
                //       [a1, a2, a3]
                // abc = [b1, b2, b3]
                //       [c1, c2, c3]
                const Eigen::Matrix3f abc
                    = mesh.vertices(mesh.indices(i, Eigen::all), Eigen::all).rowwise() - queryPoint;
                // row norms in column vector
                const Eigen::Vector3f normAbc = abc.rowwise().norm();
                const float d0 = normAbc.prod();
                const float d1 = abc.row(0).dot(abc.row(1)) * normAbc(2);
                const float d2 = abc.row(1).dot(abc.row(2)) * normAbc(0);
                const float d3 = abc.row(0).dot(abc.row(2)) * normAbc(1);
                solidAngles(i) = 2 * std::atan2(abc.determinant(), d0 + d1 + d2 + d3);
            }
        });
    return solidAngles;
}

Eigen::VectorXf calculateGeneralizedWindingNumber(
    const Mesh& mesh,
    const Eigen::MatrixX3f& queryPoints)
{
    Eigen::VectorXf windingNumbers;
    const int numQuery = queryPoints.rows();
    windingNumbers.resize(numQuery, 1);
    parallel_for(
        blocked_range<size_t>(0, numQuery),
        [&windingNumbers, &mesh, &queryPoints](const blocked_range<size_t>& r) {
            for (size_t i = r.begin(); i != r.end(); ++i) {
                windingNumbers(i)
                    = 0.25 * M_1_PI
                      * calculateSolidAnglesForPlanarTriangles(mesh, queryPoints.row(i)).sum();
            }
        });
    return windingNumbers;
}

// oneapi::tbb::flow::function_node<Mesh, Eigen::MatrixX3f, Eigen::VectorXf>
// createGeneralizedWindingNumberCalculator(
//     oneapi::tbb::flow::graph& graph,
//     std::size_t concurrency = oneapi::tbb::flow::unlimited);

}  // namespace ModelMagick


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

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

// Free energy, energy, and specific heat of triangular lattice Ising model

// reference: R. M. F. Houtappel, Physica 16, 425 (1950)

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/simpson.hpp>
#include "common.hpp"

namespace ising {
namespace free_energy {
namespace triangular {

namespace {

template<typename T, typename FVAR>
struct functor {
  functor(T Ja, T Jb, T Jc, FVAR beta) {
    using std::cosh; using std::sinh;
    T pi = boost::math::constants::pi<T>();
    c_ = 1 / (8 * pi * pi);
    sh2a_ = sinh(2 * beta * Ja);
    sh2b_ = sinh(2 * beta * Jb);
    sh2c_ = sinh(2 * beta * Jc);
    cshabc_ = cosh(2 * beta * Ja) * cosh(2 * beta * Jb) * cosh(2 * beta * Jc)
      + sh2a_ * sh2b_ * sh2c_;
  }
  FVAR operator()(T t1, T t2) const {
    using std::cos; using std::log;
    return c_ * log(cshabc_ - sh2a_ * cos(t1) - sh2b_ * cos(t2) - sh2c_ * cos(t1 + t2));
  }
  T c_;
  FVAR sh2a_, sh2b_, sh2c_, cshabc_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Ja, T Jb, T Jc, FVAR beta) {
  return functor<T, FVAR>(Ja, Jb, Jc, beta);
}

}

template<typename T, typename U>
inline U infinite(T Ja, T Jb, T Jc, U beta) {
  const unsigned long max_n = 1 << 16;
  typedef T real_t;
  if (Ja * Jb * Jc <= 0)
    throw(std::invalid_argument("Ja * Jb * Jc should be positive"));
  if (beta <= 0)
    throw(std::invalid_argument("beta should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  auto logZ = log(real_t(2)) +
    standards::simpson_2d(func(Ja, Jb, Jc, beta), real_t(0), real_t(0), 2*pi, 2*pi, 8, 8);
  auto pz = logZ;
  auto pe = abs(beta * beta * logZ.derivative(2) / logZ);
  for (unsigned long n = 16; n <= max_n; n *= 2) {
    logZ = log(real_t(2)) +
      standards::simpson_2d(func(Ja, Jb, Jc, beta), real_t(0), real_t(0), 2*pi, 2*pi, n, n);
    auto pn = abs(beta * beta * (logZ - pz).derivative(2) / logZ);
    if (pn < 2 * std::numeric_limits<real_t>::epsilon() || pn > pe) break;
    if (n == max_n) std::cerr << "Warning: integration not converge with n = " << max_n << std::endl;
    pz = logZ;
    pe = pn;
  }
  return - logZ / beta;
}

} // end namespace triangular
} // end namespace free_energy
} // end namespace ising


// Statistical Computing //

// Load typical packages
#include<cmath>
#include<iostream>
#include<iomanip>

// Typiccal C libraries
#include<cmath>
#include<cstdlib>

// C++ Data Structures
#include<set>
#include<map>

// Include Eigen Package for Matrices
#include <Eigen/Core>

// To calculate Time
#include<ctime>

// Load Matrix615 header file to read in from file
#include "Matrix615.h"

using namespace std;
// we avoid using namespace Eigen to be able to clearly see where we are calling the Eigen Package

// Driver Code
int main(int argc, char* argv[]) {

	// Steps corresponding to the commented code is associated with the following documents:

	// Tentative Read Matrix using Matrix615.h

	Matrix615<double> read_Design; // throw-away Matrices to read in data
	Matrix615<double> read_Y;
	Matrix615<double> read_ID;

	// Read the data in
	read_Design.readFromFile(argv[3]); // t
	read_Y.readFromFile(argv[2]);
	read_ID.readFromFile(argv[1]);

	// Step (1): Store the design matrix, response, and Id into an Eigen Matrix
	Eigen::MatrixXd	design_X;
	Eigen::MatrixXd outcome_Y;

	read_Design.cloneToEigen(design_X);
	read_Y.cloneToEigen(outcome_Y);

	// cout << Weights.rows() << endl << endl;
	// cout << Weights << endl;
	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	// Weights.setOnes();

	// cout << "argc is " << argc << endl;

	if (int (argc) == 5) {
		Matrix615<double> read_Weights;
		read_Weights.readFromFile(argv[4]);

		Eigen::MatrixXd Weights;
		read_Weights.cloneToEigen(Weights);
	}

	else {
		Weights.setOnes();
	}

	// Step (2): Count number of unique ID's, patients
	Eigen::MatrixXd ID_Mat;
	read_ID.cloneToEigen(ID_Mat);

	std::set<int> unique_ID; // to dynamically count n
	std::map<int, int> m_map; // to dynamically count m
	std::map<int, double> weight_map;

	for (int i = 0; i < int(ID_Mat.rows()); ++i) {
		unique_ID.insert(  ID_Mat(i,0) );
		m_map[ID_Mat(i, 0)] += 1;
		weight_map[ID_Mat(i, 0)] = Weights(i, 0);
	}

	// int n = int( unique_ID.size() );

	// cout << "n is << " << n << endl;

	// cout << "size of the id to m map is " << m_map.size() << endl;

	// cout << "m of 1st patient is " << m_map[1] << " " << m_map[100] << " " << m_map[2] << endl << endl;
	// cout << "size of weight map " << weight_map.size() << endl;

	// q = (p + 1) : # number of paramters to be estimated using GEE
	int q = int ( design_X.cols() );

	// cout << "q is " << q << endl;

	// Correlation Structure

	// Step : Weights

	// Step (3): Initialize betas *********** Should we incorporate the beta's that come from a fast linear regression or no?

	Eigen::MatrixXd Betas_new(q , 1); // dimensions (p+1)  x 1
	Betas_new.setZero();

	// cout << "Betas ***********************" << endl;
	// cout << Betas_new << endl << endl;

	// Step (4): Initialize rho and phi (rho: off diagonals of correlation structure & phi: )
	double rho = 0.0;
	// double phi = 0.0;

	// Step (5): Set up variables for iteration convergence check

	// Should we make this dynamic?

	double diff_beta = 1; // updated difference in beta estimation {beta (new) - beta (old)}

	double diff_threshold = 0.00000010; // threshold for the update difference in betas

	int iteration_threshold = 1000; // threshold on how many iterations there will be

	// Step (6): Initialize the iteration
	int iteration_count = 0;

	// Step (7): Assign appropriate value to n*
	double n_star_sum = 0.0;
	double m_sum = 0.0;

	// calculate n_star_sum

	// for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
	// 	n_star_sum = n_star_sum + (0.5) * double(m_map[*iter]) * double(double(m_map[*iter]) - double(1.0));
	// }

	// Step (8): Start the GEE Estimation

	Eigen::MatrixXd Betas_updated = Betas_new;

	Eigen::MatrixXd sandwhich_Mat;

	Eigen::MatrixXd tempGI(q, q);

	// // cout << Betas_updated << endl << endl;

	int before_EE = clock();

	// Step (10)
	while ( (diff_beta > diff_threshold) && (iteration_count < iteration_threshold) ) {
		// (1)
		Betas_updated = Betas_new;

		// (2) Vector EE ((px1) x 1) or (q x 1)
		Eigen::MatrixXd EE(q, 1);
		EE.setZero();

		// (3) Matrix GI (q x q)
		Eigen::MatrixXd GI(q, q);
		GI.setZero();

		// // (4) Matrix G ( (p+1) x (p+1) )
		// // Meat of the Sandwhich Estimator
		// Eigen::MatrixXd G(q , q);
		// G.setZero();

		// (5) Initialize phi(sum) and tau(sum)
		long double phi_sum = 0.0;
		long double tau_sum = 0.0;

		// (6) Initialize indexes for block matrix multiplications
		int start = 0;
		int end = -1;

		// inner loop, we will loop using the unique_ID set (size = n)
		// Therefore, it loops n times
		for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {

			int m = m_map[*iter];
			start = end + 1;
			end = start + m - 1;

			n_star_sum = n_star_sum + (0.5) * double(m) * double(double(m) - 1.0);
			m_sum = m_sum + double(m);

			// cout << "start is " << start << endl;
			// cout << "end is " << end << endl << endl;

			// assign mu for ith observation
			// Eigen.block(starting_row = , starting_col = , dim_row = , dim_col = )
			Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

			// cout << "mu_i is here " << endl;
			// cout << mu_i << endl << endl;

			// assign r_i (mi x 1)
			Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

			// cout << "r_i is here " << endl;
			// cout << r_i << endl;

			// add r_i ^2 to phi_sum from the ith observation
			for (int j = 0; j < int(m); ++j) {
				phi_sum = phi_sum + (r_i(j,0) * r_i(j,0));
			}

			// cout << "phi_sum is " << phi_sum << endl;

			// add to tau_sum
			for (int j = 0; j < ( int(m) - 1 ); ++j) {
				for (int k = (j + 1); k < int(m); ++k) {
					// cout << " j is " << j << " and k is " << k << endl;
					tau_sum = tau_sum + (r_i(j,0) * r_i(k,0));
				}
			}

			// cout << "tau_sum is " << tau_sum << endl;

			// create R matrix (mi x mi) for each observation
			Eigen::MatrixXd R(m, m);
			R.setConstant(rho);
			for (int d = 0; d < int(m); ++d) {
				R(d, d) = 1;
			}

			// cout << R << endl << endl << endl;
			// update EE ((p+1) x 1)
			EE = EE + ((design_X.block(start, 0, m, q).transpose()) * (R.inverse() * (r_i) )); // weight_map

			// update GI ((p+1) x (p+1)) or (q x q)
			GI = GI + ((design_X.block(start, 0, m, q).transpose()) * R.inverse() *  design_X.block(start, 0, m, q));

			// update G ((p+1) x (p+1)) or (q x q)
			// G = G + ((design_X.block(start, 0, m, q).transpose()) * ( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));

		}

		// Update beta using Newton Raphson method
		Betas_new = Betas_updated + (GI.inverse() * EE);

		// calculate the difference in the betas
		diff_beta = (Betas_new - Betas_updated).norm();
		// diff_beta = (Betas_new - Betas_updated).array().abs().sum();

		// update rho
		// cout << "tau_sum is " << tau_sum << endl;
		// cout << "phi_sum is " << phi_sum << endl;

		rho = ( (( double(m_sum) - double(q) ) * tau_sum) / ( (double(n_star_sum) - double(q) ) * phi_sum) );

		// cout << "rho is " << rho << endl << endl;

		// sandwhich_Mat = GI.inverse() * G * GI.inverse();
		tempGI = GI;

		iteration_count += 1;

	}

	// (4) Matrix G ( (p+1) x (p+1) )
	// Meat of the Sandwhich Estimator
	Eigen::MatrixXd G(q , q);
	G.setZero();

	// To calculate the sandwhich estimator
	// for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
	// 	n_star_sum = n_star_sum + (0.5) * double(m_map[*iter]) * double(double(m_map[*iter]) - double(1.0));
	// }

	int start = 0;
	int end = -1;

	for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
		int m = m_map[*iter];
		start = end + 1;
		end = start + m - 1;

		Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

		// cout << "mu_i is here " << endl;
		// cout << mu_i << endl << endl;

		// assign r_i (mi x 1)
		Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

		Eigen::MatrixXd R(m, m);

		R.setConstant(rho);

		for (int d = 0; d < int(m); ++d) {
			R(d, d) = 1;
		}

		// update G ((p+1) x (p+1)) or (q x q)
		G = G + ((design_X.block(start, 0, m, q).transpose()) * ( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));
	}

	sandwhich_Mat = tempGI.inverse() * G * tempGI.inverse();

	int after_EE = clock();

	// cout << " n start sum ended up being " << n_star_sum << endl;
	// cout << "m sum ended up being " << m_sum << endl;
	cout << "**************** GEE Results ****************" << endl;
	cout << endl << endl << "Iteration count at: " << iteration_count << endl;

	cout << "The correlation parameter: " << rho << endl;

	cout << endl << "Estimated Betas are: " << endl;
	cout << Betas_new.transpose() << endl << endl << endl;

	// cout << "The sandwhich matrix is: " << endl << endl;
	// cout << sandwhich_Mat << endl << endl;

	cout << "The beta Robust S.E.'s are: " << endl << endl;
	cout << sandwhich_Mat.diagonal().array().sqrt() << endl << endl;

	cout << " Estimated convergence time for EE is: " << endl;
	cout << " Time (secs): " << (after_EE - before_EE) / double(CLOCKS_PER_SEC) << endl;

	return 0;

}


#pragma once

#include <numeric>
#include <type_traits>
#include <utility>
#include <vector>

#include <Eigen/Dense>

namespace pcv
{

template<class NumericType>
double getLineModelError(
    const Eigen::Vector2d &model,
    const Eigen::Matrix<NumericType, 2, 1> point)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    double yEstimate = (1.0 - model[0]*point.x())/model[1];

    return std::pow(yEstimate-point.y(), 2);
}

template<class NumericType>
Eigen::Vector2d
findLineModelFromPoints(
    std::vector<Eigen::Matrix<NumericType, 2, 1>> points)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    if (points.size() < 2)
    {
        throw std::runtime_error("There must be at least the minimum number of "
                                 "data points required by modeling function.");
    }

    Eigen::Matrix<double, 2, 2> linearEquationsLhs;
    linearEquationsLhs << points[0].x(), points[0].y(),
                          points[1].x(), points[1].y();

    Eigen::Vector2d linearEquationsRhs;
    linearEquationsRhs << 1, 1;

    Eigen::Vector2d model =
        linearEquationsLhs.colPivHouseholderQr().solve(linearEquationsRhs);

    return model;
}

} // end namespace pcv


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearloadvector.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();
  double area;
  Eigen::Matrix midpoints(2, num_nodes);

  //====================
  // Your code goes here
  // supply a builder type for the element vector
  // arising from the linear form
  // use the composite edge midpoint rule with the local definition
  switch (num_nodes){
    case 3:{
      area = 0.5 *(vertices(0,1)-vertices(0,0))*((vertices(1,2)-vertices(1,0))-(vertices(1,1)-vertices(1,0))*(vertices(0,2)*vertices(0,0)));
      midpoints << vertices(0,0)+vertices(0,1),
      vertices(0,1)+vertices(0,1),
      vertices(0,0)+vertices(0,2),
      vertices(1,0)+vertices(1,1),
      vertices(1,1)+vertices(1,2),
      vertices(1,0)+vertices(1,2);

    }
  } case 4:{
    area = (vertices(0,1)-vertices(0,0))*(vertices(1,3)-vertices(1,0));
    midpoints << vertices(0,0)+vertices(0,1),
    vertices(0,1)+vertices(0,2),
    vertices(0,2)+vertices(0,3),
    vertices(0,3)+vertices(0,0),
    vertices(1,0)+vertices(1,1),
    vertices(1,1)+vertices(1,2),
    vertices(1,2)+vertices(1,3),
    vertices(1,3)+vertices(1,0),
  }

  Eigen::VectorXd fal(num_nodes);
  for (int i =0; i < num_nodes; i++){
    fal[i] = f(midpoints.col[i]);
  }

  for (int k=0; k<num_nodes; k++){
    elem_vec[k] = 0.5*fal[k];
    elem_vec[(k+1)% num_nodes] = 0.5*fal[k];
  }

  elem_vec *= area/num_nodes;

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

  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


/*
 * Copyright 2015-2017 Guillermo Frontera <guillermo.frontera@upm.es>
 *
 * 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 "algorithms_2d.h"

#include <boost/geometry.hpp>

#include <log/logger.h>
#include <exc/exception.h>

#include "polygon_traits.h"

using namespace ues::geom;

const std::string component_name = "2D Geometric Algorithms";

point<2> ues::geom::segment_intersection ( const point<2> & s1p1,
                                           const point<2> & s1p2,
                                           const point<2> & s2p1,
                                           const point<2> & s2p2,
                                           const ues::math::numeric_type & epsilon )
{
    point<2> result;
    if ( check_segment_intersection ( s1p1, s1p2, s2p1, s2p2, result, epsilon ) )
    {
        return result;
    }
    else
    {
        std::ostringstream error;
        error << "No intersection found between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " )";
        throw ues::exc::exception ( error.str(), UES_CONTEXT );
    }
}

bool ues::geom::check_segment_intersection ( const point<2> & s1p1,
                                             const point<2> & s1p2,
                                             const point<2> & s2p1,
                                             const point<2> & s2p2,
                                             point<2> & result,
                                             const ues::math::numeric_type & epsilon ) noexcept
{
    ues::log::logger lg;

    const ues::math::numeric_type & x1 = s1p1.get_x(), & y1 = s1p1.get_y(),
    & x2 = s1p2.get_x(), & y2 = s1p2.get_y(),
    & x3 = s2p1.get_x(), & y3 = s2p1.get_y(),
    & x4 = s2p2.get_x(), & y4 = s2p2.get_y();

    ues::math::numeric_type denominator = ( x1 - x2 ) * ( y3 - y4 ) - ( y1 - y2 ) * ( x3 - x4 );

    if ( denominator != 0 )
    {
        ues::math::numeric_type denominator_inv = 1 / denominator;
        ues::math::numeric_type first_factor = ( x1 * y2 - y1 * x2 );
        ues::math::numeric_type second_factor = ( x3 * y4 - y3 * x4 );
        ues::math::numeric_type x, y;
        x = ( first_factor * ( x3 - x4 ) - ( x1 - x2 ) * second_factor ) * denominator_inv;
        y = ( first_factor * ( y3 - y4 ) - ( y1 - y2 ) * second_factor ) * denominator_inv;

        result = { x, y };

        if ( x + epsilon >= std::max ( std::min ( x1, x2 ), std::min ( x3, x4 ) ) &&
                x - epsilon <= std::min ( std::max ( x1, x2 ), std::max ( x3, x4 ) ) &&
                y + epsilon >= std::max ( std::min ( y1, y2 ), std::min ( y3, y4 ) ) &&
                y - epsilon <= std::min ( std::max ( y1, y2 ), std::max ( y3, y4 ) ) )
        {

            if ( lg.min_level() <= ues::log::TRACE_LVL )
            {
                ues::log::event e ( ues::log::TRACE_LVL, component_name, "Segment intersection found" );
                e.message() << "Intersection between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " ) is " << result << ".\n";
                lg.record ( std::move ( e ) );
            }

            return true;
        }
    }

    if ( lg.min_level() <= ues::log::TRACE_LVL )
    {
        ues::log::event e ( ues::log::TRACE_LVL, component_name, "Segment intersection not found" );
        e.message() << "No intersection found between segments ( " << s1p1 << ", " << s1p2 << " ) and ( " << s2p1 << ", " << s2p2 << " ).\n";
        if ( denominator != 0 )
        {
            e.message() << "Lines containing segments intersect at " << result << ", which is out of the boundaries of the segments.\n";
        }
        else
        {
            e.message() << "Segments are parallel.\n";
        }
        lg.record ( std::move ( e ) );
    }

    return false;
}

ues::math::numeric_type ues::geom::point_to_segment_distance ( const point<2> & origin_point,
                                                    const point<2> & segment_point1,
                                                    const point<2> & segment_point2,
                                                    const ues::math::numeric_type & angle,
                                                    const ues::math::numeric_type & epsilon )
{
    point<2> s1 = segment_point1;
    point<2> s2 = segment_point2;

    ues::math::matrix transform = rotation_matrix_2d ( -angle ) *
                       translation_matrix_2d ( -origin_point.get_x(), -origin_point.get_y() );
    s1.transform ( transform );
    s2.transform ( transform );

    if ( ( s1.get_y() > epsilon && s2.get_y() > epsilon ) ||
            ( s1.get_y() < -epsilon && s2.get_y() < -epsilon ) )
    {
        std::ostringstream out;
        out << "The angle provided (" << ( angle > ues::math::pi ? angle - 2 * ues::math::pi : angle ) << " rad) from point " << origin_point;
        out << " does not intersect segment " << segment_point1 << "-" << segment_point2;

        throw ues::exc::exception ( out.str(), UES_CONTEXT );
    }

    if ( std::abs ( s1.get_y() ) < ues::math::epsilon && std::abs ( s2.get_y() ) < ues::math::epsilon )
    {
        // The segment is perpendicular.
        return std::min ( s1.get_x(), s2.get_x() );
    }

    ues::math::numeric_type w1, w2;
    if ( s1.get_y() > 0 && s2.get_y() > 0 )
    {
        if ( s1.get_y() > s2.get_y() )
        {
            w1 = 0;
            w2 = 1;
        }
        else
        {
            w1 = 1;
            w2 = 0;
        }
    }
    else if ( s1.get_y() < 0 && s2.get_y() < 0 )
    {
        if ( s1.get_y() > s2.get_y() )
        {
            w1 = 1;
            w2 = 0;
        }
        else
        {
            w1 = 0;
            w2 = 1;
        }
    }
    else
    {
        ues::math::numeric_type inv_segment_height = 1 / std::abs ( s1.get_y() - s2.get_y() );
        w1 = std::abs ( s2.get_y() ) * inv_segment_height;
        w2 = std::abs ( s1.get_y() ) * inv_segment_height;
    }

    ues::math::numeric_type intersection_distance = w1 * s1.get_x() + w2 * s2.get_x();

    // Return distance from point to segment.
    return intersection_distance;
}

bool ues::geom::check_polygon_intersection ( const polygon & poly1, const polygon & poly2 ) noexcept
{
    return boost::geometry::intersects ( poly1, poly2 );
}

ues::math::matrix ues::geom::translation_matrix_2d ( const ues::math::numeric_type & x, const ues::math::numeric_type & y ) noexcept
{
    // A 3x3 matrix for homogeneous 2D transformation
    ues::math::matrix::fixed<3, 3> transform = { 1, 0, 0, 0, 1, 0, x, y, 1 };
    return transform;
}

ues::math::matrix ues::geom::rotation_matrix_2d ( const ues::math::numeric_type & angle ) noexcept
{

    // Compute the transformation matrix
    ues::math::numeric_type sine = std::sin ( angle );
    ues::math::numeric_type cosine = std::cos ( angle );

    // A 3x3 matrix for homogeneous 2D transformation
    ues::math::matrix::fixed<3, 3> transform = { cosine, sine, 0, -sine, cosine, 0, 0, 0, 1 };
    return transform;
}


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: QR factorization of matrices from ViennaCL or Boost.uBLAS (qr.cpp and qr.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

// activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// include necessary system headers
//
#include <iostream>

//
// ViennaCL includes: We only need the qr-header
//
#include "viennacl/linalg/qr.hpp"

//
// Boost includes
//
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// Testing
//
#include "viennacl/range.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"

//
// A helper function checking the result
//
template <typename MatrixType>
double check(MatrixType const & qr, MatrixType const & ref)
{
  bool do_break = false;
  double max_error = 0;
  for (std::size_t i=0; i<ref.size1(); ++i)
  {
    for (std::size_t j=0; j<ref.size2(); ++j)
    {
      if (qr(i,j) != 0.0 && ref(i,j) != 0.0)
      {
        double rel_err = fabs(qr(i,j) - ref(i,j)) / fabs(ref(i,j) );

        if (rel_err > max_error)
          max_error = rel_err;
      }

      if (qr(i,j) != qr(i,j))
      {
        std::cout << "!!!" << std::endl;
        std::cout << "!!! NaN detected at i=" << i << " and j=" << j << std::endl;
        std::cout << "!!!" << std::endl;
        do_break = true;
        break;
      }
    }
    if (do_break)
      break;
  }
  return max_error;
}

int main (int, const char **)
{
  typedef double               ScalarType;     //feel free to change this to 'double' if supported by your hardware
  typedef boost::numeric::ublas::matrix<ScalarType>        MatrixType;
  typedef boost::numeric::ublas::vector<ScalarType>        VectorType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>        VCLMatrixType;
  typedef viennacl::vector<ScalarType>        VCLVectorType;

  std::size_t rows = 113;   //number of rows in the matrix
  std::size_t cols = 54;   //number of columns

  //
  // Create matrices with some data
  //
  MatrixType ublas_A(rows, cols);
  MatrixType Q(rows, rows);
  MatrixType R(rows, cols);

  // Some random data with a bit of extra weight on the diagonal
  for (std::size_t i=0; i<rows; ++i)
  {
    for (std::size_t j=0; j<cols; ++j)
    {
      ublas_A(i,j) = -1.0 + (i+1)*(j+1)
                     + ( (rand() % 1000) - 500.0) / 1000.0;

      if (i == j)
        ublas_A(i,j) += 10.0;

      R(i,j) = 0.0;
    }

    for (std::size_t j=0; j<rows; ++j)
      Q(i,j) = 0.0;
  }

  // keep initial input matrix for comparison
  MatrixType ublas_A_backup(ublas_A);

  //
  // Setup the matrix in ViennaCL:
  //
  VCLVectorType dummy(10);
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_A, vcl_A);

  //
  // Compute QR factorization of A. A is overwritten with Householder vectors. Coefficients are returned and a block size of 3 is used.
  // Note that at the moment the number of columns of A must be divisible by the block size
  //

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);  //computes the QR factorization

  //
  // A check for the correct result:
  //
  viennacl::linalg::recoverQ(ublas_A, ublas_betas, Q, R);
  MatrixType ublas_QR = prod(Q, R);
  double ublas_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Max rel error (ublas): " << ublas_error << std::endl;

  //
  // QR factorization in ViennaCL using Boost.uBLAS for the panel factorization
  //
  std::cout << "--- Hybrid (default) ---" << std::endl;
  viennacl::copy(ublas_A_backup, vcl_A);
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  //
  // A check for the correct result:
  //
  viennacl::copy(vcl_A, ublas_A);
  Q.clear(); R.clear();
  viennacl::linalg::recoverQ(ublas_A, hybrid_betas, Q, R);
  double hybrid_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Max rel error (hybrid): " << hybrid_error << std::endl;

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#include <random>
#include <Eigen/Dense>
#include "HODLR_Tree.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <squaredeMat.hpp> //squared exponential kernel
#include <squaredeP1Mat.hpp>

using std::normal_distribution;
namespace py = pybind11;

Eigen::MatrixXd predict(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::MatrixXd Xtest, Eigen::VectorXd sig_samps, Eigen::VectorXd rho_samps, Eigen::VectorXd tau_samps, double multiplier, int M, double tol, int nsamps) {
  /*

     Sample a draw of the GP function f*|f, sig, rho, tau, x*.
     Assume a squared exponential Gaussian Process based on
     observed function f at new test points x*.
     Temporarily assumes fit method was called with regression = true.

  */

    // Create the standard normal generator
    normal_distribution<double> norm(0, 1);
    std::mt19937 rng;
    auto r_std_normal = bind(norm, rng);

    int Ntest =  Xtest.rows();
    int N =  X.rows();
    int D =  X.cols();

    // HODLR details
    int n_levels = log(N / M) / log(2);
    bool is_sym = true;
    bool is_pd  = true;

    double tau;
    double rho;
    double sig;

    double sigsq;
    double tmpSSR;

    // Allocate space for output
    Eigen::MatrixXd fstarsamp(nsamps, Ntest);
    Eigen::VectorXd KobsNew(N);

    for (int s = 0; s < nsamps; s++) {

      sig = sig_samps(s);
      tau = tau_samps(s);
      rho = rho_samps(s);

      sigsq = pow(sig, 2.0);

      // HODLR approximation
      SQRExponentialP1_Kernel* L  = new SQRExponentialP1_Kernel(X, N, sig, rho, tau);
      HODLR_Tree* T = new HODLR_Tree(n_levels, tol, L); // With noise (i.e. Sigma + I/tau)
      T->assembleTree(is_sym, is_pd);
      T->factorize();

      for (int i = 0; i < Ntest; i++) {
        Eigen::RowVectorXd Xtest_i = Xtest.row(i);

        // Get covariance between X and Xtest
        for (int j = 0; j < N; j++) {
          Eigen::RowVectorXd tmp = X.row(j) - Xtest_i;
          tmpSSR = 0.0;
          for (int d = 0; d < D; d++) {
            tmpSSR = tmpSSR + pow(tmp(d), 2.0);
          }
          KobsNew(j) = sigsq * exp(- tmpSSR * rho);
        }

        // Get variance at Xtest
        double kNewNew = sigsq + 1e-8;

        // Get posterior mean and variance of f* at point xtest(i)
        double sdstar = pow(kNewNew - (multiplier * KobsNew.transpose() * T->solve(tau * KobsNew))(0, 0), 0.5);
        double mustar = (multiplier * KobsNew.transpose() * T->solve(tau * Y))(0, 0);

        auto normal_samp = r_std_normal();

        fstarsamp(s, i) =  sdstar * normal_samp + mustar;
      }
    }
    return fstarsamp;
}

void predict_module(py::module &m) {
    m.def("predict_f", &predict, "predicted samples of f at new X");
}


#ifndef CLASSICAL_MDS_HPP
#define CLASSICAL_MDS_HPP

#include <vector>
#include <utility>
#include <cassert>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace mathtoolbox
{
    // This function computes low-dimensional embedding by using classical multi-dimensional scaling (MDS)
    // - Input:  A distance (dissimilarity) matrix and a target dimension for embedding
    // - Output: A coordinate matrix whose i-th column corresponds to the embedded coordinates of the i-th entry
    extern inline Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, unsigned dim);

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // This function extract the N-largest eigen values and eigen vectors
    inline void ExtractNLargestEigens(unsigned n, Eigen::VectorXd& S, Eigen::MatrixXd& V)
    {
        // Note: m is the original dimension
        const unsigned m = S.rows();

        // Copy the original matrix
        const Eigen::MatrixXd original_V = V;

        // Sort by eigenvalue
        constexpr double epsilon = 1e-16;
        std::vector<std::pair<double, unsigned>> index_value_pairs(m);
        for (unsigned i = 0; i < m; ++ i) index_value_pairs[i] = std::make_pair(std::max(S(i), epsilon), i);
        std::partial_sort(index_value_pairs.begin(), index_value_pairs.begin() + n, index_value_pairs.end(), std::greater<std::pair<double, unsigned>>());

        // Resize matrices
        S.resize(n);
        V.resize(m, n);

        // Set values
        for (unsigned i = 0; i < n; ++ i)
        {
            S(i)     = index_value_pairs[i].first;
            V.col(i) = original_V.col(index_value_pairs[i].second);
        }
    }

    inline Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, unsigned dim)
    {
        assert(D.rows() == D.cols());
        assert(D.rows() >= dim);
        const unsigned n = D.rows();
        const Eigen::MatrixXd H = Eigen::MatrixXd::Identity(n, n) - (1.0 / static_cast<double>(n)) * Eigen::VectorXd::Ones(n) * Eigen::VectorXd::Ones(n).transpose();
        const Eigen::MatrixXd K = - 0.5 * H * D.cwiseAbs2() * H;
        const Eigen::EigenSolver<Eigen::MatrixXd> solver(K);
        Eigen::VectorXd S = solver.eigenvalues().real();
        Eigen::MatrixXd V = solver.eigenvectors().real();
        ExtractNLargestEigens(dim, S, V);
        const Eigen::MatrixXd X = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(S.cwiseSqrt()) * V.transpose();
        return X;
    }
}

#endif // CLASSICAL_MDS_HPP


#include <bits/stdc++.h>
#include <armadillo>
#include "logistic_regression.hpp"

using namespace std;
//constructor to initialize number of dependent variables
LogisticRegression::LogisticRegression(int num){
    number_of_variables = num;
    weights = arma::mat(number_of_variables + 1, 1);
    weights.randu();
}

arma::mat LogisticRegression::predict(arma::mat X_predict){
    arma::mat x = X_predict;
    x.insert_cols(0, arma::ones<arma::mat>(X_predict.n_rows, 1));
    arma::mat regpred = x * weights;

    for(int i=0;i<X_predict.n_rows;i++)
    {
        regpred(i,0)=(1/(1+exp(-regpred(i,0))));
    }
    return regpred;
}

arma::mat LogisticRegression::getMatrix() {
	return X_mat;
}

void LogisticRegression::train(arma::mat X_train, arma::mat y_train, float alpha, int epochs){
        arma::mat x = X_train;
        x.insert_cols(0, arma::ones<arma::mat>(X_train.n_rows, 1));
        for(int i=1;i<=epochs;i++)
        {
             //updating weights using gradient descent method
            arma::mat delta=(x.t() * (predict(X_train)-y_train));
            weights -= ((alpha * delta)/X_train.n_rows);
            cout<<"Epoch completed = "<<i<<weights<< " " << endl;
        }
}


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Points
Compile: g++ -I $(BOOST_DIR) -Wall -std=c++11  -o 01_sphere 01_sphere.cpp
*/

#include<iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

using namespace boost::geometry;

typedef boost::geometry::model::point
    <
        double, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>
    > spherical_point;

typedef boost::geometry::model::point
    <
        double, 3, boost::geometry::cs::cartesian
    > cartesian_point;

typedef boost::geometry::model::point
    <
        double, 2, boost::geometry::cs::geographic<boost::geometry::degree>
    > geographic_point;

spherical_point amsterdam(4.90, 52.37);
spherical_point paris(2.35, 48.86);

double const earth_radius = 6371; // now km

std::ostream &operator<< (std::ostream &os, const cartesian_point &p)
{
    os << "(" << get<0>(p) << ";" << get<1>(p) << ";" << get<2>(p)<< ")";
    return os;
}
std::ostream &operator<< (std::ostream &os, const geographic_point &p)
{
    os << "(" << get<0>(p) << ";" << get<1>(p) << ")";
    return os;
}

int main(int argc, char **argv)
{
    std::cout << "Distance in miles: " << distance(amsterdam, paris) * earth_radius << std::endl;
    cartesian_point paris3d, amsterdam3d;
    transform(paris, paris3d);
    transform(amsterdam, amsterdam3d);

    std::cout << "Paris 3D (on unit sphere)" << paris3d << std::endl;
    std::cout << "Amsterdam 3D (on unit sphere)" << amsterdam3d << std::endl;
    std::cout << "Distance (drill hole): " << distance(paris3d,amsterdam3d)*earth_radius << " miles" << std::endl;

    // Let us test some intuitive things like north to south pole

    // transformer: a functional trick
    auto tf = [](const spherical_point &p)->cartesian_point { cartesian_point ret; transform(p,ret); return ret;};

    std::cout << "North Pole: " << tf(spherical_point(0,90)) << std::endl;
    std::cout << "South Pole: " << tf(spherical_point(0,-90)) << std::endl;
    std::cout << "Distance: " << distance(tf(spherical_point(0,90)), tf(spherical_point(0,-90)))*earth_radius << std::endl;

    // an "external" cast without recalculating (which is not supported!)
    auto cast = [](const spherical_point &p) {return geographic_point(get<0>(p),get<1>(p));};
    std::cout << "Amsterdam (geo)" << cast(amsterdam) << std::endl;
    std::cout << "Paris (geo)" << cast(paris) << std::endl;
    // Geodesic distance
    std::cout << distance(cast(amsterdam),cast(paris)) << std::endl;

    return 0;
}


#ifndef DISTRIBUTION_HPP
#define DISTRIBUTION_HPP

#include <math.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <random>
#include <vector>

namespace ds {
enum DIRECTION { INCREASE, DECREASE };
class DrProcess {
 public:
  int totallClass;
  Eigen::VectorXd params;
  int sum;
  double alpha;  // para for  new class;

  DrProcess(double alpha) : totallClass(0), alpha(alpha), sum(0) {}

  double calProb(int classNum) {
    if (classNum > totallClass)
      return 0.0;
    else if (classNum == totallClass)
      return alpha / (sum + alpha);
    else
      return params(classNum) / (sum + alpha);
  }
  bool update(int classNum, DIRECTION drc) {
    if (classNum >= totallClass) {
      return false;
    }
    if (drc == INCREASE) {
      params(classNum)++;
      sum++;
      return true;
    } else {
      params(classNum)--;
      sum--;
      if (params(classNum) < 0)
        params(classNum) = 0;
      if (sum < 0)
        sum = 0;
      return true;
    }
  }
  bool newClass() {
    totallClass++;
    params.resize(totallClass);
    params(totallClass - 1) = 1.0;
    sum++;
  }
};
class DirDS {
 private:
  int totallClass;  // num of categories;
  Eigen::VectorXd params;
  Eigen::VectorXd samples;
  std::gamma_distribution<double> gamma;
  std::default_random_engine generator;

 public:
  explicit DirDS(int totallClass)
      : totallClass(totallClass),
        params(Eigen::VectorXd::Zero(totallClass)),
        samples(Eigen::VectorXd::Zero(totallClass)),
        gamma(1.0, 1.0),
        generator() {}

  bool update(int classNum) {
    if (classNum >= totallClass) {
      std::cerr << "more than totallClass\n";
      return false;
    } else {
      ++params(classNum);
      return true;
    }
  }

  Eigen::VectorXd sampling() {
    samples.resize(totallClass);
    for (int i = 0; i < totallClass; i++) {
      gamma.param(std::gamma_distribution<double>::param_type(params(i), 1.0));
      samples(i) = gamma(generator);
    }
    double sum = samples.sum();
    samples = samples / sum;
    return samples;
  }
};

class CatDS {
 private:
  int totallClass;
  Eigen::VectorXd params;
  DirDS dir;

 public:
  CatDS(int totallClass)
      : totallClass(totallClass),
        params(Eigen::VectorXd::Zero(totallClass)),
        dir(totallClass) {}

  bool update(int classNum) {
    dir.update(classNum);
    params = dir.sampling();
    return true;
  }
  double calProb(int classNum) {
    if (classNum >= totallClass)
      return 0.0;
    else {
      return params(classNum);
    }
  }
  void maxPro(int& classNum, double& pro) {
    Eigen::VectorXd::Index maxClass;
    pro = params.maxCoeff(&maxClass);
    classNum = int(maxClass);
    return;
  }
};
};  // namespace ds

#endif  // DISTRIBUTION_HPP


#pragma once

#include <array>
#include <cstdint>
#include <cmath>
#include <crest/geometry/vertex.hpp>
#include <crest/quadrature/rules.hpp>

#include <Eigen/Dense>

namespace crest
{
    template <typename Scalar>
    class ReferenceTriangleTransform
    {
    public:
        ReferenceTriangleTransform(Vertex<Scalar> v0, Vertex<Scalar> v1, Vertex<Scalar> v2);

        Vertex<Scalar> transform_from_reference(const Scalar & x, const Scalar & y) const;
        Scalar absolute_determinant() const { return _absdet; }

        Eigen::Matrix<Scalar, 2, 2> jacobian() const;

    private:
        Vertex<Scalar> _v0;
        Vertex<Scalar> _v1;
        Vertex<Scalar> _v2;
        Scalar _absdet;
    };

    template <typename S>
    ReferenceTriangleTransform<S>::ReferenceTriangleTransform(Vertex<S> v0,
                                                              Vertex<S> v1,
                                                              Vertex<S> v2)
            : _v0(v0), _v1(v1), _v2(v2)
    {
        _absdet = S(1.0 / 4.0) * std::abs(_v1.x * _v2.y - _v1.y * _v2.x);
    }

    /**
     *The reference triangle the quadrature rules are defined on is given by the vertices
     *(-1, 1), (-1, -1), (1, -1),
     *so for a triangle with corners A, B, C, we have that each coordinate x in R^2 is given by the relation
     *
     * x = 1/2 (A + B) + 1/2 * (A - C) z_1 + 1/2 * (B - C) z_2
     *
     * for a reference coordinate z = (z_1, z_2) in R^2. This implies the following mapping:
     *  A <-> ( 1, -1)
     *  B <-> (-1,  1)
     *  C <-> (-1, -1)
     */
    template <typename S>
    Vertex<S> ReferenceTriangleTransform<S>::transform_from_reference(const S & x, const S & y) const
    {

        return S(0.5) * (_v0 + x * _v1 + y * _v2);
    }

    template <typename Scalar>
    Eigen::Matrix<Scalar, 2, 2> ReferenceTriangleTransform<Scalar>::jacobian() const
    {
        Eigen::Matrix<Scalar, 2, 2> J;
        J(0, 0) = Scalar(0.5) * _v1.x;
        J(0, 1) = Scalar(0.5) * _v2.x;
        J(1, 0) = Scalar(0.5) * _v1.y;
        J(1, 1) = Scalar(0.5) * _v2.y;
        return J;
    };

    /**
     *
     * Returns an instance of a ReferenceTriangleTransform<Scalar>, which transforms points from
     * the reference triangle used by triquad to points in the triangle defined by the vertices (a, b, c).
     *
     * @param a
     * @param b
     * @param c
     * @return An instance of ReferenceTriangleTransform<Scalar>.
     */
    template <typename Scalar>
    ReferenceTriangleTransform<Scalar> triquad_transform(const Vertex<Scalar> & a,
                                                         const Vertex<Scalar> & b,
                                                         const Vertex<Scalar> & c)
    {
        const auto v0 = a + b;
        const auto v1 = a - c;
        const auto v2 = b - c;

        return ReferenceTriangleTransform<Scalar>(v0, v1, v2);
    };

    /**
     * Computes the integral of the function f(x, y) -> R on the reference triangled defined by the vertices
     * (-1, 1), (-1, -1), (1, -1).
     * @param f
     * @return
     */
    template <unsigned int Strength, typename Scalar, typename Function2D>
    constexpr inline Scalar triquad_ref(const Function2D & f)
    {
        using quadrature = ::crest::quadrature_rules::quadrature<Scalar, Strength>;
        constexpr auto quad = quadrature();

        Scalar result = static_cast<Scalar>(0.0);
        for (unsigned int i = 0; i < quadrature::num_points; ++i)
        {
            result += quad.w[i] * f(quad.x[i], quad.y[i]);
        }

        return result;
    };

    /**
     * Computes the integral of the function f over the triangle determined by the vertices a, b and c,
     * using Gauss quadrature rules of the given Strength.
     *
     * The Strength of the quadrature rule determines the highest degree polynomial that the quadrature rule
     * is able to integrate *exactly*. For example, a Strength of 4 indicates that triquad computes the exact
     * integral of any polynomial function of *total degree* 4.
     * @param f A callable function f(x, y) that operates on real numbers.
     * @param a
     * @param b
     * @param c
     * @return
     */
    template <unsigned int Strength, typename Scalar, typename Function2D>
    constexpr inline Scalar triquad(
            const Function2D & f,
            const Vertex<Scalar> & a,
            const Vertex<Scalar> & b,
            const Vertex<Scalar> & c)
    {
        const auto transform = triquad_transform(a, b, c);
        const auto f_transformed = [&f, &transform] (auto x, auto y)
        {
            const auto coords = transform.transform_from_reference(x, y);
            return f(coords.x, coords.y);
        };
        return transform.absolute_determinant() * triquad_ref<Strength, Scalar>(f_transformed);
    };
}


////////////////////////////////////////////////////////////////////////////////////////////////////
//                               This file is part of CosmoScout VR                               //
//      and may be used under the terms of the MIT license. See the LICENSE file for details.     //
//                        Copyright: (c) 2019 German Aerospace Center (DLR)                       //
////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef CS_UTILS_CONVERSIONS_HPP
#define CS_UTILS_CONVERSIONS_HPP

#include "cs_utils_export.hpp"

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>

/// This namespace contains utility functions for converting numbers between different units of
/// measuring. Most of the coordinate system conversion methods are based on the code from the
/// excellent book "3D Engine Design for Virtual Globes" by Patrick Cozzi and Kevin Ring
/// (http://virtualglobebook.com/).
namespace cs::utils::convert {

template <typename T>
T lightyearsToMeters(T lightyears) {
  return lightyears * 9460730472580800.0;
}

template <typename T>
T metersToLightyears(T meters) {
  return meters / 9460730472580800.0;
}

/// Converts AU to meters. One AU is equivalent to the average distance between the Earth and the
///// Sun.
template <typename T>
T astronomicalUnitsToMeters(T astronomicalUnits) {
  return astronomicalUnits * 149597870700.0;
}

/// Converts meters to AU. One AU is equivalent to the average distance between the Earth and the
/// Sun.
template <typename T>
T metersToAstronomicalUnits(T meters) {
  return meters / 149597870700.0;
}

template <typename T>
T toRadians(T degrees) {
  return static_cast<T>(degrees * glm::pi<double>() / 180.0);
}

template <typename T>
T toDegrees(T radians) {
  return static_cast<T>(radians * 180.0 / glm::pi<double>());
}

/// Projects an arbitrary cartesian point to the surface of an origin-centered ellipsoid with the
/// given radii. The projection happens towards the center of the ellipsoid.
CS_UTILS_EXPORT glm::dvec3 scaleToGeocentricSurface(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Projects an arbitrary cartesian point to the surface of an origin-centered ellipsoid with the
/// given radii. The projection happens along the surface normal of the ellipsoid. This is
/// potentially a rather expensive operation, as it involes an iterative search for the correct
/// surface normal.
CS_UTILS_EXPORT glm::dvec3 scaleToGeodeticSurface(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Computes longitude and geodetic latitude for a given surface point on an origin-centered
/// ellipsoid with the given radii.
CS_UTILS_EXPORT glm::dvec2 surfaceToLngLat(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Computes longitude and geodetic latitude for an arbitrary cartesian point relative to a
/// origin-centered ellipsoid with the given radii. This is potentially a rather expensive
/// operation, as it involes scaleToGeodeticSurface().
CS_UTILS_EXPORT glm::dvec2 cartesianToLngLat(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Same as above, but returns the distance to the surface of the ellipsoid as well.
CS_UTILS_EXPORT glm::dvec3 cartesianToLngLatHeight(
    glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Transforms longitude and geodetic latitude and elevation height to cartesian (x,y,z)
/// coordinates for an origin-centered ellipsoid with the given radii. Height is an
/// offset along the geodetic normal of the ellipsoid at lngLat.
CS_UTILS_EXPORT glm::dvec3 toCartesian(
    glm::dvec2 const& lngLat, glm::dvec3 const& radii, double height = 0.0);

/// Returns the geodetic normal vector with unit length at geodetic coordinates (lng, lat) lngLat.
CS_UTILS_EXPORT glm::dvec3 lngLatToNormal(glm::dvec2 const& lngLat);

/// Returns the geodetic normal vector with unit length for a given point on the surface of an
/// origin-centered ellipsoid with the given radii.
CS_UTILS_EXPORT glm::dvec3 surfaceToNormal(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Returns the geodetic normal vector with unit length for an arbitrary cartesian point relative to
/// an origin-centered ellipsoid with the given radii. This is potentially a rather expensive
/// operation, as it involes scaleToGeodeticSurface().
CS_UTILS_EXPORT glm::dvec3 cartesianToNormal(glm::dvec3 const& cartesian, glm::dvec3 const& radii);

/// Time in CosmoScout VR is passed around in different formats.
/// * Strings usually store time in the ISO format YYYY-MM-DDTHH:MM:SS.fffZ. The 'Z' suffix is not
///   really required on the C++ side, as time strings are always considered to be in UTC. This
///   format is also directly convertible to JavaScript Dates, here however the 'Z' is required!
///   Else the Date object will be in your local time zone. So it's a good practice to always append
///   the 'Z'.
/// * boost::posix_time::ptime is used for conversions and is also always in UTC.
/// * SPICE time is stored in doubles representing Barycentric Dynamical Time (TDB, seconds since
///   2000-01-01 12:00:00). Note that this is not the same as UTC seconds since 2000-01-01 12:00:00
///   because TDB considers leap seconds. The conversion methods below take this into account.
namespace time {

/// Converts boost::posix_time::ptime to spice time, which is defined by the Barycentric Dynamical
/// Time. Be aware, that SPICE kernels with leap seconds have to be loaded for this method to work.
/// This means, SolarSystem::init() must have been called before.
CS_UTILS_EXPORT double toSpice(boost::posix_time::ptime const& tIn);

/// Converts a time string to spice time, which is defined by the Barycentric Dynamical Time. The
/// string can be in the format YYYY-MM-DD HH:MM:SS.fff, YYYY-MM-DDTHH:MM:SS.fff, or
/// YYYY-MM-DDTHH:MM:SS.fffZ and is always interpreted as UTC. Be aware, that SPICE kernels with
/// leap seconds have to be loaded for this method to work. This means, SolarSystem::init() must
/// have been called before.
CS_UTILS_EXPORT double toSpice(std::string const& tIn);

/// Converts a time string to boost::posix_time time. The string can be in the format
/// YYYY-MM-DD HH:MM:SS.fff, YYYY-MM-DDTHH:MM:SS.fff, or YYYY-MM-DDTHH:MM:SS.fffZ and is always
/// interpreted as UTC.
CS_UTILS_EXPORT boost::posix_time::ptime toPosix(std::string const& tIn);

/// Converts a Barycentric Dynamical Time to boost::posix_time::ptime. Be aware, that SPICE kernels
/// with leap seconds have to be loaded for this method to work. This means, SolarSystem::init()
/// must have been called before.
CS_UTILS_EXPORT boost::posix_time::ptime toPosix(double tIn);

/// Converts a Barycentric Dynamical Time time to a time string in the format
/// YYYY-MM-DDTHH:MM:SS.fffZ. Be aware, that SPICE kernels with leap seconds have to be loaded for
/// this method to work. This means, SolarSystem::init() must have been called before.
CS_UTILS_EXPORT std::string toString(double tIn);

/// Converts a boost::posix_time::ptime time to a time string in the format
/// YYYY-MM-DDTHH:MM:SS.fffZ.
CS_UTILS_EXPORT std::string toString(boost::posix_time::ptime const& tIn);

} // namespace time

} // namespace cs::utils::convert

#endif // CS_UTILS_CONVERSIONS_HPP


#include "graph.h"

#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/LU>

namespace filters::pose_graph {

AbstractFactor::~AbstractFactor() {}

Graph::Graph()
	: _x0(values::Zero(1)), _sol(values::Zero(1)), _sol_cov(hessian::Zero(1, 1)), _factors({}),
	  _factor_counter(0) {}

Graph::~Graph() {
	for (auto f : _factors) {
		delete f;
	}
}

int Graph::add(AbstractFactor* f) {
	f->id = _factor_counter++;
	_factors.push_back(f);
	return f->id;
}

double Graph::eval(const values& x) {
	double sum = 0.0;
	for (auto f : _factors)
		sum += f->eval(x);
	return sum;
}

void Graph::solve(const values& x0, double alpha, int maxiters, double tol) {
	_x0 = x0;
	values x = x0;
	double error = 2 * tol;
	int i = 0;
	int N = x0.size();
	while (error > tol && i < maxiters) {
		values grad = values::Zero(N);
		hessian hess = hessian::Zero(N, N);
		for (auto f : _factors) {
			grad += f->gradient_at(x);
			hess += f->hessian_at(x);
		}
		for (int j = 0; j < N; j++) {
			// Presumably we have no factors affecting this variable
			if (hess(j, j) == 0)
				hess(j, j) = 0.001; // avoid singular matrix
		}
		x -= alpha * (hess.inverse() * grad);
		error = sqrt(grad.transpose() * grad);
		i += 1;
		if (i % 100 == 0)
			std::cout << "Graph::solve Iteration " << i << ": " << error << std::endl;
	}
	_sol = x;

	hessian hess = hessian::Zero(N, N);
	for (auto f : _factors)
		hess += f->hessian_at(_sol);
	for (int j = 0; j < N; j++) {
		if (hess(j, j) == 0)
			hess(j, j) = 0.001; // Again, avoid singular matrix
	}
	_sol_cov = hess.inverse();
}

values Graph::x0() {
	return _x0;
}

values Graph::solution() {
	return _sol;
}

hessian Graph::covariance() {
	return _sol_cov;
}

void Graph::deleteFactor(int id) {
	auto it = _factors.begin();
	while (it != _factors.end()) {
		if ((*it)->id == id) {
			delete *it;
			it = _factors.erase(it);
			return;
		} else {
			++it;
		}
	}
}

void Graph::shiftIndices(int poseSize, int firstPoseIdx) {
	auto it = _factors.begin();
	while (it != _factors.end()) {
		if (!(*it)->shiftIndices(poseSize, firstPoseIdx)) {
			delete *it;
			it = _factors.erase(it);
		} else {
			++it;
		}
	}
}

} // namespace filters::pose_graph


/*
*Topic: Implementation of Sequential Quadratic Programming in C++
*Library: Eigen, a c++ matrix operation library.
*Author: Muqian, Chen
*Current Date: 31.01.2021
*Log: The function to calculate a quadratic optimal programming constrainted by inequation
*        conditions are totally implemnted. It did take a long time because I am not familiar
*        with lots of implicit mechamism of c++. But I feel so lucky that I really started to
*        program with it. I think that I am moving towards to the aim that I really implement
*        something based on ROS. The next task is to cover the case of equation constraints
*        and add a function which automatically calculates the initial working set. Over.
*/

#include <iostream>
#include <Eigen>
#include <typeinfo>
#include <iomanip>

using namespace std;
using namespace Eigen;
VectorXd eqpCalculation(MatrixXd A, VectorXd b, MatrixXd G,
						VectorXd d, VectorXd theta);
double calculationAlpha(MatrixXd& A_working, VectorXd& b_working,
						MatrixXd& A_deactive,VectorXd& b_deactive,
						VectorXd stepLength, VectorXd theta);

//这里若是加上&就会变成按引用传递了亲，已经用test检验过了。
VectorXd eqpCalculation(MatrixXd A, VectorXd b, MatrixXd G,
	                    VectorXd d, VectorXd theta) {
	VectorXd g, h;
	g = d + G * theta;
	h = A * theta - b;
	Index g_row = g.rows();
	Index h_row = h.rows();

	//catenate the A and G as KKT Matrix
	Index A_row = A.rows();
	Index A_col = A.cols();
	Index G_row = G.rows();

	//first catenate them in horizont direction
	MatrixXd K1(G_row, A_row + G_row);
	K1 << G, A.transpose();
	MatrixXd zeros = MatrixXd::Zero(A_row, A_row);
	MatrixXd K2(A_row, A_col + A_row);
	K2 << A, zeros;
	MatrixXd K(A_row + G_row, A_row + G_row);
	K << K1, K2;

	//then catenate [g;h]
	VectorXd g_h(g_row + h_row, 1);
	g_h << g, h;
	//for debug
	//cout << "[INFO]Current KKT Matrix is:" << endl << K << endl;
	//cout << "[INFO]Current g_h is:" << endl << g_h << endl;

	//calculate KKT-Equation
	VectorXd result(G_row + A_row, 1);
	result = K.inverse() * g_h;

	return result;
	};

//implementation of a function template for removing a specific row from MatrixXf or VectorXf
//After validation, this function could work at situation MatrixXf or VectorXf
template <typename T1, typename T2> void removeRow(T1& a, T2 b) {
	// a: Matrix or Vector which need to remove the row
	// b: the row number of the removed row.

	const int newRow = a.rows() - 1;
	const int newCol = a.cols();

	if (b < a.rows()) {
		a.block(b, 0, newRow - b, a.cols()) =
			a.block(b + 1, 0, newRow - b, a.cols());
	}
	else {
		cout << "[INFO] The row to remove is out of boundary of input !" << endl;
	};
	a.conservativeResize(newRow, newCol);
}

//Implementation of a function template for adding a trivial but dimension-matched row into
//MatrixXf or VectorXf
template <typename T1, typename T2> void addRow(T1& a, T2 b) {
	// a: the target Matrix or Vector
	// b: added row into target
	// column number should be unchanged.

	int newRow = a.rows() + 1;
	int newCol = b.cols();

	a.conservativeResize(newRow, newCol);
	a.block(newRow - 1, 0, 1, newCol) = b;
};

//Implementation of a fucntion template
void setInitial(MatrixXd A, MatrixXd& A_working,  MatrixXd& A_deactive, VectorXd b, VectorXd& b_working,
			    VectorXd& b_deactive, VectorXd  initialTheta, int f) {
	//f: amount of inequations
	VectorXd result(f ,1);
	result = A * initialTheta - b;
	ArrayXd resultArray;
	resultArray = result.array().abs();
	for (int i = 0; i < f; i++) {
		if ( result(i, 0) < 1e-7) {
			//add the matched constraints into working set
			addRow(A_working, A.row(i));
			addRow(b_working, b.row(i));
		}
		else {
			//add the unmatched constraints into deacitve set
			addRow(A_deactive, A.row(i));
			addRow(b_deactive, b.row(i));
		}
	}
}

//Pass by reference could be used here for updating the working set.
double calculationAlpha(MatrixXd& A_working, VectorXd& b_working,
					   MatrixXd& A_deactive, VectorXd& b_deactive, VectorXd stepLength, VectorXd theta, int i) {
	double alpha = -1;
	int deactRow = A_deactive.rows();
	int deactCol = A_deactive.cols();

	VectorXd resultAiP(deactRow, 1);
	resultAiP = A_deactive * stepLength;

	VectorXd resultAiTheta(deactRow, 1);
	resultAiTheta = b_deactive - A_deactive * theta;

	ArrayXd result(deactRow, 1);
	ArrayXd resultUp(deactRow, 1);
	ArrayXd resultDown(deactRow, 1);
	resultUp = resultAiTheta.array();
	resultDown = resultAiP.array();
	//for debug
	//cout << "[INFO]: current resultUp is:" << endl << resultUp << endl;
	//cout << "[INFO]: current resultDown is:" << endl << resultDown << endl;

	for (int i = 0; i < deactRow; i++) {
		if (resultDown(i,0)< 0) {
			result(i, 0) = resultUp(i, 0) / resultDown(i, 0);
		}
		else {
			result(i, 0) = 1000.0;
		}
	}
	//for debug
	//cout << "[INFO]: current result is:" << endl << result << endl;

	Index minRow, minCol;
	double min = 0;
	min = result.matrix().minCoeff(&minRow, &minCol);

	//=============================Attention！！！！======================================
	//这里有个坑，就是c++中除号/出来的结果会直接舍弃小数点后面的数，比如这里min应该是1.429
	//但是只输出1，但是在这个程序中我可以不用管这个问题，因为之后是要拿去跟1做比较的。
	//不对啊，我得管，因为之后alpha若是算出小于一则按照这个规则直接就是零了。。。
	//遭重了，得把所有矩阵都改成double类型。
	//全部都改成也不行，当你使用mat.array()的时候它会自动将类型换掉（或者有别的我不懂的内
	//隐操作），通过实验得出，必须要另外声明ArrayXd变量来储存mat.array()的结果，这样才能在
	//相除操作中得到完美的小数点后面的数。
	//-----31.01.2021
	//ArrayXd result(deactRow, 1);
	//ArrayXd resultUp(deactRow, 1);
	//ArrayXd resultDown(deactRow, 1);
	//resultUp = resultAiTheta.array();
	//resultDown = resultAiP.array();
	//result = resultUp / resultDown;
	//cout << "[INFO]Current result is :" << result << endl;
	//=============================Attention！！！！======================================

	if (min < 1) {
		alpha = min;

		//add the blocking constraint into working set
		addRow(A_working, A_deactive.row(minRow));
		addRow(b_working, b_deactive.row(minRow));
		cout << "[INFO] " << i << ". loop. A constraint is added to working set and deleted from deactive set." << endl;

		//delete the blocking constraint from deactive set
		removeRow(A_deactive, minRow);
		removeRow(b_deactive, minRow);

		return alpha;
	}
	else {
		alpha = 1;
		return alpha;
	}
}

int main(int argc, char** argv)
{
	//define G and d of objective function
	MatrixXd G(2,2);
	G << 2.0, 0.0,
		 0.0, 2.0;
	VectorXd d(2,1);
	d << -2.0,
		 -5.0;

	//define initialized theta
	VectorXd theta(2, 1);
	theta(0) = 2.0;
	theta(1) = 0.0;
	int ParameterNum = theta.rows();

	//define equation constraints
	MatrixXd AEquation(0, ParameterNum);
	VectorXd bEquation(0, 1);

	//define inequation constraints
	MatrixXd AInequation(5, ParameterNum);
	AInequation << 1.0, -2.0,
				  -1.0, -2.0,
				  -1.0,  2.0,
				   1.0,  0.0,
				   0.0,  1.0;

	int AInitialRowNum = AInequation.rows();

	VectorXd bInequation(5, 1);
	bInequation << -2.0,
				   -6.0,
				   -2.0,
				    0.0,
				    0.0;

	//define working set
	MatrixXd A_working(0, ParameterNum);
	VectorXd b_working(0, 1);

	A_working = AEquation;
	b_working = bEquation;

	MatrixXd A_deactive(0, ParameterNum);
	VectorXd b_deactive(0, 1);

	setInitial(AInequation, A_working, A_deactive, bInequation,
			   b_working, b_deactive, theta, AInitialRowNum);

	//for debug
	//cout << "[INFO]Current A inequation constraints is:" << endl << A_working << endl;
	//cout << "[INFO]Current b inequation constraints is:" << endl << b_working << endl;
	//define the main variable during the calculating loop
	Index constraintsNumber = b_working.rows();
	Index thetaNumber = theta.rows();

	VectorXd result(thetaNumber+constraintsNumber, 1);
	VectorXd stepLength(thetaNumber, 1);
	VectorXd lambdaStar(constraintsNumber, 1);

	int i = 1;

	while (true) {

		result = eqpCalculation(A_working, b_working, G, d, theta);

		constraintsNumber = b_working.rows();
		stepLength << -result.head(thetaNumber);
		lambdaStar = result.tail(constraintsNumber);
		//for debug
		cout << "[INFO]: current stepLength is:" << endl << stepLength << endl;
		cout << "[INFO]: current lambdaStar is:" << endl << lambdaStar << endl;

		if ( ( stepLength.array().abs() < 1e-10 ).all() == 1 ) {
			if ((lambdaStar.array() > 0).all() == 1) {
				break;
			}
			else {
				VectorXf::Index minRow, minCol;
				float min_lambda = lambdaStar.minCoeff(&minRow, &minCol);

				//add the deleted constraints into deactive set
				addRow(A_deactive, A_working.row(minRow));
				addRow(b_deactive, b_working.row(minRow));

				//delete the constraints from working set
				removeRow(A_working, minRow);
				removeRow(b_working, minRow);
				cout << "[INFO] " << i << ". loop. A constraint is deleted from working set and added in deactive set." << endl;
				i++;
			}
		}
		else {
			float alpha = 0.0;
			alpha = calculationAlpha(A_working, b_working, A_deactive, b_deactive, stepLength, theta, i);
			cout << "[INFO]" << i << ". loop.Current alpha is :" << endl << alpha << endl;
			theta = theta + alpha * stepLength;
			cout << "[INFO]" << i << ". loop.Current Theta is :" <<endl << theta << endl;
			i++;
		}
	}

}