#include "multiplicative_relations.h"
#include <hamming/jain_commitment/jain_commitment.h>
#include <NTL/mat_GF2.h>
#include <NTL/vec_GF2.h>
#include <hamming/knowledge_of_valid_opening/knowledge_of_valid_opening.h>
#include <utils/utils.h>

void hamming_metric::multiplicative_relations::initialize_commitments_and_responses(
        commitments_t *commitments,
        responses_t *responses) {

    responses->t_i_0.SetLength(I);
    responses->t_i_1.SetLength(I);
    responses->t_i_2.SetLength(I);
    responses->t_i_j_0.SetLength(I);
    responses->t_i_j_1.SetLength(I);
    responses->t_i_j_2.SetLength(I);

    commitments->c_i_0.SetLength(I);
    commitments->c_i_1.SetLength(I);
    commitments->c_i_2.SetLength(I);
    commitments->c_i_j_0.SetLength(I);
    commitments->c_i_j_1.SetLength(I);
    commitments->c_i_j_2.SetLength(I);

    commitments->r_i_0.SetLength(I);
    commitments->r_i_1.SetLength(I);
    commitments->r_i_2.SetLength(I);
    commitments->r_i_j_0.SetLength(I);
    commitments->r_i_j_1.SetLength(I);
    commitments->r_i_j_2.SetLength(I);

    for (int i = 0; i < I; i++) {
        responses->t_i_j_0[i].SetLength(J);
        responses->t_i_j_1[i].SetLength(J);
        responses->t_i_j_2[i].SetLength(J);

        commitments->c_i_j_0[i].SetLength(J);
        commitments->c_i_j_1[i].SetLength(J);
        commitments->c_i_j_2[i].SetLength(J);

        commitments->r_i_j_0[i].SetLength(J);
        commitments->r_i_j_1[i].SetLength(J);
        commitments->r_i_j_2[i].SetLength(J);
    }
}

void hamming_metric::multiplicative_relations::generate_private_key(
        private_key_t *private_key) {

    //Generate m
    {
        private_key->m.kill();
        private_key->m.SetLength(I);

        utils::generate_random_binary_vector(
                private_key->m[0],
                JAIN_V);

        utils::generate_random_binary_vector(
                private_key->m[1],
                JAIN_V);

        private_key->m[2].SetLength(JAIN_V);
        for (int i = 0; i < JAIN_V; i++) {
            private_key->m[2][i] = private_key->m[0][i] * private_key->m[1][i];
        }
    }

    //Generate e
    {
        private_key->e.kill();
        private_key->e.SetLength(I);

        for (int i = 0; i < I; i++) {
            utils::generate_vector_of_weight_w(
                    private_key->e[i],
                    JAIN_K,
                    W);
        }
    }
}

void hamming_metric::multiplicative_relations::generate_public_key(
        public_key_t *public_key,
        const private_key_t *private_key) {

    public_key->commitments_i.kill();
    public_key->commitments_i.SetLength(I);

    utils::generate_random_binary_matrix(
            public_key->A,
            JAIN_K,
            JAIN_L + JAIN_V);

    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 r;
        utils::generate_random_binary_vector(
                r,
                JAIN_L);

        hamming_metric::commitment::generate_commitment(
                public_key->commitments_i[i],
                public_key->A,
                r,
                private_key->m[i],
                private_key->e[i]);
    }
}

void hamming_metric::multiplicative_relations::generate_random_values(
        random_values_t *random_values,
        const multiplicative_relation_matrices_t *matrices) {

    //Generate u_i_j
    {
        random_values->u_i_j.kill();
        random_values->u_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->u_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->u_i_j[i][j] = NTL::random_vec_GF2(JAIN_L);
            }
        }
    }

    //Generate f_i_j
    {
        random_values->f_i_j.kill();
        random_values->f_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->f_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->f_i_j[i][j] = NTL::random_vec_GF2(JAIN_K);
            }
        }
    }

    //Generate v_i_j
    {
        random_values->v_i_j.kill();
        random_values->v_i_j.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->v_i_j[i].SetLength(J);
            for (int j = 0; j < J; j++) {
                random_values->v_i_j[i][j] = NTL::random_vec_GF2(JAIN_V);
            }
        }
    }

    {
        random_values->u_i.kill();
        random_values->u_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->u_i[i] = NTL::random_vec_GF2(JAIN_L);
        }
    }

    {
        random_values->f_i.kill();
        random_values->f_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->f_i[i] = NTL::random_vec_GF2(JAIN_K);
        }
    }

    //Generate v_i
    {
        random_values->v_i.kill();
        random_values->v_i.SetLength(I);
        for (int i = 0; i < I; i++) {
            random_values->v_i[i].SetLength(JAIN_V);
            for (int j = 0; j < J; j++) {
                random_values->v_i[i] += (matrices->R[j] * random_values->v_i_j[i][j]);
            }
        }
    }
}

void hamming_metric::multiplicative_relations::generate_revealed_values(
        revealed_values_t *revealed_values,
        const private_key_t *private_key,
        const public_key_t *public_key) {

    revealed_values->commitments_i_j.kill();
    revealed_values->commitments_i_j.SetLength(I);

    revealed_values->e_i_j.kill();
    revealed_values->e_i_j.SetLength(I);

    revealed_values->P_i.SetLength(I);
    revealed_values->P_i_j.SetLength(I);

    NTL::Vec<NTL::vec_GF2> m_prime_i;
    {
        m_prime_i.SetLength(I);
        utils::sample_messages(m_prime_i[0], m_prime_i[1], m_prime_i[2], JAIN_V);
        utils::generate_relation_matrix(
                revealed_values->matrices._R,
                revealed_values->matrices.R,
                m_prime_i,
                private_key->m,
                JAIN_V);
    }

    {
        generate_m_prime_i_j_from_m_prime_i(
                revealed_values->m_prime_i_j,
                m_prime_i);
    }

    NTL::vec_GF2 r;
    for (int i = 0; i < I; i++) {
        revealed_values->commitments_i_j[i].SetLength(J);
        revealed_values->e_i_j[i].SetLength(J);
        revealed_values->P_i_j[i].SetLength(J);

        utils::create_permutation_matrix(
                revealed_values->P_i[i],
                JAIN_K);

        for (int j = 0; j < J; j++) {
            utils::create_permutation_matrix(
                    revealed_values->P_i_j[i][j],
                    JAIN_K);

            utils::generate_random_binary_vector(
                    r,
                    JAIN_L);

            utils::generate_vector_of_weight_w(
                    revealed_values->e_i_j[i][j],
                    JAIN_K,
                    W);

            hamming_metric::commitment::generate_commitment(
                    revealed_values->commitments_i_j[i][j],
                    public_key->A,
                    r,
                    revealed_values->m_prime_i_j[i][j],
                    revealed_values->e_i_j[i][j]);
        }
    }
}

void hamming_metric::multiplicative_relations::generate_m_prime_i_j_from_m_prime_i(
        NTL::Vec<NTL::Vec<NTL::vec_GF2>> &m_prime_i_j,
        const NTL::Vec<NTL::vec_GF2> &m_prime_i) {

    m_prime_i_j.kill();
    m_prime_i_j.SetLength(I);
    int start_index;

    for (int i = 0; i < I; i++) {
        m_prime_i_j[i].SetLength(J);
        start_index = 0;
        for (int j = 0; j < J; j++) {
            NTL::vec_GF2 tmp;
            tmp.SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                tmp[z] = m_prime_i[i][start_index + z];
            }
            start_index += JAIN_V;
            m_prime_i_j[i][j].append(tmp);
        }
    }

}

void hamming_metric::multiplicative_relations::generate_commitments_and_responses(
        responses_t *responses,
        commitments_t *commitments,
        const random_values_t *random_values,
        const revealed_values_t *revealed_values,
        const public_key_t *public_key,
        const private_key_t *private_key) {

    for (int i = 0; i < I; i++) {

        auto u_v = random_values->u_i[i];
        u_v.append(random_values->v_i[i]);
        knowledge_of_valid_opening::generate_commitment_and_response_0(
                commitments->c_i_0[i],
                commitments->r_i_0[i],
                responses->t_i_0[i],
                u_v,
                public_key->A,
                random_values->f_i[i]);

        knowledge_of_valid_opening::generate_commitment_and_response_1(
                commitments->c_i_1[i],
                commitments->r_i_1[i],
                responses->t_i_1[i],
                public_key->A,
                revealed_values->P_i[i],
                random_values->f_i[i]);

        knowledge_of_valid_opening::generate_commitment_and_response_2(
                commitments->c_i_2[i],
                commitments->r_i_2[i],
                responses->t_i_2[i],
                public_key->A,
                revealed_values->P_i[i],
                random_values->f_i[i],
                private_key->e[i]);

        for (int j = 0; j < J; j++) {

            u_v = random_values->u_i_j[i][j];
            u_v.append(random_values->v_i_j[i][j]);
            knowledge_of_valid_opening::generate_commitment_and_response_0(
                    commitments->c_i_j_0[i][j],
                    commitments->r_i_j_0[i][j],
                    responses->t_i_j_0[i][j],
                    u_v,
                    public_key->A,
                    random_values->f_i_j[i][j]);

            knowledge_of_valid_opening::generate_commitment_and_response_1(
                    commitments->c_i_j_1[i][j],
                    commitments->r_i_j_1[i][j],
                    responses->t_i_j_1[i][j],
                    public_key->A,
                    revealed_values->P_i_j[i][j],
                    random_values->f_i_j[i][j]);

            knowledge_of_valid_opening::generate_commitment_and_response_2(
                    commitments->c_i_j_2[i][j],
                    commitments->r_i_j_2[i][j],
                    responses->t_i_j_2[i][j],
                    public_key->A,
                    revealed_values->P_i_j[i][j],
                    random_values->f_i_j[i][j],
                    revealed_values->e_i_j[i][j]);
        }
    }
}

