#include <plll.hpp>
#include <Eigen/Dense>
#include <random>
#include <iostream>
#include <vector>
#include <map>
#include <numeric>
#include <fstream>
#include <algorithm>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <regex>

using namespace std;
//namespace plt = matplotlibcpp;
template <class T>
using Matrix = vector<vector<T>>;
using sec_type = std::chrono::microseconds;
using _type = float;
constexpr double _c = 0.9;

struct matrix_with_det
{
    int det;
    Matrix<int> mat;
};

template <typename T>
void getCofactor(Matrix<T> &A, Matrix<T> &temp, int p, int q, int n)
{
    int i = 0, j = 0;

    // Looping for each element of the matrix
    for (int row = 0; row < n; row++)
    {
        for (int col = 0; col < n; col++)
        {
            //  Copying into temporary matrix only those element
            //  which are not in given row and column
            if (row != p && col != q)
            {
                temp[i][j++] = A[row][col];

                // Row is filled, so increase row index and
                // reset col index
                if (j == n - 1)
                {
                    j = 0;
                    i++;
                }
            }
        }
    }
}
/* Recursive function for finding determinant of matrix.
   n is current dimension of A[][]. */
template <typename T>
T determinant(Matrix<T> &A, int n)
{

    //  Base case : if matrix contains single element
    if (n == 1)
        return A[0][0];
    T D = 0;                            // Initialize result
    Matrix<T> temp(n, vector<T>(n, 0)); // To store cofactors

    float sign = 1; // To store sign multiplier

    // Iterate for each element of first row
    for (int f = 0; f < n; f++)
    {
        // Getting Cofactor of A[0][f]
        getCofactor(A, temp, 0, f, n);
        D += sign * A[0][f] * determinant(temp, n - 1);

        // terms are to be added with alternate sign
        sign = -sign;
    }

    return D;
}

// Function to get adjoint of A[N][N] in adj[N][N].
void adjoint(Matrix<_type> &A, Matrix<_type> &adj)
{
    int n = A.size();
    if (n == 1)
    {
        adj[0][0] = 1;
        return;
    }

    // temp is used to store cofactors of A[][]
    _type sign = 1;
    Matrix<_type> temp(n, vector<_type>(n, 0));

    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            // Get cofactor of A[i][j]
            getCofactor(A, temp, i, j, n);

            // sign of adj[j][i] positive if sum of row
            // and column indexes is even.
            sign = ((i + j) % 2 == 0) ? 1 : -1;

            // Interchanging rows and columns to get the
            // transpose of the cofactor matrix
            adj[j][i] = (sign) * (determinant(temp, n - 1));
        }
    }
}

// Function to calculate and store inverse, returns false if
// matrix is singular
bool inverse(Matrix<_type> &A, Matrix<_type> &inverse)
{
    // Find determinant of A[][]
    int det = determinant(A, A.size());
    //_type det = 6;
    if (det == 0)
    {
        cout << "Singular matrix, can't find its inverse";
        return false;
    }

    // Find adjoint
    Matrix<_type> adj;
    for (int i = 0; i < A.size(); i++)
    {
        adj.push_back(vector<_type>());
        for (int j = 0; j < A.size(); j++)
            adj[i].push_back(0);
    }
    adjoint(A, adj);

    // Find Inverse using formula "inverse(A) = adj(A)/det(A)"
    for (int i = 0; i < A.size(); i++)
        for (int j = 0; j < A.size(); j++)
            inverse[i][j] = adj[i][j] / float(det);

    return true;
}

template <typename T, typename T2>
double dotProduct(const vector<T> &v1, const vector<T2> &v2)
{
    double ret = 0;
    for (int i = 0; i < v1.size(); ++i)
        ret += (v1.at(i) * v2.at(i));
    return ret;
}

template <typename T>
vector<T> operator*(const double &c, const vector<T> &v)
{
    vector<T> ret = v;
    for (auto &d : ret)
        d *= c;
    return ret;
}

vector<_type> operator*(Matrix<_type> &M, const vector<_type> &v)
{
    int n = v.size();
    vector<_type> res(n, 0);
    for (size_t i = 0; i < n; i++)
    {
        for (size_t j = 0; j < n; j++)
        {
            res[i] += M[j][i] * v[j];
        }
    }
    return res;
}
template <typename T, typename T2>
vector<T2> subtract(const vector<T> &v1, const vector<T2> &v2)
{
    vector<T2> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = (v1[i] - v2[i]);
    return ret;
}
template <typename T, typename T2>
vector<T> operator-(const vector<T> &v1, const vector<T2> &v2)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = v1[i] - v2[i];
    return ret;
}
template <typename T>
vector<T> inverse(const vector<T> &v1)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = -v1[i];
    return ret;
}
template <typename T>
bool operator==(const vector<T> &v1, const vector<T> &v2)
{
    if (v1.size() == v2.size())
    {
        if (v1.size())
        {
            if (v1[0] == v2[0])
            {
                if (v1[0] == 0)
                {
                    int sign = 0;
                    for (int i = 1; i < v1.size(); i++)
                    {
                        if (v1[i] == 0 && v2[i] == 0)
                            continue;
                        if (sign == 0)
                        {
                            if (v1[i] == v2[i])
                                sign = 1;
                            else if (v1[i] == -v2[i])
                                sign = -1;
                            else
                                return false;
                        }
                        else if (v1[i] != sign * v2[i])
                        {
                            return false;
                        }
                    }
                }
                else
                {
                    for (int i = 1; i < v1.size(); i++)
                    {
                        if (v1[i] != v2[i])
                        {
                            return false;
                        }
                    }
                }
                return true;
            }
            if (v1[0] == -v2[0])
            {
                for (int i = 1; i < v1.size(); i++)
                {
                    if (v1[i] != -v2[i])
                    {
                        return false;
                    }
                }
                return true;
            }
            return false;
        }
        else
            return true;
    }
    else
        return false;
}
template <typename T>
vector<T> operator+(const vector<T> &v1, const vector<T> &v2)
{
    vector<T> ret(v1.size());
    for (int i = 0; i < v1.size(); ++i)
        ret[i] = (v1.at(i) + v2.at(i));
    return ret;
}
vector<_type> roundV(vector<_type> a)
{
    for (size_t i = 0; i < a.size(); i++)
    {
        a[i] = round(a[i]);
    }
    return a;
}

vector<_type> proj(const vector<_type> &v1, const vector<_type> &v2)
{
    vector<_type> proj = (dotProduct(v1, v2) / dotProduct(v1, v1)) * v1;
    return proj;
}

vector<_type> proj(const vector<_type> &v1, const vector<int> &v2)
{
    vector<_type> proj = (dotProduct(v1, v2) / dotProduct(v1, v1)) * v1;
    return proj;
}

Matrix<_type> gSchmidt(const Matrix<int> &vspace)
{
    Matrix<_type> uspace(vspace.size(), vector<_type>(vspace[0].size()));
    for (int i = 0; i < vspace[0].size(); i++)
    {
        uspace[0][i] = vspace[0][i];
    }
    for (int i = 1; i < vspace.size(); ++i)
    {
        for (int j = 0; j < i; ++j)
        {
            uspace.at(i) = subtract(vspace.at(i), proj(uspace.at(j), vspace.at(i)));
        }
    }
    return uspace;
}
Matrix<_type> gSchmidt(const Matrix<_type> &vspace, int limit)
{
    Matrix<_type> uspace;
    for (int i = 0; i <= limit; ++i)
    {
        uspace.push_back(vspace.at(i));
        for (int j = 0; j < i; ++j)
        {
            uspace.at(i) = uspace.at(i) - proj(uspace.at(j), vspace.at(i));
        }
    }
    return uspace;
}
template <typename T>
void printVS(const Matrix<T> &vspace)
{
    for (auto &v : vspace)
    {
        cout << '|';
        for (auto &d : v)
            cout << d << ' ';
        cout << "|\n";
    }
}
template <typename T>
void printVS(const Matrix<T> &vspace, ostream &file)
{
    file << '[';
    for (auto &v : vspace)
    {
        file << '[';
        for (auto &d : v)
            file << d << ' ';
        file << "]\n";
    }
    file << "]\n";
}
template <typename T>
void printVec(const vector<T> &v)
{
    cout << endl;
    cout << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
    for (auto &d : v)
        cout << d << ' ';
    cout << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
}
template <typename T>
void printVec(const vector<T> &v, ostream &file)
{
    file << endl;
    file << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
    for (auto &d : v)
        file << d << ' ';
    file << endl
         << "$$$$$$$$$$$$$$$$$$$$$$$$" << endl;
}
template <typename T>
float mu(int k1, int k2, Matrix<T> *m, Matrix<_type> *gsm)
{
    T init = 0;
    vector<T> *a = &m->at(k1);
    vector<_type> *b = &gsm->at(k2);
    return std::inner_product(a->begin(), a->end(), b->begin(), init) /
           std::inner_product(b->begin(), b->end(), b->begin(), init);
}
template <typename T, typename T2>
float mu(vector<T> &a, vector<T2> &b)
{
    float init = 0.0;
    double d = std::inner_product(a.begin(), a.end(), b.begin(), init);
    double dd = std::inner_product(b.begin(), b.end(), b.begin(), init);
    return d / dd;
}
template <typename T>
double squaredLengthOfVector(vector<T> *v)
{
    return std::inner_product(v->begin(), v->end(), v->begin(), 0.0);
}
template <typename T>
double squaredLengthOfVector(vector<T> &v)
{
    return std::inner_product(v.begin(), v.end(), v.begin(), 0.0);
}

int KabatianskyLevenshteinC(Matrix<int> &M)
{
    float angle = 1;
    for (int i = 0; i < M.size(); i++)
    {
        auto v1 = M[i];
        for (int j = i + 1; j < M.size(); j++)
        {
            auto v2 = M[j];
            float t = dotProduct(v1, v2) / (sqrt(squaredLengthOfVector(&v1)) *
                                            sqrt(squaredLengthOfVector(&v2)));
            angle = fmin(angle, t);
        }
    }
    return fmax((int)(-1.0 / 2.0 * log(1 - cos(angle)) - 0.099), 1);
}

vector<int> lll_reduce(vector<_type> &gsreducing, vector<int> &target,
                       vector<int> &reducing)
{
    vector<int> t = target - round(mu(target, gsreducing)) * reducing;
    return t;
}

void LLL(Matrix<int> &B, ostream &o)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = 0; j < i; j++)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
        }
        for (int i = 1; i < B.size() && !cont; i++)
        {
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                cont = true;
            }
        }
    } while (cont);
    o << "lll iter : " << iter << "|\n";
}

