#ifndef DEFINING_ELEMENT_X8346D3
#define DEFINING_ELEMENT_X8346D3
// #define e 2.71828
// #define pi 3.14159
#include <bits/stdc++.h>
// #include <Eigen/Eigen>

// using namespace Eigen;
using namespace std;

class matrix
{
public:
    float **data;
    int shape[2];

    matrix()
    {
        this->shape[0] = 0;
        this->shape[1] = 0;
    }

    matrix(int a, int b)
    {
        this->shape[0] = a;
        this->shape[1] = b;
        this->data = (float **)malloc(sizeof(float *) * a);
        for (int i = 0; i < a; i++)
        {
            this->data[i] = (float *)malloc(sizeof(float) * b);
        }
        for (int i = 0; i < a; i++)
        {
            for (int j = 0; j < b; j++)
            {
                this->data[i][j] = 0;
            }
        }
    }

    void print()
    {
        printf("\n{ ");
        printf("Class : matrix , shape = [ %d , %d ] \n", this->shape[0], this->shape[1]);
        printf("  [ \n");
        for (int i = 0; i < this->shape[0]; i++)
        {
            printf("   [ ");
            for (int j = 0; j < this->shape[1]; j++)
            {
                if (j == this->shape[1] - 1)
                {
                    printf("%f ", data[i][j]);
                }
                else
                {
                    printf("%f , ", data[i][j]);
                }
            }
            if (i == this->shape[0] - 1)
                printf(" ]\n");
            else
            {
                printf(" ] ,\n");
            }
        }
        printf("  ] \n}\n", this->shape[0], this->shape[1]);
    }

    friend matrix operator>=(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] >= n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator<=(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] <= n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator>(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] > n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend matrix operator<(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] > n)
                    res.data[i][j] = 1;
                else
                    res.data[i][j] = 0;
            }
        }
        return res;
    }

    friend int operator==(matrix const m, matrix const &n)
    {
        if (m.shape[0] != n.shape[0] || m.shape[1] != n.shape[1])
        {
            return 0;
        }
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] != n.data[i][j])
                {
                    return 0;
                }
            }
        }
        return 1;
    }

    friend int operator!=(matrix const m, matrix const &n)
    {
        if (m.shape[0] != n.shape[0] || m.shape[1] != n.shape[1])
        {
            return 1;
        }

        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                if (m.data[i][j] != n.data[i][j])
                {
                    return 1;
                }
            }
        }
        return 0;
    }

    matrix operator()(int index)
    {
        matrix res(shape[0], 1);
        for (int i = 0; i < shape[0]; i++)
        {
            res.data[i][0] = this->data[i][index];
        }
        return res;
    }

    matrix operator()(int index1, int index2, int axis = 0)
    {
        if (axis == 0)
        {
            if (!((index1 >= 0 && index1 < shape[0]) && (index2 >= 0 && index2 < shape[0]) && index1 < index2))
            {
                fprintf(stderr, "enter correct indexing and axis , m.shape = [%d  %d ]", shape[0], shape[1]);
                exit(1);
            }
            matrix res(index2 - index1, shape[1]);
            for (int i = index1; i < index2; i++)
            {
                for (int j = 0; j < shape[1]; j++)
                {
                    res.data[i - index1][j] = data[i][j];
                }
            }

            return res;
        }
        else if (axis == 1)
        {
            if (!((index1 >= 0 && index1 < shape[1]) && (index2 >= 0 && index2 < shape[1]) && index1 < index2))
            {
                fprintf(stderr, "enter correct indexing and axis , m.shape = [%d  %d ]", shape[0], shape[1]);
                exit(1);
            }
            matrix res(shape[0], index2 - index1);

            for (int i = 0; i < shape[0]; i++)
            {
                for (int j = index1; j < index2; j++)
                {
                    res.data[i][j - index1] = data[i][j];
                }
            }

            return res;
        }
    }

    friend matrix operator/(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] / m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] / m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] / m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] / m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot divide element wise ,dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator/(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = n / (m.data[i][j]);
            }
        }
        return res;
    }

    friend matrix operator/(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = (m.data[i][j]) / n;
            }
        }
        return res;
    }

    // friend matrix operator+(matrix const m1, matrix const m2)
    // {
    //     if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
    //     {
    //         matrix res(m1.shape[0], m2.shape[1]);
    //         for (int i = 0; i < m1.shape[0]; i++)
    //         {
    //             for (int j = 0; j < m1.shape[1]; j++)
    //             {
    //                 res.data[i][j] = m1.data[i][j] + m2.data[i][j];
    //             }
    //         }
    //         return res;
    //     }
    // }

    friend matrix operator+(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] + m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[0][0];
                }
            }
            return res;
        }
        // else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        // {
        //     matrix res(m2.shape[0], m2.shape[1]);
        //     for (int i = 0; i < m2.shape[0]; i++)
        //     {
        //         for (int j = 0; j < m2.shape[1]; j++)
        //         {
        //             res.data[i][j] = m1.data[0][0] + m2.data[i][j];
        //         }
        //     }
        //     return res;
        // }

        else
        {
            fprintf(stderr, "cannot add dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator+(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] + n;
            }
        }
        return res;
    }

    friend matrix operator+(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] + n;
            }
        }
        return res;
    }

    friend matrix operator-(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] - m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] - m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] - m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] - m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot subtract ,dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator-(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] - n;
            }
        }
        return res;
    }

    friend matrix operator-(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = n - m.data[i][j];
            }
        }
        return res;
    }

    friend matrix operator*(matrix const m1, matrix const m2)
    {
        if (m1.shape[0] == m2.shape[0] && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m2.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] + m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][j] * m2.data[i][j];
                }
            }
            return res;
        }

        else if (m1.shape[0] != 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][0] * m2.data[i][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] != 1 && m1.shape[1] == m2.shape[1])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[0][j];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] != 1 && m2.shape[1] == 1 && m1.shape[0] == m2.shape[0])
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[i][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] != 1 && m1.shape[1] != 1 && m2.shape[0] == 1 && m2.shape[1] == 1)
        {
            matrix res(m1.shape[0], m1.shape[1]);
            for (int i = 0; i < m1.shape[0]; i++)
            {
                for (int j = 0; j < m1.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[i][j] * m2.data[0][0];
                }
            }
            return res;
        }
        else if (m1.shape[0] == 1 && m1.shape[1] == 1 && m2.shape[0] != 1 && m2.shape[1] != 1)
        {
            matrix res(m2.shape[0], m2.shape[1]);
            for (int i = 0; i < m2.shape[0]; i++)
            {
                for (int j = 0; j < m2.shape[1]; j++)
                {
                    res.data[i][j] = m1.data[0][0] * m2.data[i][j];
                }
            }
            return res;
        }
        else
        {
            fprintf(stderr, "cannot multiple element wise, dimensions mismatched m1.shape = [%d , %d] and m2.shape = [%d , %d]", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
            exit(1);
        }
    }

    friend matrix operator*(float const &n, matrix const m)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] * n;
            }
        }
        return res;
    }

    friend matrix operator*(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = m.data[i][j] * n;
            }
        }
        return res;
    }

    friend matrix operator^(matrix const m, float const &n)
    {
        matrix res(m.shape[0], m.shape[1]);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[i][j] = pow(m.data[i][j], n);
            }
        }
        return res;
    }
    matrix T()
    {
        matrix m1(this->shape[1], this->shape[0]);

        for (int i = 0; i < shape[0]; i++)
        {
            for (int j = 0; j < shape[1]; j++)
            {
                m1.data[j][i] = this->data[i][j];
            }
        }
        return m1;
    }
};