int hamming_metric::multiplicative_relations::verify_0(
        const NTL::Vec<NTL::vec_GF2> &c_i_0,
        const NTL::Vec<NTL::vec_GF2> &r_i_0,
        const NTL::Vec<NTL::vec_GF2> &t_i_0,
        const NTL::Vec<NTL::vec_GF2> &c_i_1,
        const NTL::Vec<NTL::vec_GF2> &r_i_1,
        const NTL::Vec<NTL::vec_GF2> &t_i_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_1,
        const NTL::Vec<NTL::mat_GF2> &P_i,
        const NTL::Vec<NTL::Vec<NTL::mat_GF2>> &P_i_j,
        const NTL::Vec<NTL::mat_GF2> &R,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t0 = utils::encode_binary_vector(t_i_0[i], JAIN_V);
        auto _t1 = utils::encode_binary_vector(t_i_1[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_0[i],
                public_key->A,
                r_i_0[i],
                _t0) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c0" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_1[i],
                public_key->A,
                r_i_1[i],
                _t1) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c1" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t0 = utils::encode_binary_vector(t_i_j_0[i][j], JAIN_V);
            _t1 = utils::encode_binary_vector(t_i_j_1[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_0[i][j],
                    public_key->A,
                    r_i_j_0[i][j],
                    _t0) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c0" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_1[i][j],
                    public_key->A,
                    r_i_j_1[i][j],
                    _t1) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 0 and c1" << std::endl;
                return 1;
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> results_i;
    NTL::Vec<NTL::Vec<NTL::vec_GF2>> results_i_j;
    results_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 result;

        if (utils::solve_equation(
                result,
                public_key->A,
                t_i_0[i] + (NTL::inv(P_i[i]) * t_i_1[i])) != 0) {
            std::cout << "No Solutions" << std::endl;
            return 1;
        }
        results_i.append(result);

        for (int j = 0; j < J; j++) {
            if (utils::solve_equation(
                    result,
                    public_key->A,
                    t_i_j_0[i][j] + (NTL::inv(P_i_j[i][j]) * t_i_j_1[i][j])) != 0) {
                std::cout << "No Solutions" << std::endl;
                return 1;
            }
            results_i_j[i].append(result);
        }
    }

    NTL::Vec<NTL::vec_GF2> b_i;
    b_i.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i[i].SetLength(JAIN_V);
        for (int j = 0; j < JAIN_V; j++) {
            b_i[i][j] = results_i[i][j + JAIN_L];
        }
    }

    NTL::Vec<NTL::Vec<NTL::vec_GF2>> b_i_j;
    b_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i_j[i].SetLength(J);
        for (int j = 0; j < J; j++) {
            b_i_j[i][j].SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                b_i_j[i][j][z] = results_i_j[i][j][z + JAIN_L];
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> r_b;
    r_b.SetLength(I);
    for (int i = 0; i < I; i++) {
        for (int j = 0; j < J; j++) {
            r_b[i].SetLength(JAIN_V);
            r_b[i] += (R[j] * b_i_j[i][j]);
        }
        if (b_i[i] != r_b[i]) {
            std::cout << "Error: verify_0" << std::endl;
            return 1;
        }
    }

    return 0;
}

int hamming_metric::multiplicative_relations::verify_1(
        const NTL::Vec<NTL::vec_GF2> &c_i_0,
        const NTL::Vec<NTL::vec_GF2> &r_i_0,
        const NTL::Vec<NTL::vec_GF2> &t_i_0,
        const NTL::Vec<NTL::vec_GF2> &c_i_2,
        const NTL::Vec<NTL::vec_GF2> &r_i_2,
        const NTL::Vec<NTL::vec_GF2> &t_i_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_0,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_2,
        const NTL::Vec<NTL::mat_GF2> &P_i,
        const NTL::Vec<NTL::Vec<NTL::mat_GF2>> &P_i_j,
        const NTL::Vec<NTL::mat_GF2> &R,
        const NTL::mat_GF2 &_R,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &commitments_i_j,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t0 = utils::encode_binary_vector(t_i_0[i], JAIN_V);
        auto _t2 = utils::encode_binary_vector(t_i_2[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_0[i],
                public_key->A,
                r_i_0[i],
                _t0) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c0" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_2[i],
                public_key->A,
                r_i_2[i],
                _t2) != 0) {
            std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c2" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t0 = utils::encode_binary_vector(t_i_j_0[i][j], JAIN_V);
            _t2 = utils::encode_binary_vector(t_i_j_2[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_0[i][j],
                    public_key->A,
                    r_i_j_0[i][j],
                    _t0) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c0" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_2[i][j],
                    public_key->A,
                    r_i_j_2[i][j],
                    _t2) != 0) {
                std::cout << "Multiplicative Relations. Verification failed on ch = 1 and c2" << std::endl;
                return 1;
            }
        }
    }

    // Check rank of matrix
    auto m = _R;
    if (NTL::gauss(m) != m.NumRows()) {
        std::cout << "Not full rank matrix" << std::endl;
        return 1;
    }

    // Ensure that each row has weight one
    for (int i = 0; i < m.NumRows(); i++) {
        if (NTL::weight(m[i]) != 1) {
            std::cout << "Invalid weight" << std::endl;
            return 1;
        }
    }

    NTL::Vec<NTL::vec_GF2> results_i;
    NTL::Vec<NTL::Vec<NTL::vec_GF2>> results_i_j;
    results_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        NTL::vec_GF2 result;

        if (utils::solve_equation(
                result,
                public_key->A,
                public_key->commitments_i[i] + t_i_0[i] + (NTL::inv(P_i[i]) * t_i_2[i])) != 0) {
            std::cout << "No Solutions 1" << std::endl;
            return 1;
        }
        results_i.append(result);

        for (int j = 0; j < J; j++) {
            if (utils::solve_equation(
                    result,
                    public_key->A,
                    commitments_i_j[i][j] + t_i_j_0[i][j] +
                    (NTL::inv(P_i_j[i][j]) * t_i_j_2[i][j])) != 0) {
                std::cout << "No Solutions 2" << std::endl;
                return 1;
            }
            results_i_j[i].append(result);
        }
    }

    NTL::Vec<NTL::vec_GF2> b_i;
    b_i.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i[i].SetLength(JAIN_V);
        for (int j = 0; j < JAIN_V; j++) {
            b_i[i][j] = results_i[i][j + JAIN_L];
        }
    }

    NTL::Vec<NTL::Vec<NTL::vec_GF2>> b_i_j;
    b_i_j.SetLength(I);
    for (int i = 0; i < I; i++) {
        b_i_j[i].SetLength(J);
        for (int j = 0; j < J; j++) {
            b_i_j[i][j].SetLength(JAIN_V);
            for (int z = 0; z < JAIN_V; z++) {
                b_i_j[i][j][z] = results_i_j[i][j][z + JAIN_L];
            }
        }
    }

    NTL::Vec<NTL::vec_GF2> r_b;
    r_b.SetLength(I);
    for (int i = 0; i < I; i++) {
        for (int j = 0; j < J; j++) {
            r_b[i].SetLength(JAIN_V);
            r_b[i] += (R[j] * b_i_j[i][j]);
        }
        if (b_i[i] != r_b[i]) {
            std::cout << "Error: verify_0" << std::endl;
            return 1;
        }
    }

    return 0;
}