void LLL(Matrix<int> &B)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = 0; j < i; j++)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                i--;
                cont = true;
            }
        }
    } while (cont);
}
// opera 918
void LLL2(Matrix<int> &B)
{
    bool cont;
    Matrix<_type> gramSmidt;
    int iter = 0;
    do
    {
        iter++;
        gramSmidt = gSchmidt(B);
        cont = false;
        for (int i = 1; i < B.size(); i++)
        {
            for (int j = i - 1; j > 0; j--)
            {
                B.at(i) = lll_reduce(gramSmidt.at(j), B.at(i), B.at(j));
            }
            float a1 = squaredLengthOfVector(&gramSmidt.at(i - 1));
            float a2 = squaredLengthOfVector(&gramSmidt.at(i));
            float mu1 = mu(i, i - 1, &B, &gramSmidt);
            if (a2 < (_c - mu1 * mu1) * a1)
            {
                B.at(i - 1).swap(B.at(i));
                i--;
                cont = true;
            }
        }
    } while (cont);
}
void preparePlot(Matrix<_type> res, Matrix<_type> og)
{
    map<_type, _type> m;
    vector<_type> x, y, z;
    int maxSize = 20;
    if (res.size() == 2)
    {
        vector<_type> bounds = 2 * og[0] + 2 * og[1];
        int boundX = bounds[0];
        int boundY = bounds[1];
        for (int i = -maxSize; i < maxSize; i++)
        {
            for (int j = -maxSize; j < maxSize; j++)
            {
                vector<_type> t = i * res.at(0) + j * res.at(1);
                if (-boundX < t[0] < boundX && -boundY < t[1] < boundY)
                {
                    x.push_back(t.at(0));
                    y.push_back(t.at(1));
                }
            }
        }
        //plt::plot(x, y, ".");
    }
    if (res.size() == 3)
    {
        vector<_type> bounds = 2 * og[0] + 2 * og[1];
        int boundX = bounds[0];
        int boundY = bounds[1];
        int boundZ = bounds[2];
        for (int i = -maxSize; i < maxSize; i++)
        {
            for (int j = -maxSize; j < maxSize; j++)
                for (int k = -maxSize; k < maxSize; j++)
                {
                    vector<_type> t = i * res.at(0) + j * res.at(1) + k * res.at(2);
                    if (-boundX < t[0] < boundX && -boundY < t[1] < boundY)
                    {
                        x.push_back(t.at(0));
                        y.push_back(t.at(1));
                        z.push_back(t.at(2));
                    }
                }
        }
    }
    /*plt::plot({ 0 }, { 0 }, "bo");
    plt::plot({ res.at(0).at(0) }, { res.at(0).at(1) }, "r+");
    plt::plot({ res.at(1).at(0) }, { res.at(1).at(1) }, "r+");
    plt::plot({ og.at(0).at(0) }, { og.at(0).at(1) }, "g*");
    plt::plot({ og.at(1).at(0) }, { og.at(1).at(1) }, "g*");*/
}
//https://tel.archives-ouvertes.fr/tel-01245066v2/document
vector<int> kleinSample(Matrix<int> &B, Matrix<_type> &GSO, float deviation, vector<int> &c)
{
    vector<int> v(c.size(), 0);
    std::random_device mch;
    std::default_random_engine generator(mch());

    double d, z;
    for (int i = B.size() - 1; i >= 0; i--)
    {
        double length = squaredLengthOfVector(GSO.at(i));
        d = dotProduct(c, GSO.at(i)) / length;
        double sigma = deviation / sqrt(length);
        std::normal_distribution<double> distribution(d, 1);
        z = round(distribution(generator));
        //std::cout << z << " " ;
        c = c - z * B[i];
        v = v + z * B[i];
    }
    //std::cout << std::endl;
    return v;
}

// todo discover
vector<_type> sample_gaussian(Matrix<_type> &B)
{
    vector<_type> a;
    a = (5 - rand() % 10) * B.back();
    for (auto b : B)
        a = a + (5 - rand() % 10) * b;
    return a;
}

bool reduce(vector<int> &p, Matrix<int> &L, vector<double> &lengths)
{
    double s = squaredLengthOfVector(p);
    for (size_t i = 0; i < L.size(); i++)
    {
        if (s >= lengths[i])
        {
            auto diff = p - L[i];
            double s2 = squaredLengthOfVector(diff);
            if (s2 < s)
            {
                p = diff;
                return true;
            }
        }
    }
    return false;
}

vector<int> gauss_reduce(vector<int> &v_new, Matrix<int> &L, Matrix<int> &S, vector<double> &lengths)
{
    bool cont = true;
    while (cont)
    {
        auto same = std::find_if(L.begin(), L.end(), [&v_new](vector<int> &v) { return v_new == v; });
        if (same != L.end())
        {
            return vector<int>(v_new.size());
        }
        // 2 раза считается разница, можно сохранить и сравнить с минимумом
        cont = reduce(v_new, L, lengths);
    }
    auto it = L.begin();
    auto it2 = lengths.begin();
    int i = 0;
    vector<int> a(v_new.size());
    while (it != L.end())
    {
        double v_new_length = squaredLengthOfVector(&v_new);
        vector<int> v = *it;
        if (lengths[i] > v_new_length)
        {
            a = v - v_new;
            if (squaredLengthOfVector(a) <= lengths[i])
            {
                S.push_back(a);
                it = L.erase(it);
                it2 = lengths.erase(it2);
            }
            else
            {
                ++it;
                ++it2, ++i;
            }
        }
        else
        {
            ++it;
            ++it2, ++i;
        }
    }
    return v_new;
}
// https://cseweb.ucsd.edu/~daniele/papers/Sieve.pdf
// c= 1000
vector<int> gaussSieve(Matrix<int> &B, float bound, ostream &o)
{
    Matrix<int> L, S;
    vector<int> v_new;
    Matrix<_type> gramSmidt = gSchmidt(B);
    vector<double> squarelengthsVector;
    int K = 0, iter = 0;
    //int c = KabatianskyLevenshteinC(B);
    map<int, int> _map;

    size_t max_number_of_collistions = 200;
    do
    {
        _map.insert(make_pair(iter, K));

        iter++;
        if (S.size() > 0)
        {
            v_new = S.back();
            S.pop_back();
        }
        else
        {
            vector<int> a(B[0].size(), 0);
            v_new = kleinSample(B, gramSmidt, 1, a);
        }
        v_new = gauss_reduce(v_new, L, S, squarelengthsVector);
        if (!v_new.size() ||
            std::count(v_new.begin(), v_new.end(), 0) == v_new.size())
        {
            K++;
        }
        else if (sqrt(squaredLengthOfVector(v_new)) < bound)
        {
            o << "early exit gauss iter : " << iter << "|\n";
            return v_new;
        }
        else
        {
            L.push_back(v_new);
            squarelengthsVector.push_back(squaredLengthOfVector(v_new));
        }
        max_number_of_collistions = std::max(max_number_of_collistions, L.size());
    } while (10 * K < max_number_of_collistions + 2000 || L.size() == 0);
    vector<int> shortestVector = L[0];
    int _a = 0, count = 0;
    for (int i = 1; i < L.size(); i++)
    {
        if (sqrt(squaredLengthOfVector(L[i])) <= bound)
        {
            count++;
        }
        if (squaredLengthOfVector(shortestVector) > squaredLengthOfVector(L[i]))
        {
            shortestVector = L[i];
            _a = i;
        }
    }
    cout << "count " << count << endl;
    cout << "gauss at : " << _map.at(_a) << endl;
    o << "gauss iter : " << iter << "|\n";
    return shortestVector;
}

vector<_type> randomInBall(const int dimm, float r)
{
    vector<_type> randVector(dimm);
    for (int i = 0; i < dimm; i++)
    {
        randVector[i] = (rand() % (int)(r + 1));
    }
    return (r * (rand() / double(RAND_MAX)) / sqrt(squaredLengthOfVector(randVector))) * randVector;
}
//https://crypto.stackexchange.com/questions/29661/how-to-find-the-value-of-a-vector-modulo-a-basis-in-lattice-based-cryptography/29701#29701
vector<_type> mod(vector<_type> vec, Matrix<_type> m)
{
    Matrix<_type> r(m.size(), vector<_type>(m.size(), 0));
    inverse(m, r);
    return m * roundV(r * vec);
}

void ListReduce(vector<int> &p, Matrix<int> &L, vector<double> &lengths)
{
    bool cont = true;
    while (cont)
    {
        auto same = std::find(L.begin(), L.end(), p);
        if (same != L.end())
        {
            p = vector<int>(p.size());
            return;
        }
        if (!reduce(p, L, lengths))
            return;
        /*auto f = std::find_if(L.begin(), L.end(), [&p](vector<int>& v) {
            return squaredLengthOfVector(&p) >= squaredLengthOfVector(&v) && squaredLengthOfVector(&(p - v)) < squaredLengthOfVector(&p);
            });
        if (f == L.end()) return;
        p = p - *f;*/
    }
}
//https://math.stackexchange.com/questions/396382/what-does-this-dollar-sign-over-arrow-in-function-mapping-mean
//https://eprint.iacr.org/2014/714.pdf
vector<int> ListSieve(Matrix<int> &B, float bound, ostream &o)
{
    Matrix<int> L;
    Matrix<_type> gramSmidt = gSchmidt(B);
    vector<int> a(B[0].size(), 0);
    vector<double> lengths;
    map<int, int> _map;
    size_t max_number_of_collistions = 200;
    int iter = 0;
    for (int i = 0, k = 0; 10*i < max_number_of_collistions + 2000 || L.size() == 0; k++)
    {
        iter++;
        vector<int> v = kleinSample(B, gramSmidt, 1, a);
        ListReduce(v, L, lengths);
        bool isZero = true;
        for (auto in : v)
        {
            if (in != 0)
            {
                isZero = false;
                break;
            }
        }
        _map.insert(make_pair(k, i));
        if (isZero)
        {
            i++;
        }
        else if (sqrt(squaredLengthOfVector(v)) < bound)
        {
            o << "early list iter : " << iter << "|\n";
            return v;
        }
        else
        {
            L.push_back(v);
            lengths.push_back(squaredLengthOfVector(v));
        }
        max_number_of_collistions = std::max(max_number_of_collistions, L.size());

    }
    vector<int> shortestVector = L[0];
    int _a = 0, count = 0;
    for (int i = 1; i < L.size(); i++)
    {
        if (sqrt(squaredLengthOfVector(L[i])) <= bound)
        {
            count++;
        }
        if (squaredLengthOfVector(shortestVector) > squaredLengthOfVector(L[i]))
        {
            shortestVector = L[i];
            _a = i;
        }
    }
    cout << "count " << count << endl;
    cout << "list at : " << _map.at(_a) << endl;
    o << "list iter : " << iter << "|\n";
    return shortestVector;
}