matrix randn(int a, int b)
{
    matrix m(a, b);
    srand(time(0));
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            m.data[i][j] = 2 * (rand() / (float)RAND_MAX) - 1;
        }
    }
    return m;
}

void print(matrix m)
{
    printf("\n{ ");
    printf("Class : matrix , shape = [ %d , %d ] \n", m.shape[0], m.shape[1]);
    printf("  [ \n");
    for (int i = 0; i < m.shape[0]; i++)
    {
        printf("   [ ");
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (j == m.shape[1] - 1)
            {
                printf("%f ", m.data[i][j]);
            }
            else
            {
                printf("%f , ", m.data[i][j]);
            }
        }
        if (i == m.shape[0] - 1)
            printf(" ]\n");
        else
        {
            printf(" ] ,\n");
        }
    }
    printf("  ] \n}", m.shape[0], m.shape[1]);
}

matrix take_matrix(char *path)
{
    FILE *fp;
    fp = fopen(path, "r");
    int dim1, dim2;
    fscanf(fp, "%d %d", &dim1, &dim2);
    matrix X(dim1, dim2);
    for (int i = 0; i < dim1; i++)
    {
        for (int j = 0; j < dim2; j++)
        {
            fscanf(fp, "%f", &X.data[i][j]);
        }
    }

    return X;
}

matrix zeros(int a, int b)
{
    matrix m(a, b);
    for (int i = 0; i < a; i++)
    {
        for (int j = 0; j < b; j++)
        {
            m.data[i][j] = 0;
        }
    }
    return m;
}

matrix dot(matrix m1, matrix m2)
{
    if (m1.shape[1] != m2.shape[0])
    {
        fprintf(stderr, "ERROR : Dimensions Mismatch in performing dot product m1.shape = [%d ,%d]  and  m2.shape = [%d ,%d]\n", m1.shape[0], m1.shape[1], m2.shape[0], m2.shape[1]);
        exit(1);
    }
    matrix multi(m1.shape[0], m2.shape[1]);

    for (int i = 0; i < m1.shape[0]; ++i)
    {
        for (int j = 0; j < m2.shape[1]; ++j)
        {
            for (int k = 0; k < m1.shape[1]; ++k)
            {
                multi.data[i][j] += m1.data[i][k] * m2.data[k][j];
            }
        }
    }
    return multi;
}

matrix abs(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] > 0)
                res.data[i][j] = m.data[i][j];
            else
                res.data[i][j] = -1 * m.data[i][j];
        }
    }
    return res;
}

matrix exp(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = pow(2.71828, m.data[i][j]);
        }
    }
    return res;
}

matrix log(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] < 0)
            {
                fprintf(stderr, "Domain error in log");
                exit(1);
            }
            else if (m.data[i][j] == 0)
                res.data[i][j] = 0;
            else
                res.data[i][j] = log(m.data[i][j]);
        }
    }
    return res;
}

float sum(matrix m)
{

    float sum = 0;
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            sum += m.data[i][j];
        }
    }
    return sum;
}

matrix m_sum(matrix m, int axis = -1)
{
    if (axis == 0)
    {
        matrix res(1, m.shape[1]);
        for (int i = 0; i < m.shape[1]; i++)
        {
            float sum = 0;
            for (int j = 0; j < m.shape[0]; j++)
            {
                sum = sum + m.data[j][i];
            }
            res.data[0][i] = sum;
        }
        return res;
    }
    else if (axis == 1)
    {
        matrix res(m.shape[0], 1);
        for (int i = 0; i < m.shape[0]; i++)
        {
            float sum = 0;
            for (int j = 0; j < m.shape[1]; j++)
            {
                sum = sum + m.data[i][j];
            }
            res.data[i][0] = sum;
        }
        return res;
    }
    else
    {
        matrix res = zeros(1, 1);
        for (int i = 0; i < m.shape[0]; i++)
        {
            for (int j = 0; j < m.shape[1]; j++)
            {
                res.data[0][0] += m.data[i][j];
            }
        }
        return res;
    }
}

matrix sigmoid(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = 1.0 / (1.0 + pow(2.71828, -1 * m.data[i][j]));
        }
    }
    return res;
}

matrix tanh(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = tanh(m.data[i][j]);
        }
    }
    return res;
}
matrix relu(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (m.data[i][j] >= 0)
            {
                res.data[i][j] = m.data[i][j];
            }
            else
            {
                res.data[i][j] = 0;
            }
        }
    }
    return res;
}

matrix normalize(matrix m)
{
    matrix res(m.shape[0], m.shape[1]);
    for (int i = 0; i < m.shape[0]; i++)
    {
        float max = m.data[i][0];
        float min = m.data[i][0];
        for (int j = 0; j < m.shape[1]; j++)
        {
            if (max < m.data[i][j])
            {
                max = m.data[i][j];
            }
            if (min > m.data[i][j])
            {
                min = m.data[i][j];
            }
        }

        for (int j = 0; j < m.shape[1]; j++)
        {
            res.data[i][j] = (m.data[i][j] - min) / (max - min);
        }
    }
    return res;
}
#endif

// matrix cost(matrix Y, matrix A_L)
// {
//     float m = Y.shape[1];
//     matrix cost = -1 * m_sum(Y * log(A_L) + (1 - Y) * log(1 - A_L), 1) / m;
//     return cost;
// }

// int main()
// {
//     matrix m1 = randn(3, 5);
//     print(m1);
//     matrix m2 = sigmoid(m1);
//     print(m2);
//     print(1 - m2);
//     print(log(m2));
// }

// int main()
// {
//     clock_t start, end;
//     float time;
//     printf("Matrix : Computing....\n");
//     for (int i = 10; i <= 1000; i += 20)
//     {
//         start = clock();
//         matrix m1 = randn(i, i), m2 = randn(i, i);
//         matrix m3 = dot(m1, m2.T());
//         end = clock();
//         float time = (float)(end - start) / CLOCKS_PER_SEC;
//         printf("%f\n", time);

//         //     start = clock();
//         //     MatrixXf m4 = MatrixXf::Random(i, i), m5 = MatrixXf::Random(i, i);
//         //     MatrixXf temp = m4 * m5;
//         //     end = clock();
//         //     time = (float)(end - start) / CLOCKS_PER_SEC;
//         //     printf("%f", time);
//         // }
//     }
// }

// class test
// {
// private:
//     matrix A = zeros(0, 0), B = zeros(0, 0);