int hamming_metric::multiplicative_relations::verify_2(
        const NTL::Vec<NTL::vec_GF2> &c_i_1,
        const NTL::Vec<NTL::vec_GF2> &r_i_1,
        const NTL::Vec<NTL::vec_GF2> &t_i_1,
        const NTL::Vec<NTL::vec_GF2> &c_i_2,
        const NTL::Vec<NTL::vec_GF2> &r_i_2,
        const NTL::Vec<NTL::vec_GF2> &t_i_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_1,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &c_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &r_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &t_i_j_2,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &e_i_j,
        const NTL::Vec<NTL::Vec<NTL::vec_GF2>> &m_prime_i_j,
        const public_key_t *public_key) {

    for (int i = 0; i < I; i++) {
        auto _t1 = utils::encode_binary_vector(t_i_1[i], JAIN_V);
        auto _t2 = utils::encode_binary_vector(t_i_2[i], JAIN_V);

        if (hamming_metric::commitment::verify(
                c_i_1[i],
                public_key->A,
                r_i_1[i],
                _t1) != 0) {
            std::cout << "Linear Relations. Verification failed on ch = 2 and c1" << std::endl;
            return 1;
        }

        if (hamming_metric::commitment::verify(
                c_i_2[i],
                public_key->A,
                r_i_2[i],
                _t2) != 0) {
            std::cout << "Linear Relations. Verification failed on ch = 2 and c2" << std::endl;
            return 1;
        }
        for (int j = 0; j < J; j++) {
            _t1 = utils::encode_binary_vector(t_i_j_1[i][j], JAIN_V);
            _t2 = utils::encode_binary_vector(t_i_j_2[i][j], JAIN_V);

            if (hamming_metric::commitment::verify(
                    c_i_j_1[i][j],
                    public_key->A,
                    r_i_j_1[i][j],
                    _t1) != 0) {
                std::cout << "Linear Relations. Verification failed on ch = 2 and c1" << std::endl;
                return 1;
            }

            if (hamming_metric::commitment::verify(
                    c_i_j_2[i][j],
                    public_key->A,
                    r_i_j_2[i][j],
                    _t2) != 0) {
                std::cout << "Linear Relations. Verification failed on ch = 2 and c2" << std::endl;
                return 1;
            }

            if (NTL::weight(e_i_j[i][j]) != W) {
                std::cout << "Error: Wrong weight" << std::endl;
                return 1;
            }

        }
    }

    for (int j = 0; j < J; j++) {
        for (int z = 0; z < m_prime_i_j[0][j].length(); z++) {
            if (m_prime_i_j[0][j][z] * m_prime_i_j[1][j][z] !=
                m_prime_i_j[2][j][z]) {
                std::cout << "Error: Wrong relations" << std::endl;
                return 1;
            }
        }

    }

    return 0;
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

/* Wrapper for DSYEV
 *  - make copy of input matrix
 */

bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = A.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues_symmetric( ub::symmetric_matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = A.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL does not handle conversion of a symmetric_matrix
    V = A;

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues(  ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = V.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    // V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    // V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

bool linalg_eigenvalues(  ub::vector<float> &E, ub::matrix<float> &V)
{
    // cout << " \n I'm really using MKL! " << endl;

    int n = V.size1();
    int lda = n ;
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    // V.resize(n, n);
    // Query and allocate the optimal workspace
    float wkopt;
    float* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    // V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    float * pV = const_cast<float*>(&V.data().begin()[0]);
    float * pE = const_cast<float*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_ssyev( LAPACK_ROW_MAJOR, 'V', 'U', n, pV , lda, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<double> &A, ub::vector<double> &E, ub::matrix<double> &V , int nmax)
{
    /*
     * INPUT:  matrix A (N,N)
     * OUTPUT: matrix V (N,NMAX)
     *         vector E (NMAX)
     */
    double wkopt;
    double* work;
    double abstol, vl, vu;

    MKL_INT lda;
    MKL_INT info;
    MKL_INT lwork;
    MKL_INT il, iu, m, ldz ;

    int n = A.size1();
    MKL_INT ifail[n];
    lda = n;
    ldz = nmax;

    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(nmax);
    V.resize(n,nmax);

    lwork = -1;
    il = 1;
    iu = nmax;
    abstol = 0.0; // use default
    vl = 0.0;
    vu = 0.0;
    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pA = const_cast<double*>(&A.data().begin()[0]);
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsyevx( LAPACK_ROW_MAJOR, 'V', 'I', 'U', n, pA , lda, vl, vu, il, iu, abstol, &m, pE, pV, nmax,  ifail );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/*
 * use expert routine to calculate only a subrange of eigenvalues
 */
bool linalg_eigenvalues( ub::matrix<float> &A, ub::vector<float> &E, ub::matrix<float> &V , int nmax)
{
    /*
     * INPUT:  matrix A (N,N)
     * OUTPUT: matrix V (N,NMAX)
     *         vector E (NMAX)
     */
    float wkopt;
    float* work;
    float abstol, vl, vu;

    MKL_INT lda;
    MKL_INT info;
    MKL_INT lwork;
    MKL_INT il, iu, m, ldz ;

    int n = A.size1();
    MKL_INT ifail[n];
    lda = n;
    ldz = nmax;

    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(nmax);
    V.resize(n,nmax);

    lwork = -1;
    il = 1;
    iu = nmax;
    abstol = 0.0; // use default
    vl = 0.0;
    vu = 0.0;
    // make a pointer to the ublas matrix so that LAPACK understands it
    float * pA = const_cast<float*>(&A.data().begin()[0]);
    float * pV = const_cast<float*>(&V.data().begin()[0]);
    float * pE = const_cast<float*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_ssyevx( LAPACK_ROW_MAJOR, 'V', 'I', 'U', n, pA , lda, vl, vu, il, iu, abstol, &m, pE, pV, nmax,  ifail );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

/* calculate the eigenvalues and vectors of the generalized eigenvalue problem */
bool linalg_eigenvalues_general( ub::matrix<double> &A,ub::matrix<double> &B, ub::vector<double> &E, ub::matrix<double> &V)
{
    // cout << " \n I'm really using MKL! " << endl;
    //check to see if matrices have same size
    int n = A.size1();
    int lda = n ;
    int ldb =B.size1();
    ub::matrix<double> _B(ldb,ldb);
    _B=B;
    if (lda!=ldb){
        cout << "Matrices A and B have not the same size"<< endl;
        exit(1);
    }
    // make sure that containers for eigenvalues and eigenvectors are of correct size
    E.resize(n);
    V.resize(n, n);
    // Query and allocate the optimal workspace
    double wkopt;
    double* work;
    int info;
    int lwork;
    lwork = -1;

    // MKL is different to GSL because it overwrites the input matrix
    V = A; // make a copy (might actually be unnecessary in most cases!)

    // make a pointer to the ublas matrix so that LAPACK understands it
    double * pB = const_cast<double*>(&_B.data().begin()[0]);
    double * pV = const_cast<double*>(&V.data().begin()[0]);
    double * pE = const_cast<double*>(&E.data()[0]);

    // call LAPACK via C interface
    info = LAPACKE_dsygv( LAPACK_ROW_MAJOR,1,'V', 'U', n, pV , lda,pB,ldb, pE );

    if( info > 0 ) {
        return false;
    } else {
        return true;
    }
};

}}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

#include "gtest/gtest.h"

#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/camera/camera.h"
#include "theia/sfm/feature.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/random.h"

namespace theia {

using Eigen::Vector3d;

void RelativeRotationFromTwoRotations(const Vector3d& rotation1,
                                      const Vector3d& rotation2,
                                      Vector3d* relative_rotation) {
  Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation_matrix1.data());
  ceres::AngleAxisToRotationMatrix(rotation2.data(), rotation_matrix2.data());

  const Eigen::Matrix3d relative_rotation_mat =
      rotation_matrix2 * rotation_matrix1.transpose();
  ceres::RotationMatrixToAngleAxis(relative_rotation_mat.data(),
                                   relative_rotation->data());
}

void RelativeTranslationFromTwoPositions(const Vector3d& position1,
                                         const Vector3d& position2,
                                         const Vector3d& rotation1,
                                         Vector3d* translation) {
  Eigen::Matrix3d rotation_matrix1;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation_matrix1.data());
  *translation = rotation_matrix1 * (position2 - position1).normalized();
}

void TwoViewInfoFromCameras(const Camera& camera1,
                            const Camera& camera2,
                            TwoViewInfo* info) {
  RelativeRotationFromTwoRotations(camera1.GetOrientationAsAngleAxis(),
                                   camera2.GetOrientationAsAngleAxis(),
                                   &info->rotation_2);
  RelativeTranslationFromTwoPositions(camera1.GetPosition(),
                                      camera2.GetPosition(),
                                      camera1.GetOrientationAsAngleAxis(),
                                      &info->position_2);
}

// Cameras are generated within a 2x2x2 box around the origin looking
// approximately toward the negative z-axis. The scene depth indicates roughly
// how many units away from the origin the 3D points will be when created. The
// points will always be in front of all cameras.
void TestTripletBaselineComputation(const double pixel_noise,
                                    const double scene_depth,
                                    const double tolerance) {
  InitRandomGenerator();

  static const int kNumTracks = 100;
  static const double kFocalLength = 1000;

  // Set up 3 views.
  Camera camera1, camera2, camera3;
  camera1.SetPosition(Vector3d(-1, 0.8, -0.2));
  camera1.SetOrientationFromAngleAxis(0.2 * Vector3d::Random());
  camera2.SetPosition(Vector3d(0.2, 0.3, -1.0));
  camera2.SetOrientationFromAngleAxis(0.2 * Vector3d::Random());
  camera3.SetPosition(Vector3d(0.8, -0.7, 0.3));
  camera3.SetOrientationFromAngleAxis(0.2 * Vector3d::Random());

  // Add tracks.
  std::vector<Feature> feature1(kNumTracks), feature2(kNumTracks),
      feature3(kNumTracks);
  for (int i = 0; i < kNumTracks; i++) {
    const Eigen::Vector4d point = Eigen::Vector3d::Random().homogeneous() +
                                  Eigen::Vector4d(0, 0, scene_depth, 0);

    // Add the observations (plus noise if applicable).
    camera1.ProjectPoint(point, &feature1[i]);
    camera2.ProjectPoint(point, &feature2[i]);
    camera3.ProjectPoint(point, &feature3[i]);

    if (pixel_noise > 0) {
      feature1[i].x() += RandGaussian(0.0, pixel_noise / kFocalLength);
      feature1[i].y() += RandGaussian(0.0, pixel_noise / kFocalLength);
      feature2[i].x() += RandGaussian(0.0, pixel_noise / kFocalLength);
      feature2[i].y() += RandGaussian(0.0, pixel_noise / kFocalLength);
      feature3[i].x() += RandGaussian(0.0, pixel_noise / kFocalLength);
      feature3[i].y() += RandGaussian(0.0, pixel_noise / kFocalLength);
    }
  }

  // Compute the baseline ratios.
  Eigen::Vector3d baseline;
  ViewTriplet triplet;
  TwoViewInfoFromCameras(camera1, camera2, &triplet.info_one_two);
  TwoViewInfoFromCameras(camera1, camera3, &triplet.info_one_three);
  TwoViewInfoFromCameras(camera2, camera3, &triplet.info_two_three);
  EXPECT_TRUE(ComputeTripletBaselineRatios(triplet,
                                           feature1,
                                           feature2,
                                           feature3,
                                           &baseline));

  // Measure the error.
  const double baseline_12 =
      (camera2.GetPosition() - camera1.GetPosition()).norm();
  const double baseline_13 =
      (camera3.GetPosition() - camera1.GetPosition()).norm();
  const double baseline_23 =
      (camera3.GetPosition() - camera2.GetPosition()).norm();
  const Eigen::Vector3d gt_baseline(1.0,
                                    baseline_13 / baseline_12,
                                    baseline_23 / baseline_12);

  EXPECT_NEAR(gt_baseline[0] / baseline[0], 1.0, tolerance);
  EXPECT_NEAR(gt_baseline[1] / baseline[1], 1.0, tolerance);
  EXPECT_NEAR(gt_baseline[2] / baseline[2], 1.0, tolerance);
}

TEST(ComputeTripletBaselineRatios, NoNoiseSmallScene) {
  TestTripletBaselineComputation(0, 2, 1e-8);
}

TEST(ComputeTripletBaselineRatios, NoiseSmallScene) {
  TestTripletBaselineComputation(1, 2, 0.05);
}

TEST(ComputeTripletBaselineRatios, NoNoiseLargeScene) {
  TestTripletBaselineComputation(0, 20, 1e-8);
}

TEST(ComputeTripletBaselineRatios, NoiseLargeScene) {
  TestTripletBaselineComputation(1, 20, 0.1);
}

}  // namespace theia


#include <Eigen/Core>

#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/shape_inference.h"
#include "tensorflow/core/framework/register_types.h"

using namespace tensorflow;

REGISTER_OP("RegularInterp")
  .Attr("T: realnumbertype")
  .Attr("ndim: int >= 1")
  .Attr("check_sorted: bool = true")
  .Attr("bounds_error: bool = false")
  .Input("points: ndim * T")
  .Input("values: T")
  .Input("xi: T")
  .Output("zi: T")
  .Output("dz: T")
  .SetShapeFn([](shape_inference::InferenceContext* c) {
    int ndim;
    TF_RETURN_IF_ERROR(c->GetAttr("ndim", &ndim));

    shape_inference::ShapeHandle shape, values_shape, xi_shape, zi_shape, tmp;

    // Get the dimensions of each axis
    TF_RETURN_IF_ERROR(c->WithRank(c->input(0), 1, &shape));
    for (int i = 1; i < ndim; ++i) {
      TF_RETURN_IF_ERROR(c->WithRank(c->input(i), 1, &tmp));
      TF_RETURN_IF_ERROR(c->Concatenate(shape, tmp, &shape));
    }

    // Make sure that the first ndim axes of values have the right shape
    TF_RETURN_IF_ERROR(c->WithRankAtLeast(c->input(ndim), ndim, &values_shape));
    TF_RETURN_IF_ERROR(c->Subshape(values_shape, ndim, &zi_shape));
    TF_RETURN_IF_ERROR(c->Subshape(values_shape, 0, ndim, &values_shape));
    TF_RETURN_IF_ERROR(c->Merge(shape, values_shape, &values_shape));

    // Make sure that the last dimension of xi is ndim
    TF_RETURN_IF_ERROR(c->WithRankAtLeast(c->input(ndim+1), 1, &xi_shape));
    shape_inference::DimensionHandle dim = c->Dim(xi_shape, -1);
    TF_RETURN_IF_ERROR(c->WithValue(dim, ndim, &dim));

    // Compute the output shape
    TF_RETURN_IF_ERROR(c->Subshape(xi_shape, 0, -1, &tmp));
    TF_RETURN_IF_ERROR(c->Concatenate(tmp, zi_shape, &tmp));
    c->set_output(0, tmp);

    TF_RETURN_IF_ERROR(c->Concatenate(xi_shape, zi_shape, &tmp));
    c->set_output(1, tmp);

    return Status::OK();
  });

// adapted from https://academy.realm.io/posts/how-we-beat-cpp-stl-binary-search/
template <typename T>
inline int64 search_sorted (int64 N, const typename TTypes<T>::ConstFlat& x, const T& value) {
  int64 low = -1;
  int64 high = N;
  while (high - low > 1) {
    int64 probe = (low + high) / 2;
    T v = x(probe);
    if (v > value)
      high = probe;
    else
      low = probe;
  }
  return high;
}

template <typename T>
class RegularInterpOp : public OpKernel {
 public:
  explicit RegularInterpOp(OpKernelConstruction* context) : OpKernel(context) {
    OP_REQUIRES_OK(context, context->GetAttr("ndim", &ndim_));
    OP_REQUIRES_OK(context, context->GetAttr("check_sorted", &check_sorted_));
    OP_REQUIRES_OK(context, context->GetAttr("bounds_error", &bounds_error_));
  }

  void Compute(OpKernelContext* context) override {
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> ConstMatrix;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> Matrix;

    // Check the dimensions of the input values
    const Tensor& values_tensor = context->input(ndim_);
    int64 values_dims = values_tensor.dims();
    OP_REQUIRES(context, (values_dims >= ndim_),
                errors::InvalidArgument("'values' must be at least 'ndim'-dimensional"));

    // Compute the total number of grid points
    Eigen::Array<int64, Eigen::Dynamic, 1> grid_dims(ndim_);
    for (int64 i = 0; i < ndim_; ++i) grid_dims(i) = values_tensor.dim_size(i);
    int64 ngrid = grid_dims.prod();

    // Compute the total number of outputs
    int64 nout = 1;
    for (int64 i = ndim_; i < values_dims; ++i) nout *= values_tensor.dim_size(i);

    // Check the dimensions of the test points
    const Tensor& xi_tensor = context->input(ndim_+1);
    int64 xi_dims = xi_tensor.dims();
    OP_REQUIRES(context, (xi_dims >= 1),
                errors::InvalidArgument("'xi' must be at least 1-dimensional"));
    OP_REQUIRES(context, (xi_tensor.dim_size(xi_dims - 1) == ndim_),
                errors::InvalidArgument("The last dimension of 'xi' must be 'ndim'"));

    // Compute the total number of test points
    int64 ntest = 1;
    for (int64 i = 0; i < xi_dims-1; ++i) ntest *= xi_tensor.dim_size(i);

    // Cast the values and test points as a matrix with the right shape
    const auto values = ConstMatrix(values_tensor.template flat<T>().data(), ngrid, nout);
    const auto xi = ConstMatrix(xi_tensor.template flat<T>().data(), ntest, ndim_);

    // Allocate temporary arrays to store indices and weights
    Eigen::Matrix<int64, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> inds(ntest, ndim_);
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> numerator(ntest, ndim_);
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> denominator(ntest, ndim_);
    Eigen::Matrix<T, Eigen::Dynamic, 1> accumulator(ndim_);

    // Compute the output shapes
    auto values_shape = values_tensor.shape(),
         zi_shape = xi_tensor.shape(),
         dz_shape = xi_tensor.shape();
    values_shape.RemoveDimRange(0, ndim_);
    zi_shape.RemoveLastDims(1);
    zi_shape.AppendShape(values_shape);
    Tensor* zi_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, zi_shape, &zi_tensor));

    dz_shape.AppendShape(values_shape);
    Tensor* dz_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, dz_shape, &dz_tensor));

    auto zi = Matrix(zi_tensor->template flat<T>().data(), ntest, nout);
    auto dz = Matrix(dz_tensor->template flat<T>().data(), ntest, ndim_*nout);
    dz.setZero();

    // Loop over dimensions and compute the indices of each test point in each grid
    for (int dim = 0; dim < ndim_; ++dim) {
      // Check the grid definition
      const Tensor& points_tensor = context->input(dim);
      int64 N = points_tensor.NumElements();
      OP_REQUIRES(context, (points_tensor.dims() == 1),
                  errors::InvalidArgument("each tensor in 'points' must be exactly 1-dimensional"));
      OP_REQUIRES(context, (grid_dims(dim) == N),
                  errors::InvalidArgument("the first 'ndim' dimensions of 'values' must have the same shape as points"));
      const auto points = points_tensor.template flat<T>();
      if (check_sorted_) {
        for (int64 n = 0; n < N-1; ++n)
          OP_REQUIRES(context, (points(n+1) > points(n)), errors::InvalidArgument("each tensor in 'points' must be sorted"));
      }

      // Find where the point should be inserted into the grid
      for (int64 n = 0; n < ntest; ++n) {
        bool out_of_bounds = false;
        int64 ind = search_sorted(N, points, xi(n, dim)) - 1;
        if (ind < 0) {
          out_of_bounds = true;
          ind = 0;
        }
        if (ind > N-2) {
          out_of_bounds = true;
          ind = N-2;
        }
        if (bounds_error_) {
          OP_REQUIRES(context, (out_of_bounds == false), errors::InvalidArgument("target point out of bounds"));
        }
        inds(n, dim) = ind;
        numerator(n, dim) = xi(n, dim) - points(ind);
        denominator(n, dim) = points(ind+1) - points(ind);
      }
    }

    // Loop over test points and compute the interpolation for that point
    unsigned ncorner = pow(2, ndim_);
    for (int n = 0; n < ntest; ++n) {

      // Madness to find the coordinates of every corner
      zi.row(n).setZero();
      dz.row(n).setZero();
      for (unsigned corner = 0; corner < ncorner; ++corner) {
        int64 factor = 1;
        int64 ind = 0;
        T weight = T(1.0);
        for (int dim = ndim_-1; dim >= 0; --dim) {
          unsigned offset = (corner >> unsigned(dim)) & 1;
          ind += factor * (inds(n, dim) + offset);
          factor *= grid_dims(dim);
          if (offset == 1) {
            weight *= numerator(n, dim) / denominator(n, dim);
            accumulator(dim) = numerator(n, dim);
          } else {
            //T norm_dist = T(1.0) - numerator(n, dim) / denominator(n, dim);
            weight *= T(1.0) - numerator(n, dim) / denominator(n, dim);
            accumulator(dim) = (numerator(n, dim) - denominator(n, dim));
          }
        }

        if (std::abs(weight) > std::numeric_limits<T>::epsilon()) {
          zi.row(n).noalias() += weight * values.row(ind);
          for (int dim = 0; dim < ndim_; ++dim) {
            dz.block(n, dim * nout, 1, nout).noalias() += (weight / accumulator(dim)) * values.row(ind);
          }
        }
      }
    }
  }

 private:
  int ndim_;
  bool check_sorted_, bounds_error_;
};