vector<int> sumVectorsWithCoeff(Matrix<int> &matrix, vector<int> &c)
{
    vector<int> a(c.size(), 0);
    for (size_t i = 0; i < c.size(); i++)
    {
        a = a + c[i] * matrix[i];
    }
    return a;
}

bool checkifBiggerByEachCoordinate(vector<int> &a, vector<int> &b)
{
    for (size_t i = 0; i < a.size(); i++)
    {
        if (abs(a[i]) > abs(b[i]))
            return false;
    }
    return true;
}

vector<int> LLLenumerate(Matrix<int> &matrix, ofstream &myfile)
{
    Matrix<int> enumeration;
    int n = matrix.size();
    int lim = pow(2, n * (n - 1) / 4.0);
    vector<int> c(n, 0);
    c[0] = 1;
    vector<int> shortest = sumVectorsWithCoeff(matrix, c);
    auto shortestL = squaredLengthOfVector(shortest);
    long long tmpL;
    vector<int> tmp;
    tmp = sumVectorsWithCoeff(matrix, c);
    if (!checkifBiggerByEachCoordinate(shortest, tmp))
    {
        tmpL = squaredLengthOfVector(tmp);
        if (tmpL > 0 && shortestL > tmpL)
        {
            shortestL = tmpL;
            shortest = tmp;
        }
    }
    for (int i = 0; i < n;)
    {
        int &a = c[i];
        if (a == -lim)
        {
            for (int j = 0; j < i; j++)
                c[j] = -lim;
            a++;
            i = 0;
            tmp = sumVectorsWithCoeff(matrix, c);
            if (!checkifBiggerByEachCoordinate(shortest, tmp))
            {
                tmpL = squaredLengthOfVector(tmp);
                if (tmpL > 0 && shortestL > tmpL)
                {
                    shortestL = tmpL;
                    shortest = tmp;
                }
            }
            // for (auto &bb : c)
            //     cout << bb;
            // cout << endl;
            // // enumeration.push_back(sumVectorsWithCoeff(matrix, c));
        }
        else if (a < lim)
        {
            a++;
            if (a == lim)
                i++;
            else
                i = 0;
            tmp = sumVectorsWithCoeff(matrix, c);
            if (!checkifBiggerByEachCoordinate(shortest, tmp))
            {
                tmpL = squaredLengthOfVector(tmp);
                if (tmpL > 0 && shortestL > tmpL)
                {
                    shortestL = tmpL;
                    shortest = tmp;
                }
            }
            // for (auto &bb : c)
            //     cout << bb;
            // cout << endl;
            // // enumeration.push_back(sumVectorsWithCoeff(matrix, c));
        }
        else if (a == lim)
        {
            i++;
        }
    }
    return shortest;
}

//https://web.eecs.umich.edu/~cpeikert/pubs/lattice-survey.pdf
Matrix<int> generateLattice(int dimm, int diff, int iter, bool print, ofstream &file)
{
    Matrix<int> m(dimm, vector<int>(dimm, 0));

    file << "---------------------------------------------\n\n"
         << "dimm: " << dimm << " iter: " << iter << " diff: " << diff << "|\n";
    int summ = 0;
    srand(time(0));
    file << '[';
    for (auto &v : m)
    {
        file << '[';
        for (auto &el : v)
        {
            float f = rand();
            el = f / RAND_MAX * (float)diff * 3;
            summ += el;
            file << el << ' ';
        }
        file << "]\n";
    }
    file << "]\n";
    file << " summ: " << summ << "\n\n";
    return m;
}
//void preprocessVoronoi(Matrix<int>& B)
//{
//    LLL(B);
//}
//
//vector<int> findRelevantHelper(Matrix<int>& B, int p, int n)
//{
//    vector<int> res(B.size(), 0);
//    for (size_t i = 0; i < n; i++)
//    {
//        if (p & 1)
//        {
//            res = res - B[i];
//        }
//        p = p >> 1;
//    }
//    for (auto& b : res)
//        b = b / 2;
//    return res;
//}
//
//vector<int> CVPP2V(vector<int>& t, Matrix<vector<int>>& Voronoi) {
//    int i = 0;
//    vector<int> t0(t.size()), ti(t.size());
//    while (find_if(Voronoi.begin(), Voronoi.end(), [&ti](auto& v) {
//        return v[0] == ti
//    }) == Voronoi.end()) {
//        float tmp = mu(ti, Voronoi[0][0]);
//        int maxi = 0;
//        for (size_t j = 1; j < Voronoi.size(); j++)
//        {
//            if (tmp < mu(ti,Voronoi[j][0])) {
//                maxi = j;
//            }
//        }
//        ti = ti - ti;
//        i++;
//    }
//
//    return t0 - ti;
//}
//vector<int> CVPP(vector<int>& t, Matrix<int>& B, Matrix<vector<int>>& Voronoi) {
//    vector<int> res(t.size(), 0), tp;
//    for (auto& v : B) {
//        res = res + mu(t, v) * v;
//    }
//    // not sure about this
//    int p = 0;
//    while(true) {
//        p++;
//        if (squaredLengthOfVector(t) < (squaredLengthOfVector(pow(2, p) * Voronoi[0][0]))) {
//            break;
//        }
//    }
//
//    for (int i = p - 1; i > 0; i--) {
//        tp = tp - CVPP2V(tp, pow(2, i - 1) * Voronoi);
//    }
//    return res - tp;
//}
//
//vector<int> rankReduceCVP(const vector<int>& t, Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H) {
//    vector<int> res(t.size(), 0), v, closest, tmp;
//    int h = mu(t, B.at(Voronoi.size()));
//    float length, tmplength;
//    int i = h - H + 1;
//    res = CVPP(t - h * B.at(Voronoi.size()), Voronoi.back());
//    length = squaredLengthOfVector(res);
//    i++;
//    // todo is it correct?
//    for (; abs(i - h) < H; i++) {
//        v = CVPP(t - i * B.at(Voronoi.size()), Voronoi.back());
//        tmp = v - t;
//        tmplength = squaredLengthOfVector(tmp);
//        if (tmplength < length) {
//            length = tmplength;
//            res = tmp;
//        }
//    }
//    return res;
//}
//
//Matrix<vector<int>> findRelevant(Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H)
//{
//    Matrix<vector<int>> res;
//    vector<int> t, c;
//    int l = pow(2, B.size());
//    // p - вектор из нелей и 1
//    for (int p = 1; p < l; p++)
//    {
//        t = findRelevantHelper(B, p, B.size());
//        c = rankReduceCVP(t, B, Voronoi, H);
//        t = 2 * (t - c);
//        c = inverse(t);
//        res.push_back({ t, c });
//    }
//    return res;
//}
//void removeNonRelevant(Matrix<vector<int>>& res)
//{
//    for (size_t i = 0; i < res.size(); i++)
//    {
//        for (size_t j = 0; j < res.size(); j++)
//        {
//            if (i != j && squaredLengthOfVector(res[j][0] - 0.5 * res[i][0]) <= squaredLengthOfVector(0.5 * res[i][0])) {
//                // debug whats in it
//                res.erase(res.begin() + i);
//            }
//        }
//
//    }
//
//}
//
//Matrix<vector<int>> computeVcell(Matrix<int>& B, Matrix<vector<int>>& Voronoi, int H)
//{
//    Matrix<vector<int>> res = findRelevant(B, Voronoi, H);
//    removeNonRelevant(res);
//    return res;
//}
//// https://cseweb.ucsd.edu/~pvoulgar/files/voronoi_full.pdf
//void basicVoronoiCell(Matrix<int>& B)
//{
//    preprocessVoronoi(B);
//    Matrix<vector<int>> Voronoi(1);
//    Matrix<int> newB;
//    Voronoi[0] = { B[0], inverse(B[0]) };
//    for (int i = 1; i < B.size(); i++)
//    {
//        for (size_t j = 0; j < i; j++)
//        {
//            newB.push_back(B[j]);
//        }
//        // nedd to remake to vec of vec of vec
//        Voronoi = computeVcell(newB, Voronoi, pow(2, 0.5 * i));
//    }
//}
// void printShortestVector(const plll::linalg::math_matrix<plll::arithmetic::Integer> & A,
//                          unsigned index, const plll::arithmetic::Integer & sqnorm)
// {
//     std::cout << "The currently shortest vector is " << A.row(index)
//               << " with squared norm " << sqnorm << " (Integer)\n";
// }
// void printShortestVector_LI(const plll::linalg::math_matrix<plll::arithmetic::NInt<long> > & A,
//                             unsigned index, const plll::arithmetic::NInt<long> & sqnorm)
// {
//     std::cout << "The currently shortest vector is " << A.row(index)
//               << " with squared norm " << sqnorm << " (NInt<long>)\n";
// }

plll::LatticeReduction::MinCallbackFunction_LI f_LI(ostream &file, vector<int> &r)
{
    return [&r, &file](const plll::linalg::math_matrix<plll::arithmetic::NInt<long>> &A,
                       unsigned index, const plll::arithmetic::NInt<long> &sqnorm) {
        auto i = A.row(index).enumerate();
        int iter = 0;
        while (i.has_current())
        {
            r[iter++] = plll::arithmetic::convert<int>(i.current());
            i.next();
        }
        file << "The currently shortest vector is " << A.row(index)
             << " with squared norm " << sqnorm << " (Integer)\n";
    };
}

plll::LatticeReduction::MinCallbackFunction f(ostream &file, vector<int> &r)
{
    return [&r, &file](const plll::linalg::math_matrix<plll::arithmetic::Integer> &A,
                       unsigned index, const plll::arithmetic::Integer &sqnorm) {
        auto i = A.row(index).enumerate();
        int iter = 0;
        while (i.has_current())
        {
            r[iter++] = plll::arithmetic::convert<int>(i.current());
            i.next();
        }
        file << "The currently shortest vector is " << A.row(index)
             << " with squared norm " << sqnorm << " (Integer)\n";
    };
}
vector<int> shortestVectorByVoronoi(Matrix<int> &B, int dimm, ostream &file)
{
    cout << "mat";
    vector<int> res(dimm);
    cout << "mat";
    plll::linalg::base_matrix<plll::arithmetic::Integer> mat(dimm, dimm);
    cout << "mat";

    plll::linalg::base_matrix<plll::arithmetic::Integer>::Enumerator iterr = mat.enumerate();
    cout << "mat";

    int i = 0, x, y;
    while (iterr.has_current())
    {
        x = i / dimm;
        y = i % dimm;
        iterr.current() = plll::arithmetic::Integer(B[x][y]);
        iterr.next();
        i++;
    }
    cout << mat << endl;
    plll::LatticeReduction lr;
    //cin >> mat;
    plll::linalg::math_matrix<plll::arithmetic::Integer> A(mat);
    lr.setLattice(A);
    lr.setSVPMode(plll::LatticeReduction::SVP_VoronoiCellSVP);
    cout << "mat";

    lr.setMinCallbackFunction(f(file, res), f_LI(file, res));
    lr.svp();
    file << "voronoi res " << lr.getLattice() << endl
         << endl;
    mat = lr.getLattice();
    iterr = mat.enumerate();
    for (int i = 0; i < dimm; i++)
    {
        res[i] = plll::arithmetic::convert<int>(iterr.current());
        iterr.next();
    }
    return res;
}