// public:
//     void get_data(matrix X, matrix Y)
//     {
//         this->A = X;
//         this->B = Y;
//     }

//     void print_a_b()
//     {
//         this->A.print();
//         this->B.print();
//     }
// };

// int main()
// {
//     test t;
//     matrix m1=zeros(10 ,4),m2=randn(10,3);
//     t.get_data(m1,m2);
//     t.print_a_b();
//}

// int main()
// {
// matrix m = randn(100, 100);
// matrix m1 = sigmoid(m);
// matrix m2 = log(m1);
// for (int i = 0; i < 100; i++)
// {
//     for (int j = 0; j < 100; j++)
//     {
//         if (!(m1.data[i][j] < 1 && m1.data[i][j] > 0))
//         {
//             printf("Error");
//         }
//     }
// }

// float ans;
// ans = 1 / (1 + pow(2.71828, 14));
// printf("%f",ans );

// printf("%f",log(0.001));
// }

// int main()
// {
//     matrix m = randn(10, 6);
//     matrix m2 = m(2, 4, 1);
//     m.print();
//     m2.print();
// }


/******************************************************************************
Copyright (c) 2021 Dmitriy Korchemkin

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

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

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

#ifndef POLYNOMIALS_ROOTS_HPP
#define POLYNOMIALS_ROOTS_HPP

#include <Eigen/Dense>
#include <iosfwd>

#include "polynomials/assert.hpp"
#include "polynomials/types.hpp"

#ifndef CERES_PUBLIC_JET_H
namespace ceres {
template <typename Scalar, int N> struct Jet;
}
#endif

namespace polynomials {

template <typename Scalar> struct RealRootFilter {
  std::optional<Scalar> operator()(const std::complex<Scalar> &c,
                                   const Scalar &tolerance) const {
    using std::abs;
    const auto re = c.real();
    const auto im = c.imag();
    const auto abs_re = abs(re);
    const auto abs_im = abs(im);
    if (abs_im < abs_re * tolerance)
      return re;
    return std::nullopt;
  }

  std::optional<Scalar> operator()(const std::complex<Scalar> &c) const {
    const auto re = c.real();
    const auto im = c.imag();
    if (im == Scalar(0.))
      return re;
    return std::nullopt;
  }
};

template <typename Scalar> struct PositiveRealRootFilter {
  std::optional<Scalar> operator()(const std::complex<Scalar> &c) const {
    auto real = rrf(c);
    if (!real)
      return std::nullopt;
    if (*real >= Scalar(0.))
      return real;
    return std::nullopt;
  }

  std::optional<Scalar> operator()(const std::complex<Scalar> &c,
                                   const Scalar &tolerance) const {
    auto real = rrf(c, tolerance);
    if (!real)
      return std::nullopt;
    if (*real >= Scalar(0.))
      return real;
    return std::nullopt;
  }

  RealRootFilter<Scalar> rrf;
};

template <typename Polynomial,
          bool fixed_degree = Polynomial::DegreeAtCompileTime != Dynamic>
struct QuotientRingMulXImpl;

template <typename Polynomial>
struct QuotientRingMulX : public QuotientRingMulXImpl<Polynomial> {
  using Base = QuotientRingMulXImpl<Polynomial>;

  template <typename... Args> QuotientRingMulX(Args... args) : Base(args...) {}
};

template <typename Polynomial> struct QuotientRingMulXImpl<Polynomial, true> {
  static constexpr Index DegreeAtCompileTime = Polynomial::DegreeAtCompileTime;
  static constexpr Index MaxDegreeAtCompileTime =
      Polynomial::MaxDegreeAtCompileTime;
  using Scalar = typename Polynomial::Scalar;

  using CompanionMatrix =
      Eigen::Matrix<Scalar, DegreeAtCompileTime, DegreeAtCompileTime>;
  using ComplexRoots = typename CompanionMatrix::EigenvaluesReturnType;
  using RealRoots =
      Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, DegreeAtCompileTime, 1>;

  QuotientRingMulXImpl(const Polynomial &p) {
    const Scalar lead_term = p[DegreeAtCompileTime];
    matrix
        .template bottomLeftCorner<DegreeAtCompileTime - 1,
                                   DegreeAtCompileTime - 1>()
        .setIdentity();
    matrix.col(DegreeAtCompileTime - 1) =
        -p.coeffs().template head<DegreeAtCompileTime>() *
        (Scalar(1.) / lead_term);
    matrix.row(0).template head<DegreeAtCompileTime - 1>().setZero();
  }

  operator CompanionMatrix() const { return matrix; }

  ComplexRoots complex_roots() const { return matrix.eigenvalues(); }

  template <typename Op, typename... Args>
  RealRoots filter_roots(Args... args) const {
    Index count_valid = 0;
    Scalar rr[DegreeAtCompileTime];
    const auto roots = complex_roots();
    const Op filter;
    for (Index i = 0; i < DegreeAtCompileTime; ++i) {
      auto real = filter(roots[i], args...);
      if (!real)
        continue;
      rr[count_valid++] = *real;
    }
    return Eigen::Map<const RealRoots>(rr, count_valid, 1);
  }

  template <typename... Args> RealRoots real_roots(Args... args) const {
    return filter_roots<RealRootFilter<Scalar>, Args...>(args...);
  }

  template <typename... Args>
  RealRoots positive_real_roots(Args... args) const {
    return filter_roots<PositiveRealRootFilter<Scalar>, Args...>(args...);
  }

private:
  CompanionMatrix matrix;
};

template <typename Polynomial> struct QuotientRingMulXImpl<Polynomial, false> {
  static constexpr Index DegreeAtCompileTime = Polynomial::DegreeAtCompileTime;
  static constexpr Index MaxDegreeAtCompileTime =
      Polynomial::MaxDegreeAtCompileTime;
  using Scalar = typename Polynomial::Scalar;

  using CompanionMatrix =
      Eigen::Matrix<Scalar, DegreeAtCompileTime, DegreeAtCompileTime, 0,
                    MaxDegreeAtCompileTime, MaxDegreeAtCompileTime>;
  using ComplexRoots = typename CompanionMatrix::EigenvaluesReturnType;
  using RealRoots =
      Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, MaxDegreeAtCompileTime, 1>;

  QuotientRingMulXImpl(const Polynomial &p) {
    const Index degree = p.degree();
    matrix.resize(degree, degree);
    const Scalar lead_term = p[degree];
    matrix.bottomLeftCorner(degree - 1, degree - 1).setIdentity();
    matrix.col(DegreeAtCompileTime - 1) =
        -p.coeffs().head(degree) * (Scalar(1.) / lead_term);
    matrix.row(0).head(degree - 1).setZero();
  }

  operator CompanionMatrix() const { return matrix; }

  ComplexRoots complex_roots() const { return matrix.eigenvalues(); }

  RealRoots real_roots() const {
    Index count_real = 0;
    const Index degree = dim();
    Eigen::Matrix<Scalar, MaxDegreeAtCompileTime, 1> rr(degree, 1);

    const auto roots = complex_roots();

    for (Index i = 0; i < degree; ++i) {
      auto im = roots[i].imag();
      if (im != Scalar(0.))
        continue;
      auto re = roots[i].real();
      rr[count_real++] = re;
    }
    return Eigen::Map<const RealRoots>(rr, count_real, 1);
  }

  RealRoots real_roots(const Scalar &tolerance) const {
    Index count_real = 0;
    const Index degree = dim();
    Eigen::Matrix<Scalar, MaxDegreeAtCompileTime, 1> rr(degree, 1);

    const auto roots = complex_roots();

    for (Index i = 0; i < degree; ++i) {
      auto re = roots[i].real();
      auto are = std::abs(re);
      auto aim = std::abs(roots[i].imag);
      if (!(aim < tolerance * are))
        continue;
      rr[count_real++] = re;
    }
    return Eigen::Map<const RealRoots>(rr, count_real, 1);
  }

  Index dim() const { return matrix.rows(); }

private:
  CompanionMatrix matrix;
};

template <typename Poly, template <typename> typename Algo, typename Scalar>
struct RootFinder {
  using Algorithm = Algo<Poly>;
  using ComplexRoots = typename Algorithm::ComplexRoots;
  using RealRoots = typename Algorithm::RealRoots;

  template <typename... Args>
  static RealRoots real_roots(const Poly &p, Args... args) {
    return Algorithm(p).real_roots(args...);
  }

  template <typename... Args>
  static RealRoots positive_real_roots(const Poly &p, Args... args) {
    return Algorithm(p).positive_real_roots(args...);
  }

  template <typename... Args>
  static ComplexRoots complex_roots(const Poly &p, Args... args) {
    return Algorithm(p).complex_roots(args...);
  }
};

template <typename Poly, template <typename> typename Algo, typename Scalar,
          int N>
struct RootFinder<Poly, Algo, ceres::Jet<Scalar, N>> {
  using ScalarPoly = DensePoly<Scalar, Poly::DegreeAtCompileTime,
                               Poly::MaxDegreeAtCompileTime>;
  using ScalarRootFinder = RootFinder<ScalarPoly, Algo>;
  using ScalarAlgorithm = Algo<ScalarPoly>;
  using ScalarComplexRoots = typename ScalarAlgorithm::ComplexRoots;
  using ScalarRealRoots = typename ScalarAlgorithm::RealRoots;
  using Jet = ceres::Jet<Scalar, N>;
  using ComplexJet = ceres::Jet<typename ScalarComplexRoots::Scalar, N>;

  using JetComplexRoots = Eigen::Matrix<ComplexJet, Poly::DegreeAtCompileTime,
                                        1, 0, Poly::MaxDegreeAtCompileTime, 1>;
  using JetRealRoots =
      Eigen::Matrix<Jet, Eigen::Dynamic, 1, 0, Poly::MaxDegreeAtCompileTime, 1>;

  static ScalarPoly cast(const Poly &p) {
    ScalarPoly sp(p.degree());
    const Index num_coeffs = p.degree() + 1;
    for (Index i = 0; i < num_coeffs; ++i)
      sp.coeffs()[i] = p.coeffs()[i].a;
    return sp;
  }

  template <typename T>
  static ceres::Jet<T, N>
  process_root(const Poly &poly, const ScalarPoly &scalar_poly, const T &root) {
    ceres::Jet<T, N> res;
    res.a = root;
    res.v.setZero();
    const T denom(-scalar_poly.df(root));
    const auto &coeffs = poly.coeffs();
    const Index num_coeffs = coeffs.size();
    T root_pow = T(1.);
    for (Index i = 0; i < num_coeffs; ++i) {
      // dx / da_k = -x^k / f'
      // dx / dp_i = \sum_k dx / da_k da_k / dp_i
      // dx / dp = (\sum_k -x^k [da_k / dp]) / f'

      res.v += root_pow * coeffs[i].v;
      root_pow *= root;
    }
    res.v /= denom;
    return res;
  }

  template <typename... Args>
  static JetComplexRoots complex_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarComplexRoots scalar_roots =
        ScalarRootFinder::complex_roots(scalar, args...);
    const Index num_roots = scalar_roots.size();
    JetComplexRoots jet(num_roots);
    for (Index i = 0; i < num_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }

  template <typename... Args>
  static JetRealRoots real_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarRealRoots scalar_roots =
        ScalarRootFinder::real_roots(scalar, args...);
    const Index num_real_roots = scalar_roots.size();
    JetRealRoots jet(num_real_roots);
    for (Index i = 0; i < num_real_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }

  template <typename... Args>
  static JetRealRoots positive_real_roots(const Poly &p, Args... args) {
    const ScalarPoly scalar = cast(p);
    const ScalarRealRoots scalar_roots =
        ScalarRootFinder::positive_real_roots(scalar, args...);
    const Index num_real_roots = scalar_roots.size();
    JetRealRoots jet(num_real_roots);
    for (Index i = 0; i < num_real_roots; ++i)
      jet[i] = process_root(p, scalar, scalar_roots[i]);
    return jet;
  }
};

} // namespace polynomials

#endif


#include <fstream>
#include <unordered_map>

#include <boost/format.hpp>

#include "local_push.h"
#include "file_serialization.h"
#include "util/log.h"
using boost::format;

double cal_rmax(double c, double epsilon) {
    return (1 - c) * epsilon;
}

double cal_rmax(DirectedG &g, double c, double epsilon, double delta) {
    // calculate r_max
    int m, n;
    m = num_edges(g);
    n = num_vertices(g);
    double d = double(m) / double(n);
    // cout << d << endl;
    double a = (1 - c) * pow(d, 2) * pow(epsilon, 2) / (c * log(2 / delta));
    // cout << pow(a,1.0/3.0) << endl;
    return pow(a, 1.0 / 3.0);
}

void LocalPush::push(NodePair &pab, double inc) {
    // the actually action of push
    n_push++;
    R[pab] += inc;
    // cout << pab.first << " " << pab.second << endl ;
    if (fabs(R[pab]) > r_max) {
        // Q.insert(pab);
        if (marker[pab] == false) {
            Q.push(pab);
            marker[pab] = true;
        }
    }
}

LocalPush::LocalPush(DirectedG &g, string gName_, double c_, double epsilon_, size_t n_) {
    // init data members
    /* init data memebrs */
    g_name = gName_;
    c = c_;
    // r_max = r_max_;
    epsilon = epsilon_;
    r_max = cal_rmax(c, epsilon);
    n = n_;
    string path = get_edge_list_path(gName_);
    n_push = 0;
    cpu_time = -1; // set the init value

    // check the local puah exits
    // string data_path =get_file_path_base() + ".P";
    // cout << "data path " << data_path << endl;
    // if(file_exists(data_path)){ // local push from start
    //     cout << "file exisis.." << endl;
    //     load();
    // }else{ // file exists
    //     /* init the P and R */
    //     cout << "file not exists, compute from scratch" << endl;
    //     for(int i=0;i<n;i++){
    //         NodePair np(i,i);
    //         R.insert({np,1});
    //         Q.push(np);
    //         marker[np] = true;
    //     }
    // }
}