#define REGISTER_KERNEL(type)                                               \
  REGISTER_KERNEL_BUILDER(                                                  \
      Name("RegularInterp").Device(DEVICE_CPU).TypeConstraint<type>("T"),   \
      RegularInterpOp<type>)

REGISTER_KERNEL(float);
REGISTER_KERNEL(double);

#undef REGISTER_KERNEL


//          Copyright(C) Rishabh Arora 2017.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// To compile, use boost with the flag -lboost_unit_test_framework.
// To run use ./a.out --log_level=all to see the entire error report.

#define BOOST_DYN_LINK
#define BOOST_TEST_MODULE  DATA_FRAME

#include <boost/test/unit_test.hpp>
#include "../include/boost/numeric/ublas/FINAL_VERSIONS/df.hpp"
using namespace boost::numeric::ublas;

BOOST_AUTO_TEST_CASE (df_column_constructors) {
	//  default constructor
	df_column c1;
	BOOST_CHECK(c1.size() == 0);

	// copy constructor with params: vector<T>
	vector <int> I(100);
	for(size_t i = 0; i < 100; i++) I(i) = i+2;
	df_column II(I);
	BOOST_CHECK(II.size() == 100);
	for(size_t i = 0; i < 100; i++) {
		BOOST_CHECK(I(i) == II.get<int>()(i));
	}

	// copy constructor with params: df_column
	df_column III(II);
	BOOST_CHECK(II.size() == III.size());
	for(size_t i = 0; i < 100; ++i) {
		BOOST_CHECK(II.get<int>()(i) == III.get<int>()(i));
	}

	df_column IV(II + III);
	BOOST_CHECK(IV.size() == II.size());
	for(size_t i = 0; i < 100; ++i) {
		BOOST_CHECK(II.get<int>()(i) + III.get<int>()(i) == IV.get<int>()(i));
	}
}

BOOST_AUTO_TEST_CASE (df_column_assignment) {
	vector < std::string > x(2);
	x(0) = "rishabh";
	x(1) = "arora";

	df_column X = x;
	BOOST_CHECK(X.size() == x.size());
	for(size_t i = 0; i < X.size(); ++i) {
		BOOST_CHECK(X.get<std::string>()(i) == x(i));
	}

	df_column Y = X;
	BOOST_CHECK(X.size() == Y.size());
	for(size_t i = 0; i < X.size(); ++i) {
		BOOST_CHECK(X.get<std::string>()(i) == Y.get<std::string>()(i));
	}
}

BOOST_AUTO_TEST_CASE (df_column_Equality_Check_Operators) {
	vector < double > a(3);
	a(0) = 1.1;
	a(1) = 1.2;
	a(2) = 1.32;
	vector < char > b(2);
	b(0) = 'a';
	b(1) = 'b';

	// should fail
	df_column A(a), B(b);
	//BOOST_CHECK(A == B);
	// should pass
	BOOST_CHECK(A != B);

	df_column C(A);
	// should pass
	BOOST_CHECK(A == C);

	// should fail
	//BOOST_CHECK(A != C);

	vector < char > d(2);
	d(0) = 'c';
	d(1) = 'd';
	df_column D(d);

	// should fail
	//BOOST_CHECK(B == D);
	// should pass
	BOOST_CHECK(B != D);
}

BOOST_AUTO_TEST_CASE (df_column_Unary_Operator) {
	// unary - operator
	vector < int > z(3);
	z(0) = 15;
	z(1) = 12;
	z(2) = 13;
	df_column Z(z);
	// move assignment :)
	df_column T = -Z;

	BOOST_CHECK(T.size() == Z.size());
	for(size_t i = 0; i < T.size(); ++i) {
		BOOST_CHECK(T.get<int>()(i) == -Z.get<int>()(i));
	}
}