matrix_with_det readFromFile(ifstream &myfile)
{
    matrix_with_det res;
    string line;
    std::regex e("det.*");
    getline(myfile, line);
    // cout << line << endl;
    // while (!regex_match(line, e))
    // {
        if (myfile.eof())
        {
            res.det = 0;
            return res;
        }
    //     getline(myfile, line);
    //     cout << line << "k" << endl;
    // }
    std::regex e2("\\d+");
    std::smatch sm;

    // std::regex_search(line, sm, e2);
    int det = 3;
    // line = sm.suffix().str();
    // cout << "\ndet " << det;
    // std::regex_search(line, sm, e2);
    int dimm = 3;

    // std::regex_search(line, sm, e2);
    // int det = stoi(sm.str());
    // line = sm.suffix().str();
    // cout << "\ndet " << det;
    // std::regex_search(line, sm, e2);
    // int dimm = stoi(sm.str());

    Matrix<int> matr(dimm, vector<int>(dimm));
    cout << "\ndimm " << dimm;

    for (int i = 0; i < dimm; i++)
    {
        getline(myfile, line);
        int j = 0;
        while (std::regex_search(line, sm, e2))
        {
            matr[i][j++] = stoi(sm.str());
            line = sm.suffix().str();
        }
        cout << endl;
    }
    printVS(matr);
    res.det = det;
    res.mat = matr;
    return res;
}

void printResVector(vector<int> &v, string method, ostream &o)
{
    o << endl
      << method << " size: " << sqrt(squaredLengthOfVector(v)) << endl
      << "[ ";
    for (auto &a : v)
    {
        o << a << " ";
    }
    o << "] " << endl;
}

int main()
{

    ofstream myfile;

    ifstream datafile;
    myfile.open("example.txt");

    datafile.open("det3.txt");
    matrix_with_det md = readFromFile(datafile);
    cout << md.det;

    while (md.det != 0)
    {
        cout << "det" << md.det;

        Matrix<int> matrix = md.mat;
        Matrix<int> matrix2 = matrix;
        Matrix<int> matrix3 = matrix;
        Matrix<int> original(matrix2);
        Matrix<float> gramSmidt = gSchmidt(matrix);

        cout << "starting voronoi" << endl;
        auto t1 = std::chrono::high_resolution_clock::now();
        int dimm = matrix.size();
        cout << "t";
        auto vector_voronoi = shortestVectorByVoronoi(matrix, matrix.size(), myfile);
        auto t2 = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "voronoi duration " << duration << endl;
        myfile << "\n\n";

        float bound = 1;
        // bound = 1;
        cout << "starting gauss" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> sv = gaussSieve(matrix2, bound, myfile);
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "gauss duration: " << duration << endl;
        myfile << "\n\n";
        cout << "f\n\n";

        cout << "starting list" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> v3 = ListSieve(matrix3, bound, myfile);
        // auto vector_voronoi = v3;
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "list duration: " << duration << endl;

        bound = 1.01 * (sqrt(dimm)) * pow(abs(md.det), 1.0 / dimm);
        myfile << "bound " << bound << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> gauss_with_bound = gaussSieve(matrix2, bound, myfile);
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "gauss with bound duration: " << duration << endl;
        myfile << "\n\n";
        cout << "f\n\n";

        cout << "starting bound list" << endl;
        t1 = std::chrono::high_resolution_clock::now();
        vector<int> list_with_bound = ListSieve(matrix3, bound, myfile);
        // auto vector_voronoi = v3;
        t2 = std::chrono::high_resolution_clock::now();
        duration = std::chrono::duration_cast<sec_type>(t2 - t1).count();
        myfile << "list with bound duration: " << duration;

        if (squaredLengthOfVector(list_with_bound) != squaredLengthOfVector(gauss_with_bound))
        {
            myfile << "found different norms of vectors";
            printResVector(list_with_bound, "list with bound", myfile);
            printResVector(gauss_with_bound, "gauss with bound", myfile);
        }
        else
        {
            myfile << endl
                   << "shortest with bound norm" << (long)squaredLengthOfVector(gauss_with_bound) << endl;
            printVec(gauss_with_bound, myfile);
        }

        if (squaredLengthOfVector(vector_voronoi) != squaredLengthOfVector(sv) || squaredLengthOfVector(sv) != squaredLengthOfVector(vector_voronoi))
        {
            myfile << "found different norms of vectors";

            printResVector(vector_voronoi, "voronoi", myfile);
            printResVector(sv, "gauss", myfile);
            printResVector(v3, "list", myfile);
        }
        else
        {
            myfile << endl
                   << "shortest with norm" << (long)squaredLengthOfVector(sv) << endl;
            printVec(sv, myfile);
        }
        myfile << "---------------------------------------------------------------------\n\n";
        md = readFromFile(datafile);
    }

    // myfile << dimm << " " << diff << " " << iter << endl;

    //         }
    //     }
    // }
    // }
    // catch (exception e)
    // {
    //     cout << e.what();
    // }
    myfile.close();
    return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
using namespace std;
#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.h>
using Sophus::SE3;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 13.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题。
***********************************************/

// ------------------------------------------------------------------
// parameters
const int boarder = 20; 	// 边缘宽度
const int width = 640;  	// 宽度
const int height = 480;  	// 高度
const double fx = 481.2f;	// 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 2;	// NCC 取的窗口半宽度
const int ncc_area = (2*ncc_window_size+1)*(2*ncc_window_size+1); // NCC窗口面积
const double min_cov = 0.1;	// 收敛判定：最小方差
const double max_cov = 10;	// 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string& path,
    vector<string>& color_image_files,
    vector<SE3>& poses
);

// 根据新的图像更新深度估计
bool update(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);

// 极线搜索
bool epipolarSearch(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    const Vector2d& pt_ref,
    const double& depth_mu,
    const double& depth_cov,
    Vector2d& pt_curr
);

// 更新深度滤波器
bool updateDepthFilter(
    const Vector2d& pt_ref,
    const Vector2d& pt_curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);

// 计算 NCC 评分
double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d& pt_curr );