Full_LocalPush::Full_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) : LocalPush(g, name, c_,
                                                                                r_max_, n_) {
    string data_path = get_file_path_base() + ".P";
    cout << "data path " << data_path << endl;
    if (file_exists(data_path)) { // local push from start
        cout << "file exisis.." << endl;
        load();
    } else { // file exists
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

Reduced_LocalPush::Reduced_LocalPush(DirectedG &g, string name, double c_, double r_max_, size_t n_) : LocalPush(g,
                                                                                name,
                                                                                c_,
                                                                                r_max_,
                                                                                n_) {
    string data_path = get_file_path_base() + ".P";
    // cout << "data path " << data_path << endl;
    if (file_exists(data_path)) { // local push from start
        // cout << "file exisis.." << endl;
        load();
        // cout << g_name << " " << r_max << " " << c << " " << n << endl;
    } else { // file exists
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch!!!" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

void Reduced_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    // the push method using reduced linear system
    // out-neighbros of a,b
    DirectedG::out_edge_iterator
            outi_iter,
            outi_end,
            outj_iter,
            outj_end;
    bool is_singleton = np.first == np.second ? true : false;
    // /* only push to partial pairs*/
    size_t out_degree_i = out_degree(np.first, g);
    size_t out_degree_j = out_degree(np.second, g);

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
    /*the indicator whether the position is common neighbor */
    vector<bool> outs_i_common(out_degree_i, false);
    vector<bool> outs_j_common(out_degree_j, false);
    // cout << "------" << endl;

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    if (is_singleton) {
        /* starting push for singleton nodes*/
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto out_a = target(*outi_iter, g); // out-neighbor
            for (; outj_iter != outj_end; outj_iter++) {
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                if (out_a < out_b) { // only push to partial pairs for a < b
                    NodePair pab(out_a, out_b); // the node-pair to be pushed to
                    double inc = c * current_residual / total_in;
                    push(pab, inc); // do the push action
                }
            }
        }
    } else {
        /* mark the common neighbors */
        size_t i, j;
        for (i = 0; i < out_degree_i; i++) {
            auto a = target(*(outi_iter + i), g);
            for (j = 0; j < out_degree_j; j++) {
                auto b = target(*(outj_iter + j), g);
                if (a == b) {
                    outs_i_common[i] = true;
                    outs_j_common[j] = true;
                    break;
                }
            }
        }
        /* starting push for non-singleton nodes*/
        auto i_begin_iter = outi_iter; // mark the begining iterator
        for (; outi_iter != outi_end; outi_iter++) {
            tie(outj_iter, outj_end) = out_edges(np.second, g);// init the iterator
            auto j_begin = outj_iter;
            bool is_i_common = outs_i_common[outi_iter - i_begin_iter];// indicator of whether i is a common neighbor
            for (; outj_iter != outj_end; outj_iter++) {
                bool is_j_common = outs_j_common[outj_iter - j_begin];
                auto out_a = target(*outi_iter, g); // out-neighbor
                auto out_b = target(*outj_iter, g);
                auto indegree_a = in_degree(out_a, g);
                auto indegree_b = in_degree(out_b, g);
                auto total_in = indegree_a * indegree_b;
                double inc = c * current_residual / total_in;
                if (out_a == out_b) { //don't push to singleton nodes
                    continue;
                }
                bool oa_less_ob = out_a < out_b ? true : false;
                // cout << "i com :" << is_i_common << " j com: " << is_j_common << endl;
                if (!oa_less_ob) {
                    swap(out_a, out_b);
                }
                NodePair pab(out_a, out_b);
                if (!is_i_common) {
                    // i is not common neighbonr
                    push(pab, inc);
                } else {
                    // i is a common neighbor
                    if (is_j_common) {
                        if (oa_less_ob) {
                            push(pab, 2 * inc); // push twice for two commons
                        }
                    } else {// notmal case
                        push(pab, inc);
                    }
                }
            }
        }
    }
    // cout << "------" << endl;
}

void Full_LocalPush::push_to_neighbors(DirectedG &g, NodePair &np, double current_residual) {
    DirectedG::out_edge_iterator
            outi_iter,
            outi_end,
            outj_iter,
            outj_end;

    tie(outi_iter, outi_end) = out_edges(np.first, g);
    for (; outi_iter != outi_end; outi_iter++) {
        auto out_a = target(*outi_iter, g); // out-neighbor
        tie(outj_iter, outj_end) = out_edges(np.second, g);
        for (; outj_iter != outj_end; outj_iter++) {
            auto out_b = target(*outj_iter, g);
            if (out_a == out_b) {
                continue;
            }
            auto indegree_a = in_degree(out_a, g);
            auto indegree_b = in_degree(out_b, g);
            auto total_in = indegree_a * indegree_b;
            NodePair pab(out_a, out_b);
            double inc = c * current_residual / total_in;
            push(pab, inc);
        }
    }
}

double Full_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    return R[np];
}

double Reduced_LocalPush::how_much_residual_to_push(DirectedG &g, NodePair &np) {
    // determine the residual value for current pair to push
    double r = R[np];
    if (np.first == np.second) { //singleton node
        return r - r_max / (1 - c); // singleton nodes do not need to push all residual as 1
        // return r;
        // return 1;
    }
    /* check whether np forms a self-loop */
    if (edge(np.first, np.second, g).second == true &&
        edge(np.second, np.first, g).second == true) { // check whether exists reverse edge
        auto in_deg_a = in_degree(np.first, g);
        auto in_deg_b = in_degree(np.second, g);
        double alpha = c / (in_deg_a * in_deg_b);
        int k = ceil(log(r_max / fabs(r)) / log(alpha));
        double residual_to_push = (1 - pow(alpha, k)) * r / (1 - alpha);
        return residual_to_push;
    } else {
        auto push_residual = r;
        return push_residual;
        // optimize for neighbor-loop
        // double current_max = 0;
        // double max_m = 0;
        // double alpha_i;
        // double alpha_0;
        // DirectedG::out_edge_iterator out_a_it, out_a_end;
        // DirectedG::out_edge_iterator out_b_it, out_b_end;
        // alpha_0 = c / (in_degree(np.first,g) * in_degree(np.second,g));
        // tie(out_a_it, out_a_end) = out_edges(np.first,g);
        // for(;out_a_it != out_a_end; out_a_it ++){
        //     tie(out_b_it, out_b_end) = out_edges(np.second,g);
        //     auto out_a = target(*out_a_it,g);
        //     auto in_a = in_degree(out_a,g);
        //     bool out_a_to_first = edge(out_a,np.first,g).second ;// whether there is a reverse link from  out_a to np.first
        //     for(;out_b_it!=out_b_end;out_b_it ++){
        //         auto out_b = target(*out_b_it,g);
        //         auto in_b = in_degree(out_b,g);
        //         double current_alpha_i = c / (in_a * in_b);
        //         bool out_b_to_second = edge(out_b,np.second,g).second;// whether there is a reverse link from out_b to np.second
        //         if(out_a_to_first && out_b_to_second){ // is a neighbor-loop
        //             NodePair bnp(out_a, out_b);
        //             double m = fabs(R[bnp] + r * current_alpha_i);
        //             if(m > r_max && m > current_max){
        //                 current_max = m;
        //                 alpha_i = current_alpha_i;
        //                 // if(fabs(r_x) > current_max){
        //                 //     current_max = r_x;
        //                 // }
        //             }
        //         }
        //     }
        // }
        // if(current_max > 0){ // if there is any self lop
        //     int k = int(ceil(log(r_max / (alpha_0 * current_max))/ log(alpha_0 * alpha_i))) + 1;
        //     double r_x= 0; // the residual to be pushed
        //     r_x = r + alpha_0 * current_max * (1 - pow(alpha_0 * alpha_i, k)) / (1 - alpha_0 * alpha_i);
        //     // cout << "neighbor loop.." << r_x << endl;
        //     return r_x;
        // }else{
        //     return r;
        // }
    }

    // // non-singleton nodes

    // // compute optimal value to push
    // double sum_airi =  0;
    // double sum_ai_square = 0;
    // DirectedG::out_edge_iterator out_a_it, out_a_end;
    // DirectedG::out_edge_iterator out_b_it, out_b_end;
    // for(;out_a_it != out_a_end; out_a_it ++){
    //     auto out_a = target(*out_a_it,g);
    //     auto in_a = in_degree(out_a,g);
    //     for(;out_b_it!=out_b_end;out_b_it ++){
    //         auto out_b = target(*out_b_it,g);
    //         auto in_b = in_degree(out_b,g);
    //         double alpha = c / (in_a * in_b);
    //         sum_airi += alpha * R[NodePair(out_a, out_b)];
    //         sum_ai_square += pow(alpha,2);
    //     }
    // }
    // r = (r - sum_airi) / (1 + sum_ai_square);

    /* determine the residual to push by computing the maximum overlapping interval */
    // cout << "current r " << r << endl;
    // auto out_deg_a = out_degree(np.first,g);
    // auto out_deg_b = out_degree(np.second,g);
    // auto total_out = out_deg_a * out_deg_b;
    // double start[total_out+1];
    // double end[total_out+1];
    // size_t i = 0;
    // if(edge(np.first,np.second,g).second == true && edge(np.second, np.first,g).second == true){
    //     // self loop
    //     double alpha = c / (in_degree(np.first,g) * in_degree(np.second,g));
    //     start[i] = (r_max -r ) / (alpha - 1);
    //     end[i] = (-r_max - r) / (alpha - 1);
    // }else{
    //     double alpha = c / (in_degree(np.first,g) * in_degree(np.second,g));
    //     start[i] = r-r_max;
    //     end[i] = r + r_max;
    // }

    // i++;

    // DirectedG::out_edge_iterator out_a_it, out_a_end;
    // DirectedG::out_edge_iterator out_b_it, out_b_end;
    // for(;out_a_it != out_a_end; out_a_it ++){
    //     auto out_a = target(*out_a_it,g);
    //     auto in_a = in_degree(out_a,g);
    //     for(;out_b_it!=out_b_end;out_b_it ++){
    //         auto out_b = target(*out_b_it,g);
    //         auto in_b = in_degree(out_b,g);
    //         double alpha = c / (in_a * in_b);
    //         double r_i = R[NodePair(out_a, out_b)];
    //         auto current_start = - (r_max + r_i) / alpha;
    //         auto current_end = (r_max - r_i) / alpha;
    //         if(current_start > end[0] || current_end < start[0]){
    //             // filter out the nodes that conflicts with source node
    //             continue;
    //         }else{
    //             if(current_start < start[0]){
    //                 current_start = start[0];
    //             }
    //             if(current_end > end[0]){
    //                 current_end = end[0];
    //             }
    //             start[i] = current_start;
    //             end[i] = current_end;
    //             i ++;
    //         }
    //     }
    // }
    // // for(i = 0; i< total_out +1;i++){
    // //     int a;
    // //     cout << "lower bound: " << start[i] << " upper bound: "<< end[i] << endl;
    // //     cin >> a;
    // // }

    // double push_residual = findMaxInterval(start, end, i); // i is the length
}

void LocalPush::local_push(DirectedG &g) { // local push given current P and R
    // cout << r_max << endl;
    auto start = std::chrono::high_resolution_clock::now();
    double sum_of_est = 0;

    while (!Q.empty()) {
        NodePair np = Q.front();
        Q.pop();
        marker[np] = false;

        double residual_to_push = how_much_residual_to_push(g, np);
        sum_of_est += residual_to_push;

        R[np] -= residual_to_push;
        P[np] += residual_to_push;
        push_to_neighbors(g, np, residual_to_push); // push residuals to neighbros of np
    }
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
#ifdef OUTPUT
    if (cpu_time == -1) {
        cpu_time = elapsed.count();
        mem_size = getValue();
    }
#endif
}

void LocalPush::save() {
    // save data to disk
    // save P
    string p_path = get_file_path_base() + ".P";
    P.save(p_path);
    // FILE *outP = fopen(p_path.c_str(), "wb");
    // cout << "saving P to " << p_path << endl;
    // P.serialize(FileSerializer(), outP);
    // fclose(outP);
    //save R
    string r_path = get_file_path_base() + ".R";
    R.save(r_path);
    // FILE *outR = fopen(r_path.c_str(), "wb");
    // cout << "saving R to " << r_path << endl;
    // R.serialize(FileSerializer(), outR);
    // fclose(outR);

    ofstream out;
    // // save meta info
    string meta_path = get_file_path_base() + ".meta";
    out.open(meta_path);
    out << g_name << endl;
    out << n << endl;
    out << epsilon << endl;
    out << c << endl;
    out << cpu_time << endl;
    out << mem_size << endl;
    auto p_size = P.size();
    auto r_size = R.size();
    out << p_size << endl;
    out << r_size << endl;
    out << double(p_size) / (n * n) << endl;
    out << double(r_size) / (n * n) << endl;
    out.close();

    // // save the experimental data
    // string exp_path =get_file_path_base()+".exp";
    // out.open(exp_path);
    // out << n << endl;
    // cout << c << endl;
    // cout << epsilon << endl;
    // out.close();
    // save exp data
    cout << "save complete" << endl;

}

void LocalPush::load() {
    // load existing local push data
    // load P
    string p_path = get_file_path_base() + ".P";
    P.load(p_path);
    // FILE *inP = fopen(p_path.c_str(), "rb");
    // cout << "loading P to " << p_path << endl;
    // P.unserialize(FileSerializer(), inP);
    // fclose(inP);
    //load R
    string r_path = get_file_path_base() + ".R";
    R.load(r_path);
    // FILE *inR = fopen(r_path.c_str(), "rb");
    // cout << "loading R to " << r_path << endl;
    // R.unserialize(FileSerializer(), inR);
    // fclose(inR);

    string meta_path = get_file_path_base() + ".meta";
    ifstream in;
    in.open(meta_path);
    in >> g_name >> n >> epsilon >> c >> cpu_time >> mem_size;
    r_max = cal_rmax(c, epsilon);
    marker.add(n); // initialize marker
    in.close();
}

void LocalPush::show() {
    // cout << P.size() << endl;
    // for(auto &item:P){
    //     cout << "(" << item.first.first << "," << item.first.second << "): " << item.second << endl;
    // }
}

string Reduced_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("RLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

string Full_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("FLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

double Full_LocalPush::query_P(unsigned long a, unsigned long b) {
    return P.query(a, b);
}

double Full_LocalPush::query_R(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    return R.query(a, b);
}

void Full_LocalPush::insert(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v, DirectedG &g) {
    DirectedG::vertex_iterator v_it, v_end;
    tie(v_it, v_end) = vertices(g);
    auto in_deg_v = in_degree(v, g);
    for (; v_it != v_end; v_it++) {
        auto a = *v_it;
        if (a != v) {
            update_residual(u, v, a, g, in_deg_v);
        } else {
            // a == v
            R[NodePair(a, a)] = 1 - P.query(a, a);
        }
    }
}

void Full_LocalPush::update_residual(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                                     DirectedG::vertex_descriptor a, DirectedG &g, int in_deg_v) {
    // 1st: accumulate estimates
    DirectedG::in_edge_iterator in_a_it, in_a_end;
    tie(in_a_it, in_a_end) = in_edges(a, g);
    float estimation_sum = 0;
    for (; in_a_it != in_a_end; in_a_it++) {
        auto ina = source(*in_a_it, g);
        estimation_sum += P.query(u, ina);
    }

    // 2nd: update residual
    auto np = NodePair(v, a);
    float left_part = c * estimation_sum / (in_deg_v + 1) / in_degree(a, g);
    float right_part = (P.query(v, a) + R.query(v, a)) / in_deg_v;
    R[np] = left_part - right_part;
    // 3rd: update queue
    if (fabs(R[np]) > r_max) {
        if (!marker[np]) {
            Q.push(np);
            marker[np] = true;
        }
    }
}

void LocalPush::insert(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // insert edge(u,v) to g, noted we assume g is already updated, and we just update P and R
    DirectedG::vertex_iterator v_it, v_end;
    tie(v_it, v_end) = vertices(g);
    for (; v_it != v_end; v_it++) {
        auto a = *v_it;
//        auto in_deg_v = in_degree(v, g); // the new degree of v
        if (a > v) {
            update_residual(g, v, a);
        } else if (a < v) {
            update_residual(g, a, v);
        } else {
            // a == v
            R[NodePair(a, a)] = 1 - P[NodePair(a, a)];
        }
    }
//    local_push(g);
}

void LocalPush::remove(DirectedG::vertex_descriptor u, DirectedG::vertex_descriptor v,
                       DirectedG &g) {
    // remove edge(a,b) to g, noted we assume g is already updated, and we just update P and R
}

double LocalPush::query_P(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a > b) {
        return P[NodePair(b, a)];
    } else {
        return P[NodePair(a, b)];
    }
}

double LocalPush::query_R(DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    if (a > b) {
        return R[NodePair(b, a)];
    } else {
        return R[NodePair(a, b)];
    }
}

void Reduced_LocalPush::update_residual(DirectedG &g, DirectedG::vertex_descriptor a, DirectedG::vertex_descriptor b) {
    // cout << "-----" << endl;
    // cout << "updating residual "<< a << " " << b << endl;
    NodePair np(a, b); // a < b
    PairMarker indicator; // the indicator of in-neighbors of (a,b)
    DirectedG::in_edge_iterator in_a_it, in_a_end, in_b_it, in_b_end;
    tie(in_a_it, in_a_end) = in_edges(a, g);
    auto in_deg_a = in_degree(a, g);
    auto in_deg_b = in_degree(b, g);
    if (in_deg_a * in_deg_b > 0) {
//        cout << format("in_deg_a:%s, in_deg_b:%s") % in_deg_a % in_deg_b << endl;
        double sum_neighbor_residuals = 0;
        for (; in_a_it != in_a_end; in_a_it++) {
            tie(in_b_it, in_b_end) = in_edges(b, g);
            for (; in_b_it != in_b_end; in_b_it++) {
                auto ina = source(*in_a_it, g);
                auto inb = source(*in_b_it, g);
                // indicator[NodePair(min(ina,inb), max(ina,inb))] = true;
                sum_neighbor_residuals += P[NodePair(min(ina, inb), max(ina, inb))];
            }
        }
        // for(auto& item:indicator){
        //     // cout << "collect neighbor P (" << item.first.first
        //     //     << "," << item.first.second <<"): " << P[item.first]  << endl;
        //     sum_neighbor_residuals += P[item.first]; // item.first is the node pair
        // }
        R[np] = c * sum_neighbor_residuals / (in_deg_a * in_deg_b) - P[np];
        // cout << "neighbros residuals " << sum_neighbor_residuals << endl;
        // cout << " new residual " << R[np] <<  " its current estimates " << P[np] << endl;
        if (fabs(R[NodePair(a, b)]) > r_max) {
            if (marker[np] == false) {
                Q.push(np);
                marker[np] = true;
            }
        }
    }
}

// dynamic update support: deleting and inserting edge, time complexity: O(E)
void Reduced_LocalPush::update_residuals_by_deleting_edge(DirectedG &g, DirectedG::vertex_descriptor u,
                                                          DirectedG::vertex_descriptor v) {
    // udpate residual when an edge (u,v) is removed from G
    // assumption: G has not been updated, (u,v) exists in G
    // a: the iterated node over G
    // v: the node whose in-neighbor is changed
    DirectedG::vertex_iterator v_begin, v_end, v_it;
    tie(v_begin, v_end) = vertices(g);
    for (v_it = v_begin; v_it != v_end; v_it++) {
        auto a = *v_it;
        if (a != v && in_degree(a, g) != 0) {
            NodePair np = a < v ? NodePair(a, v) : NodePair(v, a); // the node pair to be updated

            DirectedG::in_edge_iterator in_a_iter, in_a_begin, in_a_end;
            double P_av = P.query(np.first, np.second); // current P(a,v)
            auto &R_av_ref = R[np]; // current R(a,v)
            double updated_R = P_av + R_av_ref;

            if (in_degree(v, g) == 1) {
                // u is the only in-neighbor of v
                updated_R = -P_av;
            } else {
                tie(in_a_begin, in_a_end) = in_edges(a, g);
                double u_contrib = 0;
                //iterate over a's in-neighbors
                for (in_a_iter = in_a_begin; in_a_iter != in_a_end; in_a_iter++) {
                    auto a_prime = source(*in_a_iter, g);
                    double contrib = 0;
                    if (a_prime > u) {
                        contrib = c * P.query(u, a_prime);
                    } else if (a_prime < u) {
                        contrib = c * P.query(a_prime, u);
                    } else if (a_prime == u) {
                        contrib = c * sqrt(2) * P.query(a_prime, u);
                    }
                    u_contrib += contrib;
                }
                u_contrib = u_contrib / (in_degree(v, g) * in_degree(a, g));
                updated_R = updated_R - u_contrib;
                updated_R = updated_R * ((in_degree(v, g)) / (in_degree(v, g) - 1.0));
                updated_R -= P_av;
            }
            R_av_ref = updated_R;

            if (fabs(updated_R) / sqrt(2) > r_max) {
                if (!marker[np]) {
                    Q.push(np);
                    marker[np] = true;
                }
            }
        }
    }
}

void Reduced_LocalPush::update_residuals_by_adding_edge(DirectedG &g, DirectedG::vertex_descriptor u,
                                                        DirectedG::vertex_descriptor v) {
    // update the residual when an edge (u,v) is inserted to g
    // node a: the iterated node over G
    // node v: whose in-neighbor has changed (u added)
    // assumption: g has not been updated, (u,v) does not exist in g
    DirectedG::vertex_iterator v_begin, v_end, v_it;
    tie(v_begin, v_end) = vertices(g);
    for (v_it = v_begin; v_it != v_end; v_it++) { // ignore the starting node 0
        auto a = *v_it;

        if (a != v && in_degree(a, g) != 0) {
            double u_contrib = 0; // the amount of R^{'} related to u
            NodePair np = a < v ? NodePair(a, v) : NodePair(v, a); // the node pair to be updated

            auto &residual_ref = R[np];
            auto scaled_rest =
                    (in_degree(v, g) / (in_degree(v, g) + 1.0)) * (residual_ref + P.query(np.first, np.second));

            // iterate over a's in-neighbors
            DirectedG::in_edge_iterator in_a_iter, in_a_begin, in_a_end;
            tie(in_a_begin, in_a_end) = in_edges(a, g);
            for (in_a_iter = in_a_begin; in_a_iter != in_a_end; in_a_iter++) {
                auto a_prime = source(*in_a_iter, g);
                double contrib = 0;
                if (a_prime > u) {
                    contrib = c * P.query(u, a_prime);
                } else if (a_prime < u) {
                    contrib = c * P.query(a_prime, u);
                } else if (a_prime == u) {
                    contrib = c * sqrt(2) * P.query(a_prime, u);
                }
                u_contrib += contrib;
            }
            u_contrib = u_contrib / ((in_degree(v, g) + 1.0) * in_degree(a, g));

            auto updated_residual = u_contrib + scaled_rest - P.query(np.first, np.second);
            residual_ref = updated_residual;

            if (fabs(updated_residual) / sqrt(2) > r_max) {
                if (!marker[np]) {
                    Q.push(np);
                    marker[np] = true;
                }
            }
        }
    }
}

void Reduced_LocalPush::update_edges(DirectedG &g, vector<NodePair> edges, char update_type = '+') {
    // update G by a set of edges
    if (update_type == '+') {
        for (auto &e: edges) {
            int s = e.first;
            int t = e.second;
            update_residuals_by_adding_edge(g, s, t);
            add_edge(s, t, g);
        }
    } else if (update_type == '-') {
        for (auto &e: edges) {
            int s = e.first;
            int t = e.second;

            if (edge(s, t, g).second) {
                update_residuals_by_deleting_edge(g, s, t);
                remove_edge(s, t, g);
            }
        }
    } else {
        cout << "Please indicate the edge update type" << endl;
        return;
    }
    log_info("finish updating residuals...");
    Reduced_LocalPush::local_push(g);
}


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_MODELS_DYNAMIC_INTEGRATION_HPP_
#define BARK_MODELS_DYNAMIC_INTEGRATION_HPP_
#include <Eigen/Core>

#include "bark/geometry/angle.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"

namespace bark {
namespace models {
namespace dynamic {

inline State euler_int(const DynamicModel& model,
                const State &x,
                const Input &u,
                float dt) {
  State new_x = x + dt * model.StateSpaceModel(x, u);
  new_x(StateDefinition::THETA_POSITION) =
      geometry::Norm0To2PI(new_x(StateDefinition::THETA_POSITION));
  return new_x;
}

inline State rk4(const DynamicModel& model, const State& x, const Input& u,
                 float dt) {
  State k0 = dt * model.StateSpaceModel(x, u);
  State k1 = dt * model.StateSpaceModel(x + k0 / 2, u);
  State k2 = dt * model.StateSpaceModel(x + k1 / 2, u);
  State k3 = dt * model.StateSpaceModel(x + k2, u);
  return x + 1.0 / 6.0 * (k0 + 2 * k1 + 2 * k2 + k3);
}

}  // namespace dynamic
}  // namespace models
}  // namespace bark

#endif  // BARK_MODELS_DYNAMIC_INTEGRATION_HPP_


#pragma once
#ifndef CANNON_PHYSICS_RK4_INTEGRATOR_H
#define CANNON_PHYSICS_RK4_INTEGRATOR_H

/*!
 * \file cannon/physics/rk4_integrator.hpp
 * \brief File containing RK4Integrator class definition.
 */

#include <functional>

#include <Eigen/Dense>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

using namespace Eigen;
using namespace boost::numeric::odeint;

namespace cannon {
  namespace physics {

    /*!
     * \brief Class representing a fourth-order Runge-Kutta integrator.
     */
    class RK4Integrator {
      public:
        using system_type = std::function<void(const VectorXd&, VectorXd&, double)>;

        RK4Integrator() = delete;

        /*!
         * \brief Constructor taking the system to integrate, state dimension,
         * and timestep for integration.
         */
        RK4Integrator(system_type system, unsigned int state_dim, double
            dt) : system_(system), state_dim_(state_dim), dt_(dt) {
          state_ = VectorXd::Zero(state_dim_);
        }

        /*!
         * \brief Perform a single integration step using the timestep of this
         * integrator.
         *
         * \returns The new state, post-integration.
         */
        const VectorXd& step();

        /*!
         * \brief Set the state of the system being integrated.
         *
         * \param state The state to set.
         */
        void set_state(const VectorXd& state);

        /*!
         * \brief Get the current state of the system being integrated.
         *
         * \returns The current state.
         */
        VectorXd get_state() const;

        /*!
         * \brief Set the current time for the system being integrated.
         *
         * \param t The time to set.
         */
        void set_time(double t);

        /*!
         * \brief Get the current time in the system being integrated.
         *
         * \returns The current time.
         */
        double get_time() const;

      private:
        //using stepper_type = runge_kutta4<VectorXd, double, VectorXd, double, vector_space_algebra>;
        using stepper_type = runge_kutta_dopri5<VectorXd, double, VectorXd, double, vector_space_algebra>;

        system_type system_; //!< The system to be integrated
        stepper_type stepper_; //!< Internal ODEInt stepper

        unsigned int state_dim_; //!< State dimension of the system being integrated
        VectorXd state_; //!< Current state of the system being integrated

        double dt_; //!< Integration timestep
        double t_ = 0.0; //!< Current time in the integrated system

    };

  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_RK4_INTEGRATOR_H */