BOOST_AUTO_TEST_CASE (df_column_Binary_Operators) {
	vector < int > z(3);
	z(0) = 15;
	z(1) = 12;
	z(2) = 13;
	df_column Z(z);
	df_column Y(2*Z);
	BOOST_CHECK(Z.size() == Y.size());
	for(size_t i = 0; i < Y.size(); ++i) {
		BOOST_CHECK(2 * Z.get<int>()(i) == Y.get<int>()(i));
	}

	Z *= 2;
	for(size_t i = 0; i < Y.size(); ++i) {
		BOOST_CHECK(Z.get<int>()(i) == Y.get<int>()(i));
	}

	df_column X = Z + 3;
	BOOST_CHECK(X.size() == Z.size());
	for(size_t i = 0; i < Z.size(); ++i) {
		BOOST_CHECK(X.get <int>()(i) == Z.get<int>()(i) + 3);
	}
	X += 3;
	for(size_t i = 0; i < Z.size(); ++i) {
		BOOST_CHECK(X.get <int>()(i) == Z.get<int>()(i) + 6);
	}
	X -= 3;
	for(size_t i = 0; i < Z.size(); ++i) {
		BOOST_CHECK(X.get <int>()(i) == Z.get<int>()(i) + 3);
	}

	df_column A = X + Z;
	BOOST_CHECK(A.size() == X.size() && A.size() == Z.size());
	for(size_t i = 0; i < Z.size(); ++i) {
		BOOST_CHECK(A.get<int>()(i) == X.get <int>()(i) + Z.get<int>()(i));
	}
	A -= X;
	for(size_t i = 0; i < Z.size(); ++i) {
		BOOST_CHECK(A.get<int>()(i) == Z.get<int>()(i));
	}
}

// No static_assert checking. Manual looking shows that results are perfect. :-)
BOOST_AUTO_TEST_CASE (df_column_Printing) {
	vector < int > z(3);
	z(0) = 15;
	z(1) = 12;
	z(2) = 13;
	df_column Z(z);
	df_column Y(2*Z);
	Z.print();
	Y.summary < int, double> ();
}

BOOST_AUTO_TEST_CASE (df_column_Statistical_Summaries) {

	// Mean
	vector<double> v(6);
    v(0) = 10, v(1) = 20, v(2) = 30, v(3) = 40, v(4) = 50, v(5) = 60.123777;
    df_column V(v);
    double mean = V.Mean<double, double>();
    double var = (10.0 + 20.0 + 30.0 + 40.0 + 50.0 + 60.123777) / 6.0;
    //std::cout << mean << ' ' << var << '\n';
    BOOST_CHECK(fabs(mean - var) < 1e-9);

	// Median
	double Median = V.Median<double, double>();
	BOOST_CHECK( fabs(Median - 35) < 1e-9);
	v.resize(7);
	v(6) = 70;
	V = v;
	Median = V.Median<double, double>();
	BOOST_CHECK( fabs(Median - 40) < 1e-9);

	// Max, Min
    vector < long long > r(10);
    for(int i = 0; i < 10; ++i) r(i) = 1e16 + rand() % 1000;
    long long mx = -1, mn = 1e18;
	for(int i = 0; i < 10; ++i) {
		if (r(i) > mx) mx = r(i);
		if (r(i) < mn) mn = r(i);
	}
	df_column R(r);
	long long MAX = R.Max<long long, long long>();
	long long MIN = R.Min<long long, long long>();
	BOOST_CHECK(mx == MAX);
	BOOST_CHECK(mn == MIN);
}

BOOST_AUTO_TEST_CASE (data_frame_Constructors) {
	// default constructor
	data_frame df1;
	BOOST_CHECK(df1.nrow() == 0 && df1.ncol() == 0);

	vector < std::string > names(2);
	names(0) = "col1";
	names(1) = "col2";

	vector < int > x(2), y(2);
	x(0) = 1;
	y(1) = 1;
	vector < df_column > cols(2);
	cols (0) = x;
	cols (1) = y;
	data_frame df2(names, cols);
	BOOST_CHECK(df2.ncol() == 2 && df2.nrow() == 2);

	for(size_t i = 0; i < df2.ncol(); ++i) {
		BOOST_CHECK(names(i) == df2.headers()(i));
	}

	for(size_t j = 0; j < df2.nrow(); ++j) {
		BOOST_CHECK(x(j) == df2["col1"].get<int>()(j));
	}
	for(size_t j = 0; j < df2.nrow(); ++j) {
		BOOST_CHECK(y(j) == df2["col2"].get<int>()(j));
	}
}

BOOST_AUTO_TEST_CASE (data_frame_Column_Accessors_Manipulators) {
	// Column Retrieval
	vector < std::string > names(3);
	names(0) = "x";
	names(1) = "y";
	names(2) = "z";

	vector < char > x(2);
	vector < bool > y(2);
	vector < std::string > z(2);
	x(0) = 'p';
	x(1) = 'q';
	y(0) = true;
	y(1) = false;
	z(0) = "Oh!";
	z(1) = "Yeah..";

	vector < df_column > cols(3);
	cols (0) = x;
	cols (1) = y;
	cols (2) = z;

	data_frame df(names, cols);
	BOOST_CHECK(df.ncol() == 3 && df.nrow() == 2);

	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(x(i) == df["x"].get<char>()(i));
		BOOST_CHECK(x(i) == df[0].get<char>()(i));
		BOOST_CHECK(x(i) == df.column<char>("x")(i));
		BOOST_CHECK(x(i) == df.column<char>(0)(i));
	}

	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(y(i) == df["y"].get<bool>()(i));
		BOOST_CHECK(y(i) == df[1].get<bool>()(i));
		BOOST_CHECK(y(i) == df.column<bool>("y")(i));
		BOOST_CHECK(y(i) == df.column<bool>(1)(i));
	}

	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(z(i) == df["z"].get<std::string>()(i));
		BOOST_CHECK(z(i) == df[2].get<std::string>()(i));
		BOOST_CHECK(z(i) == df.column<std::string>("z")(i));
		BOOST_CHECK(z(i) == df.column<std::string>(2)(i));
	}

	/// Column Accessors + Updating
	// via string
	df["x"] = y;
	BOOST_CHECK(df["x"].size() == df.nrow());
	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(y(i) == df["x"].get<bool>()(i));
		BOOST_CHECK(y(i) == df[0].get<bool>()(i));
		BOOST_CHECK(y(i) == df.column<bool>("x")(i));
		BOOST_CHECK(y(i) == df.column<bool>(0)(i));
	}
	// via index
	df[0] = x;  // df is back to original
	BOOST_CHECK(df["x"].size() == df.nrow());
	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(x(i) == df["x"].get<char>()(i));
		BOOST_CHECK(x(i) == df[0].get<char>()(i));
		BOOST_CHECK(x(i) == df.column<char>("x")(i));
		BOOST_CHECK(x(i) == df.column<char>(0)(i));
	}

	/// erasing columns
	// via index
	size_t ncol_earlier = df.ncol();
	df.erase_column(0);
	BOOST_CHECK(df.ncol() == ncol_earlier-1);
	// test for correct restructuring of data_frame after the erase
	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(y(i) == df["y"].get<bool>()(i));
		BOOST_CHECK(y(i) == df[0].get<bool>()(i));
		BOOST_CHECK(y(i) == df.column<bool>("y")(i));
		BOOST_CHECK(y(i) == df.column<bool>(0)(i));
	}

	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(z(i) == df["z"].get<std::string>()(i));
		BOOST_CHECK(z(i) == df[1].get<std::string>()(i));
		BOOST_CHECK(z(i) == df.column<std::string>("z")(i));
		BOOST_CHECK(z(i) == df.column<std::string>(1)(i));
	}
	// via string
	ncol_earlier = df.ncol();
	df.erase_column("y");
	BOOST_CHECK(df.ncol() == ncol_earlier-1);
 	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(z(i) == df["z"].get<std::string>()(i));
		BOOST_CHECK(z(i) == df[0].get<std::string>()(i));
		BOOST_CHECK(z(i) == df.column<std::string>("z")(i));
		BOOST_CHECK(z(i) == df.column<std::string>(0)(i));
	}

	/// Add columns via accessor
	// via string
	df.print();
	ncol_earlier = df.ncol();
	df["y"] = y;
	BOOST_CHECK(df.ncol() == ncol_earlier + 1);
	df.print();
 }

BOOST_AUTO_TEST_CASE (data_frame_Equality_Check_Operators) {
	// Column Retrieval
	vector < std::string > names(3);
	names(0) = "x";
	names(1) = "y";
	names(2) = "z";

	vector < char > x(2);
	vector < bool > y(2);
	vector < std::string > z(2);
	x(0) = 'p';
	x(1) = 'q';
	y(0) = true;
	y(1) = false;
	z(0) = "Oh!";
	z(1) = "Yeah..";

	vector < df_column > cols(3);
	cols (0) = x;
	cols (1) = y;
	cols (2) = z;

	data_frame df(names, cols);
	data_frame df2(df);
	// should pass
	BOOST_CHECK(df == df2);

	data_frame empty;
	// should pass
	BOOST_CHECK(df != empty);

	// should fail
	//BOOST_CHECK(df == empty);
}

BOOST_AUTO_TEST_CASE (data_frame_Unary_Operators) {
	vector < std::string > names(4);
	names(0) = "a", names(1) = "b", names(2) = "c", names(3) = "d";
	vector < int > x(2), y(2);
	x(0) = 1;
	y(1) = 1;
	vector < df_column > cols(4);
	cols (0) = x;
	cols (1) = y;
	cols (2) = x;
	cols (3) = y;
	data_frame df(names, cols);
	// move constructor :)
	data_frame negate_df(-df);
	BOOST_CHECK (negate_df.nrow() == df.nrow() && negate_df.ncol() == df.ncol());
	for(size_t i = 0; i < negate_df.ncol(); ++i) negate_df.colname(i) == names(i);
	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(x(i) == -negate_df[0].get<int>()(i));
		BOOST_CHECK(x(i) == -negate_df[2].get<int>()(i));
	}
	for(size_t i = 0; i < df.nrow(); ++i) {
		BOOST_CHECK(y(i) == -negate_df[1].get<int>()(i));
		BOOST_CHECK(y(i) == -negate_df[3].get<int>()(i));
	}
}

BOOST_AUTO_TEST_CASE (data_frame_Statistical_Summaries) {
	vector < int > x(2);
	vector < std::string > y(2);
	vector < double > z(2);
	x(0) = 10, x(1) = 11;
	y(0) = "rishabh", y(1) = "arora";
	z(0) = 1.2, z(1) = 2.0;
	vector < std::string > headers(3);
	headers(0) = "x", headers(1) = "y", headers(2) = "z";
	vector < df_column > A(3);
	A(0) = x, A(1) = y, A(2) = z;
	data_frame df(headers, A);
	// Already checked above that df_column_summaries work fine. and data_Frame summaries are extension of df_column_summaries for each column of the data_Frame.
	df.summary();
}