// 双线性灰度插值
inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) {
    uchar* d = & img.data[ int(pt(1,0))*img.step+int(pt(0,0)) ];
    double xx = pt(0,0) - floor(pt(0,0));
    double yy = pt(1,0) - floor(pt(1,0));
    return  (( 1-xx ) * ( 1-yy ) * double(d[0]) +
            xx* ( 1-yy ) * double(d[1]) +
            ( 1-xx ) *yy* double(d[img.step]) +
            xx*yy*double(d[img.step+1]))/255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
bool plotDepth( const Mat& depth );

// 像素到相机坐标系
inline Vector3d px2cam ( const Vector2d px ) {
    return Vector3d (
        (px(0,0) - cx)/fx,
        (px(1,0) - cy)/fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px ( const Vector3d p_cam ) {
    return Vector2d (
        p_cam(0,0)*fx/p_cam(2,0) + cx,
        p_cam(1,0)*fy/p_cam(2,0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside( const Vector2d& pt ) {
    return pt(0,0) >= boarder && pt(1,0)>=boarder
        && pt(0,0)+boarder<width && pt(1,0)+boarder<=height;
}

// 显示极线匹配
void showEpipolarMatch( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr );

// 显示极线
void showEpipolarLine( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr );
// ------------------------------------------------------------------

int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: dense_mapping path_to_test_dataset"<<endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3> poses_TWC;
    bool ret = readDatasetFiles( argv[1], color_image_files, poses_TWC );
    if ( ret==false )
    {
        cout<<"Reading image files failed!"<<endl;
        return -1;
    }
    cout<<"read total "<<color_image_files.size()<<" files."<<endl;

    // 第一张图
    Mat ref = imread( color_image_files[0], 0 );                // gray-scale image
    SE3 pose_ref_TWC = poses_TWC[0];
    double init_depth   = 3.0;    // 深度初始值
    double init_cov2    = 3.0;    // 方差初始值
    Mat depth( height, width, CV_64F, init_depth );             // 深度图
    Mat depth_cov( height, width, CV_64F, init_cov2 );          // 深度图方差

    for ( int index=1; index<color_image_files.size(); index++ )
    {
        cout<<"*** loop "<<index<<" ***"<<endl;
        Mat curr = imread( color_image_files[index], 0 );
        if (curr.data == nullptr) continue;
        SE3 pose_curr_TWC = poses_TWC[index];
        SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update( ref, curr, pose_T_C_R, depth, depth_cov );
        plotDepth( depth );
        imshow("image", curr);
        waitKey(1);
    }

    cout<<"estimation returns, saving depth map ..."<<endl;
    imwrite( "depth.png", depth );
    cout<<"done."<<endl;

    return 0;
}

bool readDatasetFiles(
    const string& path,
    vector< string >& color_image_files,
    std::vector<SE3>& poses
)
{
    ifstream fin( path+"/first_200_frames_traj_over_table_input_sequence.txt");
    if ( !fin ) return false;

    while ( !fin.eof() )
    {
		// 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin>>image;
        double data[7];
        for ( double& d:data ) fin>>d;

        color_image_files.push_back( path+string("/images/")+image );
        poses.push_back(
            SE3( Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if ( !fin.good() ) break;
    }
    return true;
}

// 对整个深度图进行更新
bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov )
{
#pragma omp parallel for
    for ( int x=boarder; x<width-boarder; x++ )
#pragma omp parallel for
        for ( int y=boarder; y<height-boarder; y++ )
        {
			// 遍历每个像素
            if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            bool ret = epipolarSearch (
                ref,
                curr,
                T_C_R,
                Vector2d(x,y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov.ptr<double>(y)[x]),
                pt_curr
            );

            if ( ret == false ) // 匹配失败
                continue;

			// 取消该注释以显示匹配
            // showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr );

            // 匹配成功，更新深度图
            updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov );
        }
}

// 极线搜索
// 方法见书 13.2 13.3 两节
bool epipolarSearch(
    const Mat& ref, const Mat& curr,
    const SE3& T_C_R, const Vector2d& pt_ref,
    const double& depth_mu, const double& depth_cov,
    Vector2d& pt_curr )
{
    Vector3d f_ref = px2cam( pt_ref );
    f_ref.normalize();
    Vector3d P_ref = f_ref*depth_mu;	// 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按深度均值投影的像素
    double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov;
    if ( d_min<0.1 ) d_min = 0.1;
    Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) );	// 按最小深度投影的像素
    Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) );	// 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;	// 极线（线段形式）
    Vector2d epipolar_direction = epipolar_line;		// 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5*epipolar_line.norm();	// 极线线段的半长度
    if ( half_length>100 ) half_length = 100;   // 我们不希望搜索太多东西

	// 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for ( double l=-half_length; l<=half_length; l+=0.7 )  // l+=sqrt(2)
    {
        Vector2d px_curr = px_mean_curr + l*epipolar_direction;  // 待匹配点
        if ( !inside(px_curr) )
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC( ref, curr, pt_ref, px_curr );
        if ( ncc>best_ncc )
        {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if ( best_ncc < 0.85f )      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC (
    const Mat& ref, const Mat& curr,
    const Vector2d& pt_ref, const Vector2d& pt_curr
)
{
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for ( int x=-ncc_window_size; x<=ncc_window_size; x++ )
        for ( int y=-ncc_window_size; y<=ncc_window_size; y++ )
        {
            double value_ref = double(ref.ptr<uchar>( int(y+pt_ref(1,0)) )[ int(x+pt_ref(0,0)) ])/255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue( curr, pt_curr+Vector2d(x,y) );
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

	// 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for ( int i=0; i<values_ref.size(); i++ )
    {
        double n = (values_ref[i]-mean_ref) * (values_curr[i]-mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i]-mean_ref)*(values_ref[i]-mean_ref);
        demoniator2 += (values_curr[i]-mean_curr)*(values_curr[i]-mean_curr);
    }
    return numerator / sqrt( demoniator1*demoniator2+1e-10 );   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d& pt_ref,
    const Vector2d& pt_curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
)
{
    // 我是一只喵
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3 T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam( pt_ref );
    f_ref.normalize();
    Vector3d f_curr = px2cam( pt_curr );
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
    //    [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
    // 二阶方程用克莱默法则求解并解之
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
    Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
    double A[4];
    A[0] = f_ref.dot ( f_ref );
    A[2] = f_ref.dot ( f2 );
    A[1] = -A[2];
    A[3] = - f2.dot ( f2 );
    double d = A[0]*A[3]-A[1]*A[2];
    Vector2d lambdavec =
        Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
                    -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
    Vector3d xm = lambdavec ( 0,0 ) * f_ref;
    Vector3d xn = t + lambdavec ( 1,0 ) * f2;
    Vector3d d_esti = ( xm+xn ) / 2.0;  // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();   // 深度值

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref*depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos( f_ref.dot(t)/t_norm );
    double beta = acos( -a.dot(t)/(a_norm*t_norm));
    double beta_prime = beta + atan(1/fx);
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov*d_cov;

    // 高斯融合
    double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
    double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];

    double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
    double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );

    depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse;
    depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;

    return true;
}

// 后面这些太简单我就不注释了（其实是因为懒）
bool plotDepth(const Mat& depth)
{
    imshow( "depth", depth*0.4 );
    waitKey(1);
}

void showEpipolarMatch(const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr)
{
    Mat ref_show, curr_show;
    cv::cvtColor( ref, ref_show, CV_GRAY2BGR );
    cv::cvtColor( curr, curr_show, CV_GRAY2BGR );

    cv::circle( ref_show, cv::Point2f(px_ref(0,0), px_ref(1,0)), 5, cv::Scalar(0,0,250), 2);
    cv::circle( curr_show, cv::Point2f(px_curr(0,0), px_curr(1,0)), 5, cv::Scalar(0,0,250), 2);

    imshow("ref", ref_show );
    imshow("curr", curr_show );
    waitKey(1);
}

void showEpipolarLine(const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr)
{

    Mat ref_show, curr_show;
    cv::cvtColor( ref, ref_show, CV_GRAY2BGR );
    cv::cvtColor( curr, curr_show, CV_GRAY2BGR );

    cv::circle( ref_show, cv::Point2f(px_ref(0,0), px_ref(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::circle( curr_show, cv::Point2f(px_min_curr(0,0), px_min_curr(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::circle( curr_show, cv::Point2f(px_max_curr(0,0), px_max_curr(1,0)), 5, cv::Scalar(0,255,0), 2);
    cv::line( curr_show, Point2f(px_min_curr(0,0), px_min_curr(1,0)), Point2f(px_max_curr(0,0), px_max_curr(1,0)), Scalar(0,255,0), 1);

    imshow("ref", ref_show );
    imshow("curr", curr_show );
    waitKey(1);
}


#include <ceres/ceres.h>
#include <ceres/gradient_checker.h>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "wave/optimization/ceres/local_params/spherical_parameterization.hpp"
#include "wave/wave_test.hpp"
#include "wave/utils/math.hpp"
#include "wave/geometry_og/transformation.hpp"

namespace {

class TestSpherical : public ceres::SizedCostFunction<3, 3> {
 private:
    const double *const normal;

 public:
    virtual ~TestSpherical() {};

    TestSpherical(const double *const normal) : normal(normal) {}

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
        // Want to align the optimization parameter with the normal, error is a - b

        residuals[0] = -normal[0] + parameters[0][0];
        residuals[1] = -normal[1] + parameters[0][1];
        residuals[2] = -normal[2] + parameters[0][2];

        if (jacobians) {
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J(jacobians[0]);
            J.setIdentity();
        }
        return true;
    }
};

}

namespace wave {

TEST(easy_cost_function, alignment) {
    Vec3 normal;
    normal << -1, 2, 4;
    normal.normalize();

    Vec3 initial;
    initial.setRandom();
    initial.normalize();

    ceres::Problem problem;
    ceres::CostFunction *cost = new TestSpherical(normal.data());
    problem.AddResidualBlock(cost, nullptr, initial.data());

    ceres::LocalParameterization *spherical = new SphericalParameterization();
    problem.SetParameterization(initial.data(), spherical);

    ceres::Solver::Options options;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport();

    EXPECT_LT((initial - normal).norm(), 1e-6);
    EXPECT_NEAR(initial.norm(), 1.0, 1e-6);
}

}


//////////////////////////////////////////////////////////////////////////////
// monomials_horner::policy::power2divfact.hpp                              //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MONOMIALS_HORNER_POLICY_MULTI_POWER2DIVFACT_HPP_ER_2009
#define BOOST_MONOMIALS_HORNER_POLICY_MULTI_POWER2DIVFACT_HPP_ER_2009
#include <boost/math/special_functions/factorials.hpp>

namespace boost{
namespace monomials_horner{

    // (a,b,c) --> 2^{a+b+c}/(a! b! c!)
    struct multi_power2divfact{
        typedef double                      result_type;
        typedef double                      first_argument_type;
        typedef unsigned                    second_argument_type;
        multi_power2divfact(){}
        result_type operator()(
            first_argument_type a1,
            second_argument_type a2
        )const{
            result_type d = std::pow(2.0,static_cast<result_type>(a2));
            result_type f = math::factorial<result_type>(a2);
            result_type r = d/f;
            return a1 * static_cast<result_type>(r);
        }
        static result_type initial_value;
    };
    multi_power2divfact::result_type multi_power2divfact::initial_value
        = static_cast<result_type>(1);

}// monomials_horner
}// boost

#endif


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/reverse.hpp>
#include <autodiff/reverse/eigen.hpp>
using namespace autodiff;

// The scalar function for which the gradient is needed
var f(const VectorXvar& x)
{
    return sqrt(x.cwiseProduct(x).sum()); // sqrt(sum([x(i) * x(i) for i = 1:5]))
}

int main()
{
    VectorXvar x(5);     // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    var u = f(x);  // the output variable u

    VectorXd g;  // the gradient vector to be computed in method `hessian`
    MatrixXd H = hessian(u, x, g);  // evaluate the Hessian matrix H and the gradient vector g of u

    cout << "u = " << u << endl;    // print the evaluated output variable u
    cout << "g = \n" << g << endl;  // print the evaluated gradient vector of u
    cout << "H = \n" << H << endl;  // print the evaluated Hessian matrix of u
}


#include "forward_kinematics.h"
#include <iostream>
#include <stdio.h>
#include <Eigen/Dense>
#include <unordered_map>
#include <bitset>
#include <string>
#include <utility>
//perform the forward kinematics of the robot to figure out where you are
wsState* fk (configState* c)
{
  double theta0 = tick_to_radians(c->theta[0]);
  double theta1 = tick_to_radians(c->theta[1]);
  double theta2 = tick_to_radians(c->theta[2]);
  double theta3 = tick_to_radians(c->theta[3]);
  double theta4 = tick_to_radians(c->theta[4]);
  /*  Eigen::Matrix4d T_01 = DH(M_PI/2 , 0, 26.5, theta0);
  Eigen::Matrix4d T_12 = DH(0, 150, 0 , M_PI/2+theta1);
  Eigen::Matrix4d T_23 = DH(0, 150, 0, theta2);
  Eigen::Matrix4d T_34 = DH(-M_PI/2, 0, 0, M_PI/2+theta3);
  Eigen::Matrix4d T_45 = DH(0,0,116.525, theta4);

  Eigen::Matrix4d T_05 = T_01*T_12*T_23*T_34*T_45;

  wsState* outstate = new wsState;
  outstate->x = T_05(0,3);
  outstate->y = T_05(1,3);
  outstate->z = T_05(2,3);
  outstate->alpha = tick_to_radians(c->theta[0]+c->theta[1]+
      c->theta[2]+c->theta[3]+c->theta[4]);
  //TODO calculate angles based on rotation matrix
  */
  double c0 = cos(theta0);
  double s0 = sin(theta0);
  double c1 = cos(theta1);
  double s1 = sin(theta1);
  double c12 = cos(theta1+theta2);
  double s12 = sin(theta1+theta2);
  double c123 = cos(theta1+theta2+theta3);
  double s123 = sin(theta1+theta2+theta3);

  wsState* outstate = new wsState;
  outstate->x = c0*(150*(c1+c12) + 116.525*s123);
  outstate->y = s0*(150*(c1+c12) + 116.525*s123);
  outstate->z = 150*(s12+s1) - 116.525*c123 + 26.5;
  outstate->alpha = theta1+ theta2+ theta3;

  return outstate;

}//end forward_kinematics

Eigen::Matrix4d DH(double alpha,double a,double d,double theta)
{
  Eigen::Matrix4d T;
  T << cos(theta),sin(theta)*cos(alpha),-sin(theta)*sin(alpha),a*cos(theta),
    sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha),a*sin(theta),
    0,sin(alpha),cos(alpha),d,
    0,0,0,1;
  return T;
}

//calculate Euclidean ndistance between two states
double distance(wsState* s1,wsState* s2)
{
  return (pow(s1->x - s2->x,2)+pow(s1->y-s2->y,2)+pow(s1->z-s2->z,2)+pow(s1->alpha-s2->alpha,2));
}

configState* clone_configstate(configState* c){

  configState* clone = new configState;
  clone->theta[0] = c->theta[0];
  clone->theta[1] = c->theta[1];
  clone->theta[2] = c->theta[2];
  clone->theta[3] = c->theta[3];
  clone->theta[4] = c->theta[4];
  return clone;
}

double tick_to_radians(int i){
  return (M_PI/100)*i;
}

wsState* create_wsstate(){
   return (wsState*)malloc(sizeof(struct wsState));
}

configState* create_configstate(){
  return (configState*)malloc(sizeof(struct configState));
}

visData* create_visdata(){
  return (visData*)malloc(sizeof(struct visData));
}

void deallocate_wsstate(wsState* w){
  if(w != 0)
    free(w);
}

void deallocate_configstate(configState* c){
  if(c != 0)
    free(c);
}

void deallocate_visdata(visData* v){
  if(v != 0)
    free(v);
}

void wsstate_tostring(wsState* w){
    printf("Work Space State: x:%f, y:%f, z:%f\n",
      w->x, w->y, w->z);
}

void configstate_tostring(configState* c){
  printf("Theta1: %i, Theta2: %i, Theta3: %i, Theta4: %i, Theta5: %i\n"
      , c->theta[0], c->theta[1],c->theta[2], c->theta[3], c->theta[4]);
}


//Neural network in C++
// Author: Sethu Iyer
//Sigmoid is used because the sigmoid function assumes
//minimal structure and reflects our general state of ignorance about the underlying model.

//This code implements simple 3 layer feed forward neural network with no regularization.
// we use fast sigmoid approximation to speed up the calculation.

#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

class Neural_Network
{
   private:
    //define the architecture of the network
    int inputLayerSize = 2;
    int  outputLayerSize = 1;
    int hiddenLayerSize = 3;

    // activation function and the weights
    arma::mat W1,W2;
    arma::mat z2,a2;
    arma::mat z3,a3;
    arma::mat delta3,delta2;
    arma::mat dJdW1, djdW2;

  public:
       Neural_Network()
     {
          //fills in random weights
        W1.randu(2,3);
        W2.randu(3,1);
    }

      //helper functions
        static double sigmoid(double x)
    {
      return x / (1 + abs(x));
    }
    static double sigmoidPrime(double x)
    {
      double f = sigmoid(x);
      return f * (1-f);
    }

    //forwarding function.
    arma::mat forward(arma::mat X)
    {
        z2 = X * W1;
        a2 = z2;
        a2.transform(*sigmoid);
        z3 = a2 * W2;
        a3 = z3;
        a3.transform(*sigmoid);
        return a3;
    }

    //cost function
    double costFunction(mat X,mat y)
    {
      mat yHat = forward(X);

      double cost = 0;
      for(int i = 0; i < outputLayerSize; i++)
      {
        cost =  cost + (yHat[i] - y[i])*(yHat[i] - y[i]);
      }
      cost = cost / 2;
      return cost;

    }

    //derivative of cost function
    //backpropogation of errors can be understood in this code
    void computeGradients(mat X, mat y)
    {

      mat yHat = forward(X);
      mat z = z3;
      z.transform(*sigmoidPrime);
      delta3 = (yHat - y.t()) % z;
      djdW2 = a2.t() * delta3;

      z = z2;
      z.transform(*sigmoidPrime);
      delta2 = delta3 * W2.t()*z;
      dJdW1 = X.t() * delta2;

    }

    //alpha is our learning rate
    void gradientDescent(mat X,mat y,double alpha,int numIter)
    {
      int i;
      mat transdj1,transdj2;
      for(i=0;i<numIter;i++)
      {
        computeGradients(X,y);
        transdj1 = dJdW1;
        transdj1.transform([=] (double x) { return x * alpha;});
        W1 = W1 - transdj1;
    transdj2 = djdW2;
        transdj2.transform([=] (double x ) { return x *alpha;});
        W2 = W2 - transdj2;
      }
    }
};

int main()
{

    mat X = {{0.3,1},{0.5,0.2},{1,0.4}};
    mat y = {0.75,0.82,0.93};
    Neural_Network nn;
    cout<<"Initial Cost: "<<nn.costFunction(X,y)<<endl;
    nn.gradientDescent(X,y,0.01,400);
    cout<<"After 400 Iterations: "<<nn.costFunction(X,y)<<endl;
    nn.gradientDescent(X,y,0.01,400);
    cout<<"After 800 Iterations: "<<nn.costFunction(X,y)<<endl;
    return 0;
}

3-Layer Neural Network in C++/neuralnet.cpp


//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0 ; // 第i个数据点的计算误差
            // 填写计算error的表达式
            error = yi- exp(ae * xi * xi + be * xi + ce);
            Vector3d J; // 雅可比矩阵
            J[0] = -exp(ae * xi * xi + be * xi + ce)*xi*xi;  // de/da
            J[1] = -exp(ae * xi * xi + be * xi + ce)*xi;  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;
        dx = H.ldlt().solve(b);
	// end your code here

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}


#include <armadillo>
#include <cstring>
#include <iostream>

using namespace std;
using namespace arma;

/*  ----------------- useful help functions --------------  */

struct MyVec
{
    double *data;
    size_t n;
};

struct MyMat
{
    double *data;
    size_t num_rows, num_cols;
};

extern "C" void freeMyVec(MyVec &vec)
{
    delete[] vec.data;
    memset((void *)&vec, 0, sizeof(vec));
}

extern "C" void freeMyMat(MyMat &mat)
{
    delete[] mat.data;
    memset((void *)&mat, 0, sizeof(mat));
}

struct HuberCovResult
{
    MyVec means;
    MyMat cov;
};

extern "C" void freeHuberCovResult(const HuberCovResult &result)
{
    delete[] result.means.data;
    delete[] result.cov.data;
    memset((void *)&result, 0, sizeof(result));
}

              /* --------------------------------- */
/*------------ Tuning-Free Huber Regression Funtions ---------------*/
              /* --------------------------------- */

int sgn(const double x)
{
    return (x > 0) - (x < 0);
}

double f1(const double x, const arma::vec &resSq, const int n, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(n))) - rhs;
}

double rootf1(const arma::vec &resSq, const int n, const double rhs, double low, double up, const double tol = 0.001, const int maxIte = 500)
{
    int ite = 1;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = f1(mid, resSq, n, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double f2(const double x, const arma::vec &resSq, const int N, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(N))) - rhs;
}

double rootf2(const arma::vec &resSq, const int n, const int d, const int N, const double rhs, double low, double up, const double tol = 0.001,
              const int maxIte = 500)
{
    int ite = 0;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = f2(mid, resSq, N, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double g1(const double x, const arma::vec &resSq, const int n, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(n))) - rhs;
}

double rootg1(const arma::vec &resSq, const int n, const double rhs, double low, double up, const double tol = 0.001, const int maxIte = 500)
{
    int ite = 0;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = g1(mid, resSq, n, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double huberDer(const arma::vec &res, const double tau, const int n)
{
    double rst = 0.0;
    for (int i = 0; i < n; i++)
    {
        double cur = res(i);
        rst -= std::abs(cur) <= tau ? cur : tau * sgn(cur);
    }
    return rst / n;
}

extern "C" double huberMean(MyVec _X, const double tol = 0.001, const int iteMax = 500)
{
    arma::vec X(_X.data, _X.n);
    int n = X.n_rows;
    double rhs = std::log(n) / n;
    double mx = arma::mean(X);
    X -= mx;
    double tau = arma::stddev(X) * std::sqrt((long double)n / std::log(n));
    double derOld = huberDer(X, tau, n);
    double mu = -derOld, muDiff = -derOld;
    arma::vec res = X - mu;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    double derNew = huberDer(res, tau, n);
    double derDiff = derNew - derOld;
    int ite = 1;
    while (std::abs(derNew) > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = muDiff * derDiff;
        if (cross > 0)
        {
            double a1 = cross / derDiff * derDiff;
            double a2 = muDiff * muDiff / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        derOld = derNew;
        muDiff = -alpha * derNew;
        mu += muDiff;
        res = X - mu;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        derNew = huberDer(res, tau, n);
        derDiff = derNew - derOld;
        ite++;
    }
    return mu + mx;
}

double _huberMean(arma::vec X, const double tol = 0.001, const int iteMax = 500)
{
    int n = X.n_rows;
    double rhs = std::log(n) / n;
    double mx = arma::mean(X);
    X -= mx;
    double tau = arma::stddev(X) * std::sqrt((long double)n / std::log(n));
    double derOld = huberDer(X, tau, n);
    double mu = -derOld, muDiff = -derOld;
    arma::vec res = X - mu;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    double derNew = huberDer(res, tau, n);
    double derDiff = derNew - derOld;
    int ite = 1;
    while (std::abs(derNew) > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = muDiff * derDiff;
        if (cross > 0)
        {
            double a1 = cross / derDiff * derDiff;
            double a2 = muDiff * muDiff / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        derOld = derNew;
        muDiff = -alpha * derNew;
        mu += muDiff;
        res = X - mu;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        derNew = huberDer(res, tau, n);
        derDiff = derNew - derOld;
        ite++;
    }
    return mu + mx;
}

double hMeanCov(const arma::vec &Z, const int n, const int d, const int N, double rhs, const double epsilon = 0.0001, const int iteMax = 500)
{
    double muOld = 0;
    double muNew = arma::mean(Z);
    double tau = arma::stddev(Z) * std::sqrt((long double)n / (2 * std::log(d) + std::log(n)));
    int iteNum = 0;
    arma::vec res(n), resSq(n), w(n);
    while ((std::abs(muNew - muOld) > epsilon) && iteNum < iteMax)
    {
        muOld = muNew;
        res = Z - muOld;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf2(resSq, n, d, N, rhs, arma::min(resSq), arma::accu(resSq)));
        w = arma::min(tau / arma::abs(res), arma::ones(N));
        muNew = arma::as_scalar(Z.t() * w) / arma::accu(w);
        iteNum++;
    }
    return muNew;
}

extern "C" HuberCovResult huberCov(const MyMat &_X)
{
    const arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    const int n = X.n_rows;
    const int p = X.n_cols;

    double rhs2 = (2 * std::log(p) + std::log(n)) / n;
    arma::vec mu(p);
    arma::mat sigmaHat(p, p);
    for (int j = 0; j < p; j++)
    {
        mu(j) = _huberMean(X.col(j), n);
        double theta = _huberMean(arma::square(X.col(j)), n);
        double temp = mu(j) * mu(j);
        if (theta > temp)
        {
            theta -= temp;
        }
        sigmaHat(j, j) = theta;
    }
    int N = n * (n - 1) >> 1;
    arma::mat Y(N, p);
    for (int i = 0, k = 0; i < n - 1; i++)
    {
        for (int j = i + 1; j < n; j++)
        {
            Y.row(k++) = X.row(i) - X.row(j);
        }
    }
    for (int i = 0; i < p - 1; i++)
    {
        for (int j = i + 1; j < p; j++)
        {
            sigmaHat(i, j) = sigmaHat(j, i) = hMeanCov(0.5 * Y.col(i) % Y.col(j), n, p, N, rhs2);
        }
    }

    HuberCovResult result;

    result.means.data = new double[mu.size()];
    result.means.n = mu.size();
    memcpy(result.means.data, mu.memptr(), mu.size() * sizeof(double));

    result.cov.data = new double[sigmaHat.n_rows * sigmaHat.n_cols];
    result.cov.num_rows = sigmaHat.n_rows;
    result.cov.num_cols = sigmaHat.n_cols;
    memcpy(result.cov.data, sigmaHat.memptr(), sigmaHat.n_rows * sigmaHat.n_cols * sizeof(double));

    return result;
}

double mad(const arma::vec &x)
{
    return 1.482602 * arma::median(arma::abs(x - arma::median(x)));
}

arma::mat standardize(arma::mat X, const arma::rowvec &mx, const arma::vec &sx, const int p)
{
    for (int i = 0; i < p; i++)
    {
        X.col(i) = (X.col(i) - mx(i)) / sx(i);
    }
    return X;
}

void updateHuber(const arma::mat &Z, const arma::vec &res, arma::vec &der, arma::vec &grad, const int n, const double tau, const double n1)
{
    for (int i = 0; i < n; i++)
    {
        double cur = res(i);
        if (std::abs(cur) <= tau)
        {
            der(i) = -cur;
        }
        else
        {
            der(i) = -tau * sgn(cur);
        }
    }
    grad = n1 * Z.t() * der;
}

extern "C" MyVec adaHuberReg(MyMat _X, MyVec _Y, const double tol = 0.0001, const int iteMax = 5000)
{
    arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    arma::vec Y(_Y.data, _Y.n);
    int n = X.n_rows;
    int p = X.n_cols;
    const double n1 = 1.0 / n;
    double rhs = n1 * (p + std::log(n * p));
    arma::rowvec mx = arma::mean(X, 0);
    arma::vec sx = arma::stddev(X, 0, 0).t();
    double my = arma::mean(Y);
    arma::mat Z = arma::join_rows(arma::ones(n), standardize(X, mx, sx, p));
    Y -= my;
    double tau = 1.345 * mad(Y);
    arma::vec der(n);
    arma::vec gradOld(p + 1), gradNew(p + 1);
    updateHuber(Z, Y, der, gradOld, n, tau, n1);
    arma::vec beta = -gradOld, betaDiff = -gradOld;
    arma::vec res = Y - Z * beta;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootg1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    updateHuber(Z, res, der, gradNew, n, tau, n1);
    arma::vec gradDiff = gradNew - gradOld;
    int ite = 1;
    while (arma::norm(gradNew, "inf") > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = arma::as_scalar(betaDiff.t() * gradDiff);
        if (cross > 0)
        {
            double a1 = cross / arma::as_scalar(gradDiff.t() * gradDiff);
            double a2 = arma::as_scalar(betaDiff.t() * betaDiff) / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        gradOld = gradNew;
        betaDiff = -alpha * gradNew;
        beta += betaDiff;
        res -= Z * betaDiff;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootg1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        updateHuber(Z, res, der, gradNew, n, tau, n1);
        gradDiff = gradNew - gradOld;
        ite++;
    }
    beta.rows(1, p) /= sx;
    beta(0) = _huberMean(Y + my - X * beta.rows(1, p));

    MyVec result;
    result.data = new double[n];
    result.n = n;
    memcpy(result.data, beta.memptr(), n * sizeof(double));
    return result;
}

extern "C" MyVec huberReg(MyMat _X, MyVec _Y, const double tol = 0.0001, const double constTau = 1.345, const int iteMax = 5000)
{
    arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    arma::vec Y(_Y.data, _Y.n);
    int n = X.n_rows;
    int p = X.n_cols;
    const double n1 = 1.0 / n;
    arma::rowvec mx = arma::mean(X, 0);
    arma::vec sx = arma::stddev(X, 0, 0).t();
    double my = arma::mean(Y);
    arma::mat Z = arma::join_rows(arma::ones(n), standardize(X, mx, sx, p));
    Y -= my;
    double tau = constTau * mad(Y);
    arma::vec der(n);
    arma::vec gradOld(p + 1), gradNew(p + 1);
    updateHuber(Z, Y, der, gradOld, n, tau, n1);
    arma::vec beta = -gradOld, betaDiff = -gradOld;
    arma::vec res = Y - Z * beta;
    tau = constTau * mad(res);
    updateHuber(Z, res, der, gradNew, n, tau, n1);
    arma::vec gradDiff = gradNew - gradOld;
    int ite = 1;
    while (arma::norm(gradNew, "inf") > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = arma::as_scalar(betaDiff.t() * gradDiff);
        if (cross > 0)
        {
            double a1 = cross / arma::as_scalar(gradDiff.t() * gradDiff);
            double a2 = arma::as_scalar(betaDiff.t() * betaDiff) / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        gradOld = gradNew;
        betaDiff = -alpha * gradNew;
        beta += betaDiff;
        res -= Z * betaDiff;
        tau = constTau * mad(res);
        updateHuber(Z, res, der, gradNew, n, tau, n1);
        gradDiff = gradNew - gradOld;
        ite++;
    }
    beta.rows(1, p) /= sx;
    beta(0) = _huberMean(Y + my - X * beta.rows(1, p), n);

    MyVec result;
    result.data = new double[n];
    result.n = n;
    memcpy(result.data, beta.memptr(), n * sizeof(double));
    return result;
}

              /* - */
/*------------ LASSO ------------*/
              /* - */

arma::vec softThresh(const arma::vec& x, const arma::vec& lambda)
{
  return arma::sign(x) % arma::max(arma::abs(x) - lambda, arma::zeros(x.size()));
}

arma::vec cmptLambda(const arma::vec& beta, const double lambda)
{
  arma::vec rst = lambda * arma::ones(beta.size());
  rst(0) = 0;
  return rst;
}

double loss(const arma::vec& Y, const arma::vec& Ynew, const std::string lossType, const double tau)
{
  double rst = 0;
  if (lossType == "l2") {
    rst = arma::mean(arma::square(Y - Ynew)) / 2;
  } else if (lossType == "Huber") {
    arma::vec res = Y - Ynew;
    for (int i = 0; i < Y.size(); i++) {
      if (std::abs(res(i)) <= tau) {
        rst += res(i) * res(i) / 2;
      } else {
        rst += tau * std::abs(res(i)) - tau * tau / 2;
      }
    }
    rst /= Y.size();
  }
  return rst;
}

arma::vec gradLoss(const arma::mat& X, const arma::vec& Y, const arma::vec& beta,
                   const std::string lossType, const double tau)
{
  arma::vec res = Y - X * beta;
  arma::vec rst = arma::zeros(beta.size());
  if (lossType == "l2") {
    rst = -1 * (res.t() * X).t();
  } else if (lossType == "Huber") {
    for (int i = 0; i < Y.size(); i++) {
      if (std::abs(res(i)) <= tau) {
        rst -= res(i) * X.row(i).t();
      } else {
        rst -= tau * sgn(res(i)) * X.row(i).t();
      }
    }
  }
  return rst / Y.size();
}

arma::vec updateBeta(const arma::mat& X, const arma::vec& Y, arma::vec beta, const double phi,
                     const arma::vec& Lambda, const std::string lossType, const double tau)
{
  arma::vec first = beta - gradLoss(X, Y, beta, lossType, tau) / phi;
  arma::vec second = Lambda / phi;
  return softThresh(first, second);
}

double cmptPsi(const arma::mat& X, const arma::vec& Y, const arma::vec& betaNew,
               const arma::vec& beta, const double phi, const std::string lossType,
               const double tau)
{
  arma::vec diff = betaNew - beta;
  double rst = loss(Y, X * beta, lossType, tau)
    + arma::as_scalar((gradLoss(X, Y, beta, lossType, tau)).t() * diff)
    + phi * arma::as_scalar(diff.t() * diff) / 2;
    return rst;
}

Rcpp::List LAMM(const arma::mat& X, const arma::vec& Y, const arma::vec& Lambda, arma::vec beta,
                const double phi, const std::string lossType, const double tau,
                const double gamma)
{
  double phiNew = phi;
  arma::vec betaNew = arma::vec();
  double FVal, PsiVal;
  while (true) {
    betaNew = updateBeta(X, Y, beta, phiNew, Lambda, lossType, tau);
    FVal = loss(Y, X * betaNew, lossType, tau);
    PsiVal = cmptPsi(X, Y, betaNew, beta, phiNew, lossType, tau);
    if (FVal <= PsiVal) {
      break;
    }
    phiNew *= gamma;
  }
  return Rcpp::List::create(Rcpp::Named("beta") = betaNew, Rcpp::Named("phi") = phiNew);
}

arma::vec lasso(const arma::mat& X, const arma::vec& Y, const double lambda,
                const double phi0 = 0.001, const double gamma = 1.5,
                const double epsilon_c = 0.001, const int iteMax = 500)
{
  int d = X.n_cols - 1;
  arma::vec beta = arma::zeros(d + 1);
  arma::vec betaNew = arma::zeros(d + 1);
  arma::vec Lambda = cmptLambda(beta, lambda);
  double phi = phi0;
  int ite = 0;
  Rcpp::List listLAMM;
  while (ite < iteMax) {
    ite++;
    listLAMM = LAMM(X, Y, Lambda, beta, phi, "l2", 1, gamma);
    betaNew = Rcpp::as<arma::vec>(listLAMM["beta"]);
    phi = listLAMM["phi"];
    phi = std::max(phi0, phi / gamma);
    if (arma::norm(betaNew - beta, "inf") <= epsilon_c) {
      break;
    }
    beta = betaNew;
  }
  return betaNew;
}

Rcpp::List huberLasso(const arma::mat& X, const arma::vec& Y, const double lambda,
                      double tau = -1, const double constTau = 1.345, const double phi0 = 0.001,
                      const double gamma = 1.5, const double epsilon_c = 0.001,
                      const int iteMax = 500)
{
  int n = X.n_rows;
  int d = X.n_cols - 1;
  arma::vec beta = arma::zeros(d + 1);
  arma::vec betaNew = arma::zeros(d + 1);
  arma::vec Lambda = cmptLambda(beta, lambda);
  double mad;
  if (tau <= 0) {
    arma::vec betaLasso = lasso(X, Y, lambda, phi0, gamma, epsilon_c, iteMax);
    arma::vec res = Y - X * betaLasso;
    mad = arma::median(arma::abs(res - arma::median(res))) / 0.6744898;
    tau = constTau * mad;
  }
  double phi = phi0;
  int ite = 0;
  Rcpp::List listLAMM;
  arma::vec res(n);
  while (ite < iteMax) {
    ite++;
    listLAMM = LAMM(X, Y, Lambda, beta, phi, "Huber", tau, gamma);
    betaNew = Rcpp::as<arma::vec>(listLAMM["beta"]);
    phi = listLAMM["phi"];
    phi = std::max(phi0, phi / gamma);
    if (arma::norm(betaNew - beta, "inf") <= epsilon_c) {
      break;
    }
    beta = betaNew;
    res = Y - X * beta;
    mad = arma::median(arma::abs(res - arma::median(res))) / 0.6744898;
    tau = constTau * mad;
  }
  return Rcpp::List::create(Rcpp::Named("beta") = betaNew, Rcpp::Named("tau") = tau,
                            Rcpp::Named("iteration") = ite);
}

arma::uvec getIndex(const int n, const int low, const int up)
{
  arma::vec seq = arma::regspace(0, n - 1);
  return arma::find(seq >= low && seq <= up);
}

arma::uvec getIndexComp(const int n, const int low, const int up)
{
  arma::vec seq = arma::regspace(0, n - 1);
  return arma::find(seq < low || seq > up);
}

double pairPred(const arma::mat& X, const arma::vec& Y, const arma::vec& beta)
{
  int n = X.n_rows;
  int d = X.n_cols - 1;
  int m = n * (n - 1) >> 1;
  arma::mat pairX(m, d + 1);
  arma::vec pairY(m);
  int k = 0;
  for (int i = 0; i < n; i++) {
    for (int j = i + 1; j < n; j++) {
      pairX.row(k) = X.row(i) - X.row(j);
      pairY(k++) = Y(i) - Y(j);
    }
  }
  arma::vec predY = pairX * beta;
  return arma::accu(arma::square(pairY - predY));
}

Rcpp::List cvHuberLasso(const arma::mat& X, const arma::vec& Y,
                        Rcpp::Nullable<Rcpp::NumericVector> lSeq = R_NilValue, int nlambda = 30,
                        const double constTau = 2.5, const double phi0 = 0.001,
                        const double gamma = 1.5, const double epsilon_c = 0.001,
                        const int iteMax = 500, int nfolds = 3)
{
  int n = X.n_rows;
  int d = X.n_cols;
  arma::mat Z(n, d + 1);
  Z.cols(1, d) = X;
  Z.col(0) = arma::ones(n);
  arma::vec lambdaSeq = arma::vec();
  if (lSeq.isNotNull()) {
    lambdaSeq = Rcpp::as<arma::vec>(lSeq);
    nlambda = lambdaSeq.size();
  } else {
    double lambdaMax = arma::max(arma::abs(Y.t() * Z)) / n;
    double lambdaMin = 0.01 * lambdaMax;
    lambdaSeq = arma::exp(arma::linspace(std::log((long double)lambdaMin),
                                   std::log((long double)lambdaMax), nlambda));
  }
  if (nfolds > 10 || nfolds > n) {
    nfolds = n < 10 ? n : 10;
  }
  int size = n / nfolds;
  arma::vec mse = arma::zeros(nlambda);
  int low, up;
  arma::uvec idx, idxComp;
  Rcpp::List hLassoList;
  arma::vec thetaHat(d + 1);
  for (int i = 0; i < nlambda; i++) {
    for (int j = 0; j < nfolds; j++) {
      low = j * size;
      up = (j == (nfolds - 1)) ? (n - 1) : ((j + 1) * size - 1);
      idx = getIndex(n, low, up);
      idxComp = getIndexComp(n, low, up);
      hLassoList = huberLasso(Z.rows(idxComp), Y.rows(idxComp), lambdaSeq(i), -1, constTau, phi0,
                              gamma, epsilon_c, iteMax);
      thetaHat = Rcpp::as<arma::vec>(hLassoList["beta"]);
      mse(i) += pairPred(Z.rows(idx), Y.rows(idx), thetaHat);
    }
  }
  arma::uword cvIdx = mse.index_min();
  hLassoList = huberLasso(Z, Y, lambdaSeq(cvIdx), -1, constTau, phi0, gamma, epsilon_c, iteMax);
  arma::vec theta = Rcpp::as<arma::vec>(hLassoList["beta"]);
  Rcpp::List listMean = huberMean(Y - Z.cols(1, d) * theta.rows(1, d));
  theta(0) = listMean["mu"];
  return Rcpp::List::create(Rcpp::Named("theta") = theta, Rcpp::Named("lambdaSeq") = lambdaSeq,
                            Rcpp::Named("lambdaMin") = lambdaSeq(cvIdx),
                            Rcpp::Named("tauCoef") = hLassoList["tau"],
                            Rcpp::Named("tauItcp") = listMean["tau"],
                            Rcpp::Named("iteCoef") = hLassoList["iteration"],
                            Rcpp::Named("iteItcp") = listMean["iteration"]);
}

              /* ------------------------------------ */
/*------------ Alternating Gradient Descent Functions ------------*/
              /* ------------------------------------ */

double LnVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return (arma::accu(arma::sqrt(tau * tau + arma::square(Y - mu)) - tau) / (z * std::sqrt(n)) + z * tau / std::sqrt(n)) / n;
}

double gradientMuVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return -(1 / std::sqrt(n)) * arma::accu((Y - mu) / (z * arma::sqrt(tau * tau + arma::square(Y - mu)))) / n;
}

double gradientTauVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return (1 / std::sqrt(n)) * arma::accu(tau / (z * arma::sqrt(tau * tau + arma::square(Y - mu)))) / n - (std::sqrt(n) / z - z / std::sqrt(n)) / n;
}

extern "C" double agd(MyVec _Y, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double eta1 = 1.0;
    double eta2 = std::sqrt(n);
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;
    double LMuOld = std::numeric_limits<double>::infinity();
    double LMuNew = LnVal(Y, muNew, tauNew, n, z);

    while (((LMuOld - LMuNew > 1e-10) && std::abs(muOld - muNew) > epsilon) && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        muOld = muNew;
        tauOld = tauNew;
        LMuOld = LMuNew;

        double gradientLnTauVal = gradientTauVal(Y, muOld, tauOld, n, z);
        tauNew = tauOld - eta2 * gradientLnTauVal;

        double gradientLnMuVal = gradientMuVal(Y, muOld, tauNew, n, z);
        muNew = muOld - eta1 * gradientLnMuVal;
        LMuNew = LnVal(Y, muNew, tauNew, n, z);
        iteNum += 1;
    }
    return muNew;
}

extern "C" double agdBB(MyVec _Y, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double eta1 = 1.0;
    double eta2 = std::sqrt(n);
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;

    double stepSizeTau = eta2;
    double stepSizeMu = eta1;

    double LMuOld = std::numeric_limits<double>::infinity();
    double LMuNew = LnVal(Y, muNew, tauNew, n, z);

    double lastTauTilde;
    double lastGradTau;
    double lastMuTilde;
    double lastGradMu;

    while (((LMuOld - LMuNew) > epsilon * epsilon && std::abs(muOld - muNew) > epsilon) && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        LMuOld = LMuNew;
        muOld = muNew;
        tauOld = tauNew;

        double gradTau = gradientTauVal(Y, muOld, tauOld, n, z);
        if (iteNum > 0)
        {
            double sTau = tauOld - lastTauTilde;
            double yTau = gradTau - lastGradTau;
            stepSizeTau = std::max(eta2, std::min(std::min((sTau * yTau) / (yTau * yTau), (sTau * sTau) / std::max(0.000001, sTau * yTau)), 1e3));
        }
        lastGradTau = gradTau;
        lastTauTilde = tauOld;

        tauNew = std::max(1.35, tauOld - stepSizeTau * (gradientTauVal(Y, muOld, tauOld, n, z)));

        double gradMu = gradientMuVal(Y, muOld, tauNew, n, z);
        if (iteNum > 0)
        {
            double sMu = muOld - lastMuTilde;
            double yMu = gradMu - lastGradMu;
            stepSizeMu = std::max(eta1, std::min(std::min((sMu * yMu) / (yMu * yMu), (sMu * sMu) / std::max(0.000001, sMu * yMu)), 1e3));
        }
        lastGradMu = gradMu;
        lastMuTilde = muOld;

        muNew = muOld - stepSizeMu * (gradientMuVal(Y, muOld, tauNew, n, z));
        LMuNew = LnVal(Y, muNew, tauNew, n, z);
        iteNum += 1;
    }
    return muNew;
}

extern "C" double agdBacktracking(MyVec _Y, double s1 = 1.0, double gamma1 = 0.5, double beta1 = 0.8, double s2 = 1.0,
                                  double gamma2 = 0.5, double beta2 = 0.8, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;

    while (std::abs(muOld - muNew) > epsilon && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        muOld = muNew;
        tauOld = tauNew;
        double LTauOld = LnVal(Y, muOld, tauOld, n, z);
        double gradientLnTauVal = gradientTauVal(Y, muOld, tauOld, n, z);

        /*backtracking for eta2*/
        double eta2 = s2;
        tauNew = tauOld - eta2*gradientLnTauVal;
        double LTauNew = LnVal(Y, muOld, tauNew, n ,z);

        while (LTauNew > LTauOld + gamma2*eta2*gradientLnTauVal)
        {
            eta2 = beta2*eta2;
            tauNew = tauOld - eta2*gradientLnTauVal;
            LTauNew = LnVal(Y, muOld, tauNew, n ,z);
            gradientLnTauVal = gradientTauVal(Y, muOld, tauNew, n, z);
        }

        double LMuOld = LTauNew;
        double gradientLnMuVal = gradientMuVal(Y, muOld, tauNew, n, z);

        /*backtracking for eta1*/
        double eta1 = s1;
        muNew = muOld - eta1*gradientLnMuVal;
        double LMuNew = LnVal(Y, muNew, tauNew, n ,z);

        while (LMuNew > LMuOld + gamma1*eta1*gradientLnMuVal)
        {
            eta1 = beta1*eta1;
            muNew = muOld - eta1*gradientLnMuVal;
            LMuNew = LnVal(Y, muNew, tauNew, n ,z);
            gradientLnMuVal = gradientMuVal(Y, muNew, tauNew, n, z);
        }
        iteNum += 1;
    }
    return muNew;
}