BOOST_AUTO_TEST_CASE (data_frame_Binary_Operators) {
	vector < std::string > headers(2);
	headers(0) = "a";
	headers(1) = "b";
	vector < double > a(2), b(2);
	a(0) = 1.1, a(1) = 2.1;
	b(0) = 1.2, b(1) = 2.3;
	vector < df_column > col(2);
	col(0) = a;
	col(1) = b;
	data_frame df1(headers, col);
	vector < df_column > col2(2);
	a = 2 * a;
	b = 2 * b;
	col2(0) = a;
	col2(1) = b;
	data_frame df2(headers, col2);

	data_frame df3 = df1 + df2;
	BOOST_CHECK(df3.nrow() == df1.nrow() && df3.nrow() == df2.nrow());
	BOOST_CHECK(df3.ncol() == df1.ncol() && df3.ncol() == df2.ncol());

	for(size_t i = 0; i < df3.nrow(); ++i) {
		BOOST_CHECK(df3[0].get<double>()(i) == df1[0].get<double>()(i) + df2[0].get<double>()(i));
	}
	for(size_t i = 0; i < df3.nrow(); ++i) {
		BOOST_CHECK(df3[1].get<double>()(i) == df1[1].get<double>()(i) + df2[1].get<double>()(i));
	}

	data_frame df4 = df3 - df2;
	BOOST_CHECK(df4.nrow() == df3.nrow() && df4.nrow() == df2.nrow());
	BOOST_CHECK(df4.ncol() == df3.ncol() && df4.ncol() == df2.ncol());

	for(size_t i = 0; i < df4.nrow(); ++i) {
		BOOST_CHECK(fabs(df4[0].get<double>()(i) - df1[0].get<double>()(i)) < 1e-9);
	}
	for(size_t i = 0; i < df3.nrow(); ++i) {
		BOOST_CHECK(fabs(df4[1].get<double>()(i) - df1[1].get<double>()(i)) < 1e-9);
	}

	data_frame df5 = df1 + 5;
	BOOST_CHECK(df5.nrow() == df1.nrow());
	BOOST_CHECK(df5.ncol() == df1.ncol());

	for(size_t i = 0; i < df5.nrow(); ++i) {
		BOOST_CHECK(fabs(df5[0].get<double>()(i) - df1[0].get<double>()(i) - 5) < 1e-9);
	}
	for(size_t i = 0; i < df5.nrow(); ++i) {
		BOOST_CHECK(fabs(df5[1].get<double>()(i) - df1[1].get<double>()(i)- 5) < 1e-9);
	}

	data_frame df6 = df1 - 10;
	BOOST_CHECK(df6.nrow() == df1.nrow());
	BOOST_CHECK(df6.ncol() == df1.ncol());

	for(size_t i = 0; i < df6.nrow(); ++i) {
		BOOST_CHECK(fabs(df6[0].get<double>()(i) - df1[0].get<double>()(i) + 10) < 1e-9);
	}
	for(size_t i = 0; i < df6.nrow(); ++i) {
		BOOST_CHECK(fabs(df6[1].get<double>()(i) - df1[1].get<double>()(i) + 10) < 1e-9);
	}

	data_frame df7 = 2 * df1;
	BOOST_CHECK(df7.nrow() == df1.nrow());
	BOOST_CHECK(df7.ncol() == df1.ncol());

	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 2 * df1[0].get<double>()(i)) < 1e-9);
	}
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 2 * df1[1].get<double>()(i)) < 1e-9);
	}

	df7 += df1;
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 3 * df1[0].get<double>()(i)) < 1e-9);
	}
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 3 * df1[1].get<double>()(i)) < 1e-9);
	}

	df7 -= df1;
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 2 * df1[0].get<double>()(i)) < 1e-9);
	}
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 2 * df1[1].get<double>()(i)) < 1e-9);
	}

	df7 += 5;
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 2 * df1[0].get<double>()(i) - 5) < 1e-9);
	}
	for(size_t i = 0; i < df6.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 2 * df1[1].get<double>()(i) - 5) < 1e-9);
	}

	df7 -= 10;
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 2 * df1[0].get<double>()(i) + 5) < 1e-9);
	}
	for(size_t i = 0; i < df6.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 2 * df1[1].get<double>()(i) + 5) < 1e-9);
	}

	df7 += 5;
	df7 *= 3;

	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[0].get<double>()(i) - 6 * df1[0].get<double>()(i)) < 1e-9);
	}
	for(size_t i = 0; i < df7.nrow(); ++i) {
		BOOST_CHECK(fabs(df7[1].get<double>()(i) - 6 * df1[1].get<double>()(i)) < 1e-9);
	}
}

BOOST_AUTO_TEST_CASE (data_frame_row_accessor) {
	vector < int > a(3);
	vector < std::string > b(3);
	vector < bool > c(3);
	vector < long long > d(3);
	vector < char > e(3);
	vector < std::string > headers(5);
	headers(0) = "a", headers(1) = "b", headers(2) = "c", headers(3) = "d", headers(4) = "e";
	a(0) = 1, a(1) = 2, a(2) = 3;
	b(0) = "R", b(1) = "RArora", b(2) = "Arora";
	c(0) = 0, c(1) = 1, c(2) = 0;
	d(0) = (long long)1e10, d(1) = (long long)1e10 + 100, d(2) = (long long)1e10 - 100;
	e(0) = 'a', e(1) = 'b', e(2) = 'c';
	vector < df_column > col(5);
	col(0) = a, col(1) = b, col(2) = c, col(3) = d, col(4) = e;
	data_frame df(headers, col);
	vector < boost::variant < COLUMN_DATA_TYPES > > row = df(0);
	BOOST_CHECK(row.size() == df.ncol());
	BOOST_CHECK(boost::get<int>(row(0)) == a(0));
	BOOST_CHECK(boost::get<std::string>(row(1)) == b(0));
	BOOST_CHECK(boost::get<bool>(row(2)) == c(0));
	BOOST_CHECK(boost::get<long long>(row(3)) == d(0));
	BOOST_CHECK(boost::get<char>(row(4)) == e(0));
}

BOOST_AUTO_TEST_CASE (data_frame_proxies) {
	vector < int > a(3);
	vector < std::string > b(3);
	vector < bool > c(3);
	vector < long long > d(3);
	vector < char > e(3);

	vector < std::string > headers(5);

	headers(0) = "a", headers(1) = "b", headers(2) = "c", headers(3) = "d", headers(4) = "e";
	a(0) = 1, a(1) = 2, a(2) = 3;
	b(0) = "R", b(1) = "RArora", b(2) = "Arora";
	c(0) = 0, c(1) = 1, c(2) = 0;
	d(0) = (long long)1e10, d(1) = (long long)1e10 + 100, d(2) = (long long)1e10 - 100;
	e(0) = 'a', e(1) = 'b', e(2) = 'c';

	vector < df_column > col(5);
	col(0) = a, col(1) = b, col(2) = c, col(3) = d, col(4) = e;
	data_frame df(headers, col);
	data_frame_range dfr1(&df, range(0, 3));
	BOOST_CHECK(dfr1.size() == 3);
	//dfr1.print();
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr1[0].get<int>()(i) == a(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr1[1].get<std::string>()(i) == b(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr1[2].get<bool>()(i) == c(i));
	}

	data_frame_slice dfs1(&df, slice(0, 2, 3));
	BOOST_CHECK(dfs1.size() == 3);
	//dfs1.print();
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs1[0].get<int>()(i) == a(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs1[1].get<bool>()(i) == c(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs1[2].get<char>()(i) == e(i));
	}

	indirect_array<> ia(3);
	ia[0] = 0, ia[1] = 1, ia[2] = 4;
	data_frame_indirect dfi1(&df, ia);
	// dfi1.print();
	BOOST_CHECK(dfs1.size() == 3);

	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi1[0].get<int>()(i) == a(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi1[1].get<std::string>()(i) == b(i));
	}
	for (int i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi1[2].get<char>()(i) == e(i));
	}

	data_frame_range dfr(&df, range(3, 4));
	dfr += 3;
	dfr.print();
	// now only column 3 is updated, rest of the data_frame remains the same.
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr[0].get<long long>()(i) == d(i) + 3);
	}

	dfr -= 3;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr[0].get<long long>()(i) == d(i));
	}

	dfr *= 5;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfr[0].get<long long>()(i) == 5 * d(i));
	}

	data_frame_slice dfs(&df, slice(0, 3, 2));
	dfs += 3;

	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs[0].get<int>()(i) == a(i) + 3);
		BOOST_CHECK(dfs[1].get<long long>()(i) == 5 * d(i) + 3);
	}

	dfs -= 3;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs[0].get<int>()(i) == a(i));
		BOOST_CHECK(dfs[1].get<long long>()(i) == 5 * d(i));
	}

	dfs *= 2;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfs[0].get<int>()(i) == 2 * a(i));
		BOOST_CHECK(dfs[1].get<long long>()(i) == 10 * d(i) );
	}

	indirect_array <> ia1(2);
	ia1[0] = 0, ia1[1] = 3;
	data_frame_indirect dfi(&df, ia1);
	dfi += 1;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi[0].get<int>()(i) == 2 * a(i) + 1);
		BOOST_CHECK(dfi[1].get<long long>()(i) == 10 * d(i) + 1);
	}
	dfi -= 1;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi[0].get<int>()(i) == 2 * a(i));
		BOOST_CHECK(dfi[1].get<long long>()(i) == 10 * d(i));
	}
	dfi *= 100;
	for(size_t i = 0; i < 3; ++i) {
		BOOST_CHECK(dfi[0].get<int>()(i) == 200 * a(i));
		BOOST_CHECK(dfi[1].get<long long>()(i) == 1000 * d(i));
	}
}


#define BOOST_TEST_MODULE random
#include <boost/test/included/unit_test.hpp>

#include "../src/random.hpp"

BOOST_AUTO_TEST_SUITE(test_uniform_real_generator_sute)
BOOST_AUTO_TEST_CASE(test_correct_rand) {
    UniformRealGenerator rnd;

    int N = 1000;
    double result;

    for (int i = 0; i < N; i++) {
        result = rnd();
        BOOST_CHECK_GE(result, 0.0);
        BOOST_CHECK_LE(result, 1.0);
    }

    rnd = UniformRealGenerator(0, 100.0, 200.0);
    for (int i = 0; i < N; i++) {
        result = rnd();
        BOOST_CHECK_GE(result, 100.0);
        BOOST_CHECK_LE(result, 200.0);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef POLYLINECOMPRESSOR_H_
#define POLYLINECOMPRESSOR_H_

#include "util/coordinate.hpp"

#include <algorithm>
#include <boost/assert.hpp>
#include <string>
#include <vector>

namespace osrm
{
namespace engine
{
namespace detail
{
std::string encode(std::vector<int> &numbers);
std::int32_t decode_polyline_integer(std::string::const_iterator &first,
                                     std::string::const_iterator last);
} // namespace detail
using CoordVectorForwardIter = std::vector<util::Coordinate>::const_iterator;
// Encodes geometry into polyline format.
// See: https://developers.google.com/maps/documentation/utilities/polylinealgorithm

template <unsigned POLYLINE_PRECISION = 100000>
std::string encodePolyline(CoordVectorForwardIter begin, CoordVectorForwardIter end)
{
    double coordinate_to_polyline = POLYLINE_PRECISION / COORDINATE_PRECISION;
    auto size = std::distance(begin, end);
    if (size == 0)
    {
        return {};
    }

    std::vector<int> delta_numbers;
    BOOST_ASSERT(size > 0);
    delta_numbers.reserve((size - 1) * 2);
    int current_lat = 0;
    int current_lon = 0;
    std::for_each(
        begin,
        end,
        [&delta_numbers, &current_lat, &current_lon, coordinate_to_polyline](
            const util::Coordinate loc) {
            const int lat_diff =
                std::round(static_cast<int>(loc.lat) * coordinate_to_polyline) - current_lat;
            const int lon_diff =
                std::round(static_cast<int>(loc.lon) * coordinate_to_polyline) - current_lon;
            delta_numbers.emplace_back(lat_diff);
            delta_numbers.emplace_back(lon_diff);
            current_lat += lat_diff;
            current_lon += lon_diff;
        });
    return detail::encode(delta_numbers);
}

// Decodes geometry from polyline format
// See: https://developers.google.com/maps/documentation/utilities/polylinealgorithm

template <unsigned POLYLINE_PRECISION = 100000>
std::vector<util::Coordinate> decodePolyline(const std::string &polyline)
{
    double polyline_to_coordinate = COORDINATE_PRECISION / POLYLINE_PRECISION;
    std::vector<util::Coordinate> coordinates;
    std::int32_t latitude = 0, longitude = 0;

    std::string::const_iterator first = polyline.begin();
    const std::string::const_iterator last = polyline.end();
    while (first != last)
    {
        const auto dlat = detail::decode_polyline_integer(first, last);
        const auto dlon = detail::decode_polyline_integer(first, last);

        latitude += dlat;
        longitude += dlon;

        coordinates.emplace_back(util::Coordinate{
            util::FixedLongitude{static_cast<std::int32_t>(longitude * polyline_to_coordinate)},
            util::FixedLatitude{static_cast<std::int32_t>(latitude * polyline_to_coordinate)}});
    }
    return coordinates;
}
} // namespace engine
} // namespace osrm

#endif /* POLYLINECOMPRESSOR_H_ */


#pragma once

#include <cmath>
#include <boost/optional.hpp>
#include <boost/functional/hash.hpp>
#include <QtCore/QPointF>
#include <protobuf/Point.pb.h>
#include <sstream>
#include <string>

namespace Geometry2d {
/**
Simple class to represent a point in 2d space. Uses floating point coordinates
*/
class Point {
public:
    const double& x() const { return _x; }
    const double& y() const { return _y; }
    double& x() { return _x; }
    double& y() { return _y; }

    /**
    sets the point to x,y
    @param x the x coordinate
    @param y the y coordinate
    */
    Point(double x = 0, double y = 0) : _x(x), _y(y) {}

    /**
     * Implicit constructor for creating a Point from a Packet::Point
     */
    Point(const Packet::Point& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a QPointF
     */
    Point(const QPointF& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a QPoint
     */
    Point(const QPoint& other) : Point(other.x(), other.y()) {}

    /**
     * Implicit constructor for creating a Point from a double*
     */
    Point(const double* other) : Point(other[0], other[1]) {}

    /**
     * to draw stuff and interface with QT
     */
    QPointF toQPointF() const { return QPointF(x(), y()); }

    operator Packet::Point() const {
        Packet::Point out;
        out.set_x(x());
        out.set_y(y());
        return out;
    }
    /**
     * does vector addition
     * adds the + operator, shorthand
     */
    Point operator+(Point other) const {
        return Point(x() + other.x(), y() + other.y());
    }

    /**
     * see operator+
     * does vector division, note the operator
     */
    Point operator/(Point other) const {
        return Point(x() / other.x(), y() / other.y());
    }

    /**
     * @returns (x*x,y*y)
     */
    Point operator*(Point other) const {
        return Point(x() * other.x(), y() * other.y());
    }

    /**
     * see operator+
     * does vector subtraction, note the operator
     * without parameter, it is the negative
     */
    Point operator-(Point other) const {
        return Point(x() - other.x(), y() - other.y());
    }

    /**
     * multiplies the point by a -1 vector
     */
    Point operator-() const { return Point(-x(), -y()); }

    /**
     * see operator+
     * this modifies the value instead of returning a new value
     */
    Point& operator+=(Point other) {
        x() += other.x();
        y() += other.y();

        return *this;
    }

    /**
     * see operator-
     * this modifies the value instead of returning a new value
     */
    Point& operator-=(Point other) {
        x() -= other.x();
        y() -= other.y();

        return *this;
    }

    /**
     * see operator*
     * this modifies the value instead of returning a new value
     */
    Point& operator*=(double s) {
        x() *= s;
        y() *= s;

        return *this;
    }

    /**
     * see operator/
     * this modifies the value instead of returning a new value
     */
    Point& operator/=(double s) {
        x() /= s;
        y() /= s;

        return *this;
    }

    /**
     * adds the / operator for vectors
     *  scalar division
     */
    Point operator/(double s) const { return Point(x() / s, y() / s); }
    /**
     * adds the * operator for vectors
     * scalar multiplication
     */
    Point operator*(double s) const { return Point(x() * s, y() * s); }

    /**
     * compares two points to see if both x and y are the same
     * adds the == operator
     */
    bool operator==(Point other) const {
        return x() == other.x() && y() == other.y();
    }

    /**
     * this is the negation of operator operator !=
     */
    bool operator!=(Point other) const {
        return x() != other.x() || y() != other.y();
    }

    const double& operator[](int i) const {
        if (0 == i) {
            return _x;
        } else if (1 == i) {
            return _y;
        } else {
            throw std::out_of_range("Out of range index for Geometry2d::Point");
        }
    }

    double& operator[](int i) {
        return const_cast<double&>((static_cast<const Point*>(this))->operator[](i));
    }

    /**
     * Hash function for Geometry2d::Point
     */
    static size_t hash(Point pt) {
        size_t seed = 0;
        boost::hash_combine(seed, pt.x());
        boost::hash_combine(seed, pt.y());
        return seed;
    }

    /**
    computes the dot product of this point and another.
    behaves as if the points were 2d vectors
    @param p the second point
    @return the dot product of the two
    */
    double dot(Point p) const { return x() * p.x() + y() * p.y(); }

    /**
    computes the magnitude of the point, as if it were a vector
    @return the magnitude of the point
    */
    double mag() const { return sqrtf(x() * x() + y() * y()); }

    /**
    computes magnitude squared
    this is faster than mag()
    @return the magnitude squared
    */
    double magsq() const { return x() * x() + y() * y(); }

    /**
     * @brief Restricts the point to a given magnitude
     * @param max The magnitude to restrict the vector
     */
    Point& clamp(double max) {
        double ratio = mag() / max;
        if (ratio > 1) {
            x() /= ratio;
            y() /= ratio;
        }
        return *this;
    }

    /**
    rotates the point around another point by specified angle in the CCW
    direction
    @param origin the point to rotate around
    @param angle the angle in radians
    */
    Point& rotate(const Point& origin, double angle) {
        *this -= origin;
        rotate(angle);
        *this += origin;
        return *this;
    }

    /**
    * rotates the point around the origin
    */
    Point& rotate(double angle) {
        double newX = x() * cos(angle) - y() * sin(angle);
        double newY = y() * cos(angle) + x() * sin(angle);
        x() = newX;
        y() = newY;
        return *this;
    }

    /**
     * Like rotate(), but returns a new point instead of changing *this
     */
    Point rotated(double angle) const {
        double newX = x() * cos(angle) - y() * sin(angle);
        double newY = y() * cos(angle) + x() * sin(angle);
        return Point(newX, newY);
    }

    /**
     * Returns a new Point rotated around the origin
     */
    Point rotated(const Point& origin, double angle) const {
        return rotated(*this, origin, angle);
    }

    /**
    * static function to use rotate
    */
    static Point rotated(const Point& pt, const Point& origin, double angle) {
        Point newPt = pt;
        newPt.rotate(origin, angle);
        return newPt;
    }

    /**
    computes the distance from the current point to another
    @param other the point to find the distance to
    @return the distance between the points
    */
    double distTo(const Point& other) const {
        Point delta = other - *this;
        return delta.mag();
    }

    /**
    * Returns a vector with the same direction as this vector but with magnitude
    * given,
    * unless this vector is zero.
    * If the vector is (0,0), Point(0,0) is returned
    */
    Point normalized(double magnitude = 1.0) const {
        double m = mag();
        if (m == 0) {
            return Point(0, 0);
        }

        return Point(magnitude * x() / m, magnitude * y() / m);
    }

    /// Alias for normalized() - matches Eigen's syntax
    Point norm() const { return normalized(); }

    /**
    * Returns true if this point is within the given distance (threshold) of
    * (pt)
    */
    bool nearPoint(const Point& other, double threshold) const {
        return (*this - other).magsq() <= (threshold * threshold);
    }

    /**
    * Returns the angle of this point in radians CCW from +X.
    */
    double angle() const { return atan2(y(), x()); }

    /**
    * Returns a unit vector in the given direction (in radians)
    */
    static Point direction(double theta) {
        return Point(cos(theta), sin(theta));
    }

    /** returns the perpendicular to the point, Clockwise */
    Point perpCW() const { return Point(y(), -x()); }

    /** returns the perpendicular to the point, Counter Clockwise */
    Point perpCCW() const { return Point(-y(), x()); }

    /** saturates the magnitude of a vector */
    static Geometry2d::Point saturate(Geometry2d::Point value, double max) {
        double mag = value.mag();
        if (mag > fabs(max)) {
            return value.normalized() * fabs(max);
        }
        return value;
    }

    double angleTo(const Point& other) const { return (other - *this).angle(); }

    double cross(const Point& other) const {
        return x() * other.y() - y() * other.x();
    }

    /** returns the angle between the two normalized points (radians) */
    double angleBetween(const Point& other) const {
        return acos(normalized().dot(other.normalized()));
    }

    bool nearlyEquals(Point other) const;

    std::string toString() const {
        std::stringstream str;
        str << "Point(" << x() << ", " << y() << ")";
        return str.str();
    }

    friend std::ostream& operator<<(std::ostream& stream, const Point& point) {
        stream << point.toString();
        return stream;
    }

private:
    double _x, _y;
};  // \class Point

// global operations

/**
 * adds the * operator for vectors
 * scalar multiplication
 */
inline Point operator*(const double& s, const Point& pt) {
    return Point(pt.x() * s, pt.y() * s);
}
}


#include "ros/ros.h"
// 话题同步处理
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
// opencv接口
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include <opencv2/calib3d/calib3d.hpp>
//ros消息类型头文件
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"
//系统指令头文件
#include <boost/thread/thread.hpp>
#include <boost/foreach.hpp>
// pcl点云库
#include <pcl/io/pcd_io.h>
#include "pcl_ros/point_cloud.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
//内部时钟
#include <chrono>
#include "sensor_msgs/Imu.h"
#include "eigen3/Eigen/Core"
#include <sophus/se3.hpp>
#include <iostream>
//g2o
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;

sensor_msgs::Image image_;
cv::Mat cvColorImgMat;
cv::Mat cvColorImgMat2;
pcl::PCLPointCloud2 pcl_pc2;
pcl::PCLPointCloud2 pcl_pc2l;
uint8_t flag=0;
long int cnt = 0;
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};
/// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

  virtual void computeError() override {
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
    _error = _measurement - pose->estimate() * _point;
  }

  virtual void linearizeOplus() override {
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  bool read(istream &in) {}

  bool write(ostream &out) const {}

protected:
  Eigen::Vector3d _point;
};
void pose_estimation_3d3d(const vector<Point3f> &pts1,
                          const vector<Point3f> &pts2,
                          Mat &R, Mat &t) {
  Point3f p1, p2;     // center of mass
  int N = pts1.size();
  for (int i = 0; i < N; i++) {
    p1 += pts1[i];
    p2 += pts2[i];
  }
  p1 = Point3f(Vec3f(p1) / N);
  p2 = Point3f(Vec3f(p2) / N);
  vector<Point3f> q1(N), q2(N); // remove the center
  for (int i = 0; i < N; i++) {
    q1[i] = pts1[i] - p1;
    q2[i] = pts2[i] - p2;
  }

  // compute q1*q2^T
  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < N; i++) {
    W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
  }
  cout << "W=" << W << endl;

  // SVD on W
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();

  cout << "U=" << U << endl;
  cout << "V=" << V << endl;

  Eigen::Matrix3d R_ = U * (V.transpose());
  if (R_.determinant() < 0) {
    R_ = -R_;
  }
  Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

  // convert to cv::Mat
  R = (Mat_<double>(3, 3) <<
    R_(0, 0), R_(0, 1), R_(0, 2),
    R_(1, 0), R_(1, 1), R_(1, 2),
    R_(2, 0), R_(2, 1), R_(2, 2)
  );
  t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

void bundleAdjustmentGaussNewton(const VecVector3d &points_3d,const VecVector2d &points_2d,const cv::Mat &K,Sophus::SE3d &pose) {
    typedef Eigen::Matrix<double,6,1> Vector6d;
    const int iterations = 10;
    double cost = 0,lastCost = 0;
    double fx = K.at<double>(0,0);
    double fy = K.at<double>(1,1);
    double cx = K.at<double>(0,2);
    double cy = K.at<double>(1,2);
    for(int iter = 0;iter < iterations; iter++){
        Eigen::Matrix<double,6,6> H = Eigen::Matrix<double,6,6>::Zero();
        Vector6d b = Vector6d::Zero();
        cost = 0;
        //compute cost
        for(int i = 0;i < points_3d.size(); i++){
            Eigen::Vector3d pc = pose * points_3d[i];
            double inv_z = 1.0 / pc[2];
            double inv_z2 = inv_z * inv_z;
            Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx,fy * pc[1] / pc[2] + cy);
            Eigen::Vector2d e = points_2d[i] - proj;
            cost+=e.squaredNorm();

            Eigen::Matrix<double,2,6> J;
            J << -fx * inv_z,
            0,
            fx * pc[0] * inv_z2,
            fx * pc[0] * pc[1] * inv_z2,
            -fx - fx * pc[0] * pc[0] * inv_z2,
            fx * pc[1] * inv_z,
            0,
            -fy * inv_z,
            fy * pc[1] * inv_z2,
            fy + fy * pc[1] * pc[1] * inv_z2,
            -fy * pc[0] * pc[1] * inv_z2,
            -fy * pc[0] * inv_z;

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }
        Vector6d dx;
        dx = H.ldlt().solve(b);
        if(isnan(dx[0])){
            std::cout << "result is nan!" << std::endl;
            break;
        }

        if(iter > 0 && cost >= lastCost){
            // cost increase, update is not good
            std::cout << "cost: " << cost << ", last cost: " << lastCost << std::endl;
            break;
        }

        //update estimation
        pose = Sophus::SE3d::exp(dx) * pose;
        lastCost = cost;
        std::cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << std::endl;
        if(dx.norm() < 1e-6){
            break;
        }
    }
    std::cout << "pose by g-n \n" << pose.matrix() <<std::endl;
}

void callback(const sensor_msgs::ImuConstPtr& imu,const sensor_msgs::PointCloud2ConstPtr& point){
    std::cout <<"this is quternion"   << imu-> orientation.x << std::endl;
    cv_bridge::CvImagePtr cvImagePtr;
    pcl_conversions::toPCL(*point,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
    ROS_INFO("Cloud:width = %d,height = %d",point->width,point->height);
    try{
        pcl::toROSMsg(*point,image_);
    }catch(std::runtime_error e){
        ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what());
    }
    try{
        cvImagePtr = cv_bridge::toCvCopy(image_,sensor_msgs::image_encodings::BGR8);
    }catch(cv_bridge::Exception e){
        ROS_ERROR_STREAM("Cv_bridge Exception:" << e.what());
        return;
    }
    cvColorImgMat = cvImagePtr->image;

    if(flag==0){//如果是第一次进入，则和第一帧解算相同
        // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud_last);
        cvColorImgMat2 = cvImagePtr->image;
        flag =2;
    }else{
        // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(pcl_pc2l,*temp_cloud_last);
    }

    std::vector<cv::KeyPoint> keypoints,keypoints2;
    cv::Mat descriptors,descriptors2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    detector->detect(cvColorImgMat,keypoints);
    detector->detect(cvColorImgMat2,keypoints2);
    descriptor->compute(cvColorImgMat,keypoints,descriptors);
    descriptor->compute(cvColorImgMat2,keypoints2,descriptors2);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    ROS_INFO("extract ORB cost = %f seconds",time_used.count());

    // cvColorImgMat2 = cvColorImgMat;
    // cv::Mat outimg,outimg2;
    // cv::drawKeypoints(cvColorImgMat,keypoints,outimg,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT);
    // cv::drawKeypoints(cvColorImgMat2,keypoints2,outimg2,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT);
    // cv::imshow("colorview",outimg2);
    // cv::imshow("ORB feature",outimg);

    //match
    std::vector<cv::DMatch> matches;
    t1 = std::chrono::steady_clock::now();
    try
    {
        matcher->match(descriptors,descriptors2,matches);
    }
    catch(const cv::Exception& e)
    {
        ROS_INFO(e.what());
    }

    t2 = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    ROS_INFO("match ORB cost %f seconds",time_used.count());

    //choise better match
    auto min_max = std::minmax_element(matches.begin(),matches.end(),
        [](const cv::DMatch &m1,const cv::DMatch &m2){return m1.distance < m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;
    std::vector<cv::DMatch> good_matches;
    for (int i = 0; i < descriptors.rows; i++){
        if(matches[i].distance <= std::max(2 * min_dist,30.0))
            good_matches.push_back(matches[i]);
    }

    //获取3D点
    cv::Point3f d;
    std::vector<cv::Point3f> pts_3d,pts_3d_last;
    std::vector<cv::Point2f> pts_2d,pts_2d_last;
    BOOST_FOREACH(cv::DMatch m,good_matches){
        d.x = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].x;
        d.y = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].y;
        d.z = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].z;
        if(isnan(d.x)||isnan(d.y)||isnan(d.z))
            continue;
        pts_3d.push_back(d);
        pts_2d.push_back(keypoints[m.queryIdx].pt);
        // ROS_INFO("%f %f %f",d.x,d.y,d.z);
        // ROS_INFO("%d %d",int(keypoints[m.queryIdx].pt.x),int(keypoints[m.queryIdx].pt.y));
    }
    std::cout << "3d-2s pairs: " << pts_3d.size() << std::endl;
    cv::Mat K = (cv::Mat_<double>(3, 3) << 525.0, 0, 319.5, 0, 525.0, 239.5, 0, 0, 1);

    //Opencv 解位姿
    // t1 = std::chrono::steady_clock::now();
    // cv::Mat r, t;
    // cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t,false,CV_ITERATIVE); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    // cv::Mat R;
    // cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    // t2 = std::chrono::steady_clock::now();
    // time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    // std::cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << std::endl;

    // std::cout << "R=" << std::endl << R << std::endl;
    // std::cout << "t=" << std::endl << t << std::endl;

    //gaussNewton 解位姿
    VecVector3d pts_3d_eigen;
    VecVector2d pts_2d_eigen;
    for (size_t i = 0; i < pts_3d.size(); ++i) {
        pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
        pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
    }
    std::cout << "calling bundle adjustment by gauss newton " << std::endl;
    Sophus::SE3d pose_gn;
    t1 = std::chrono::steady_clock::now();
    bundleAdjustmentGaussNewton(pts_3d_eigen,pts_2d_eigen,K,pose_gn);
    t2 = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << std::endl;

    //保存上一帧的数据
    pcl_pc2l = pcl_pc2;
    cvColorImgMat2 = cvColorImgMat;

    //draw the answer
    cv::Mat img_match;
    cv::Mat img_goodmatch;
    cv::drawMatches(cvColorImgMat,keypoints,cvColorImgMat2,keypoints2,good_matches,img_goodmatch);
    cv::imshow("good matches",img_goodmatch);
    // cv::imshow("grayview",cvGrayImgMat);
    cv::waitKey(5);
}
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"grayView");
    ros::NodeHandle nh_;
    message_filters::Subscriber<sensor_msgs::Imu> sub(nh_,"/imu_data",1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> sub2(nh_,"/camera/depth_registered/points",1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu,sensor_msgs::PointCloud2> MySync;
    message_filters::Synchronizer<MySync> sync(MySync(10),sub,sub2);
    sync.registerCallback(boost::bind(&callback,_1,_2));
    // cv::namedWindow("colorview",cv::WINDOW_NORMAL);
    // cv::moveWindow("colorview",100,100);
    // cv::namedWindow("grayview",cv::WINDOW_NORMAL);
    // cv::moveWindow("grayview",600,100);
    ros::spin();
    return 0;
}