#include "orb_algos.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "xcam_log.h"

#define EPS 1e-10

#include <time.h>
double begin_tick;
double end_tick;

unsigned long getTime()
{
    struct timespec ts;
    clock_gettime(1, &ts);
    return (ts.tv_sec * 1000 + ts.tv_nsec / 1000000);
}

void work_begin()
{
    begin_tick = getTime();
}

void work_end(const char* module_name)
{
    end_tick = getTime() - begin_tick;
    LOGE_AORB("[%s] TIME = %lf ms \n", module_name, end_tick);
}

ORBList* initList(U32 bytes) {
    ORBList* list = (ORBList*)malloc(sizeof(ORBList));
    list->bytes = bytes;
    list->start = NULL;
    list->end = NULL;
    list->length = 0;
    return list;
}

void freeList(ORBList* list) {
    if (list != NULL) {
        for(Node* item = list->start; item != NULL; item = item->next) {
            if (item->data != NULL) {
                free(item->data);
                item->data = NULL;
            }
        }
        free(list);
        list = NULL;
    }
}

int push(ORBList* list, void* data) {

    Node* node = (Node*)malloc(sizeof(Node));
    node->data = malloc(list->bytes);
    node->next = NULL;
    memcpy(node->data, data, list->bytes);
    if (list->start == NULL) {
        list->start = node;
        list->end = node;
    } else {
        list->end->next = node;
        list->end = node;
    }

    list->length++;
    return list->length;
}

void init_matchpoints(orb_matched_point_t* point, U32 row1, U32 col1, U32 row2, U32 col2, U32 score) {
    if (point == NULL) {
        return;
    }

    point->col1 = col1;
    point->row1 = row1;
    point->col2 = col2;
    point->row2 = row2;
    point->score = score;
}

ORBList* get_roi_points_list (rk_aiq_orb_algo_stat_t* keypoints, orb_rect_t roi)
{
    ORBList* roi_points_list = initList(sizeof(rk_aiq_orb_featue_point));

    for (unsigned int i = 0; i < keypoints->num_points; i++) {
        rk_aiq_orb_featue_point* point = keypoints->points + i;
        if (point->x < roi.left ||
                point->x > roi.right ||
                point->y < roi.top ||
                point->y > roi.bottom) {
            continue;
        }

        push(roi_points_list, point);
    }

#if 1
    FILE* fp = fopen("/tmp/points0.txt", "wb");
    for (unsigned int i = 0; i < keypoints->num_points; i++) {
        rk_aiq_orb_featue_point* point = keypoints->points + i;
        fprintf(fp, "%d, %d\n", point->x, point->y);
    }
    fflush(fp);
    fclose(fp);
#endif
    return roi_points_list;
}

ORBList* matching(ORBList* roi_points_list, rk_aiq_orb_algo_stat_t* keypoints2, orb_rect_t roi) {
    orb_matched_point_t keypoint = {0};
    ORBList* matched_list = initList(sizeof(orb_matched_point_t));

    for(Node* point = roi_points_list->start; point != NULL; point = point->next) {
        rk_aiq_orb_featue_point* descriptor1 = (rk_aiq_orb_featue_point*)point->data;
        keypoint.score = 0;

        for (unsigned int j = 0; j < keypoints2->num_points; j++) {
            rk_aiq_orb_featue_point* descriptor2 = keypoints2->points + j;

            unsigned int success_num = 120;
            for(int k = 0; k < DESCRIPTOR_SIZE; k++) {
                U8 xor_val = descriptor1->brief[k] ^ descriptor2->brief[k];
                while (xor_val) {
                    if (xor_val & 1) {
                        success_num--;
                    }
                    xor_val = xor_val >> 1;
                }
            }

            if(success_num > 110) { //120
                if (success_num > keypoint.score) {
                    init_matchpoints(&keypoint,
                                     descriptor1->y, descriptor1->x,
                                     descriptor2->y, descriptor2->x,
                                     success_num);
                }

                //orb_matched_point_t* f = (orb_matched_point_t*)(matched_list->start);
                //LOGE_AORB("found match: (%d-%d)-(%d-%d)",
                //    f->col1, f->row1, f->col2, f->row2);
            }
        }

        if (keypoint.score > 0) {
            push(matched_list, &keypoint);
        }

        if (matched_list->length > 30) {
            break;
        }
    }

#if 1
    FILE* fp = fopen("/tmp/points1.txt", "wb");
    for (unsigned int i = 0; i < keypoints2->num_points; i++) {
        rk_aiq_orb_featue_point* point = keypoints2->points + i;
        fprintf(fp, "%d, %d\n", point->x, point->y);
    }
    fflush(fp);
    fclose(fp);
#endif

    return matched_list;
}

void gaussian_elimination(double *input, int n)
{
    double * A = input;
    int i = 0;
    int j = 0;
    //m = 8 rows, n = 9 cols
    int m = n - 1;
    while (i < m && j < n)
    {
        // Find pivot in column j, starting in row i:
        int maxi = i;
        for(int k = i + 1; k < m; k++)
        {
            if(fabs(A[k * n + j]) > fabs(A[maxi * n + j]))
            {
                maxi = k;
            }
        }
        if (A[maxi * n + j] != 0)
        {
            //swap rows i and maxi, but do not change the value of i
            if(i != maxi)
                for(int k = 0; k < n; k++)
                {
                    float aux = A[i * n + k];
                    A[i * n + k] = A[maxi * n + k];
                    A[maxi * n + k] = aux;
                }
            //Now A[i,j] will contain the old value of A[maxi,j].
            //divide each entry in row i by A[i,j]
            float A_ij = A[i * n + j];
            for(int k = 0; k < n; k++)
            {
                A[i * n + k] /= A_ij;
            }
            //Now A[i,j] will have the value 1.
            for(int u = i + 1; u < m; u++)
            {
                //subtract A[u,j] * row i from row u
                float A_uj = A[u * n + j];
                for(int k = 0; k < n; k++)
                {
                    A[u * n + k] -= A_uj * A[i * n + k];
                }
                //Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
            }
            i++;
        }
        j++;
    }

    //back substitution
    for(int i = m - 2; i >= 0; i--)
    {
        for(int j = i + 1; j < n - 1; j++)
        {
            A[i * n + m] -= A[i * n + j] * A[j * n + m];
        }
    }
}

int get_trusted_four_points(ORBList* matched_keypoints, point_t src[4], point_t dst[4])
{
    point_t center = {0};

    if (matched_keypoints->length < 4) {
        return -1;
    }

    for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
        orb_matched_point_t* keypoints = (orb_matched_point_t*)point->data;
        center.col += keypoints->col1;
        center.row += keypoints->row1;
    }

    center.col = center.col / matched_keypoints->length;
    center.row = center.row / matched_keypoints->length;

    int subx, suby, dist;
    int max_dist_lt = 0;
    int max_dist_rt = 0;
    int max_dist_lb = 0;
    int max_dist_rb = 0;
    bool found_lt, found_rt, found_lb, found_rb;

    found_lt = found_rt = found_lb = found_rb = false;
    for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
        orb_matched_point_t* keypoints = (orb_matched_point_t*)point->data;
        if (keypoints->col1 < center.col &&
                keypoints->row1 < center.row) {
            subx = keypoints->col1 - center.col;
            suby = keypoints->row1 - center.row;
            dist = subx * subx + suby * suby;
            if (dist > max_dist_lt) {
                found_lt = true;
                max_dist_lt = dist;
                src[0].col = keypoints->col1;
                src[0].row = keypoints->row1;
                dst[0].col = keypoints->col2;
                dst[0].row = keypoints->row2;
            }
        }

        if (keypoints->col1 > center.col &&
                keypoints->row1 < center.row) {
            subx = keypoints->col1 - center.col;
            suby = keypoints->row1 - center.row;
            dist = subx * subx + suby * suby;
            if (dist > max_dist_rt) {
                found_rt = true;
                max_dist_rt = dist;
                src[1].col = keypoints->col1;
                src[1].row = keypoints->row1;
                dst[1].col = keypoints->col2;
                dst[1].row = keypoints->row2;
            }
        }

        if (keypoints->col1 < center.col &&
                keypoints->row1 > center.row) {
            subx = keypoints->col1 - center.col;
            suby = keypoints->row1 - center.row;
            dist = subx * subx + suby * suby;
            if (dist > max_dist_lb) {
                found_lb = true;
                max_dist_lb = dist;
                src[2].col = keypoints->col1;
                src[2].row = keypoints->row1;
                dst[2].col = keypoints->col2;
                dst[2].row = keypoints->row2;
            }
        }

        if (keypoints->col1 > center.col &&
                keypoints->row1 > center.row) {
            subx = keypoints->col1 - center.col;
            suby = keypoints->row1 - center.row;
            dist = subx * subx + suby * suby;
            if (dist > max_dist_rb) {
                found_rb = true;
                max_dist_rb = dist;
                src[3].col = keypoints->col1;
                src[3].row = keypoints->row1;
                dst[3].col = keypoints->col2;
                dst[3].row = keypoints->row2;
            }
        }
    }

    if (!found_lt || !found_rt || !found_lb || !found_rb) {
        return -1;
    }

    return 0;
}

int find_homography_by_four_points(ORBList* matched_keypoints, double homography[9])
{
    int ret = 0;
    point_t src[4] = {0};
    point_t dst[4] = {0};

    ret = get_trusted_four_points(matched_keypoints, src, dst);
    if (ret < 0) {
        return -1;
    }

    // create the equation system to be solved
    // src and dst must implement [] operator for point access
    //
    // from: Multiple View Geometry in Computer Vision 2ed
    //       Hartley R. and Zisserman A.
    //
    // x' = xH
    // where H is the homography: a 3 by 3 matrix
    // that transformed to inhomogeneous coordinates for each point
    // gives the following equations for each point:
    //
    // x' * (h31*x + h32*y + h33) = h11*x + h12*y + h13
    // y' * (h31*x + h32*y + h33) = h21*x + h22*y + h23
    //
    // as the homography is scale independent we can let h33 be 1 (indeed any of the terms)
    // so for 4 points we have 8 equations for 8 terms to solve: h11 - h32
    // after ordering the terms it gives the following matrix
    // that can be solved with gaussian elimination:

    double P[8][9] =
    {
        {-src[0].col, -src[0].row, -1,   0,   0,  0, src[0].col * dst[0].col, src[0].row * dst[0].col, -dst[0].col }, // h11
        {  0,   0,  0, -src[0].col, -src[0].row, -1, src[0].col * dst[0].row, src[0].row * dst[0].row, -dst[0].row }, // h12

        {-src[1].col, -src[1].row, -1,   0,   0,  0, src[1].col * dst[1].col, src[1].row * dst[1].col, -dst[1].col }, // h13
        {  0,   0,  0, -src[1].col, -src[1].row, -1, src[1].col * dst[1].row, src[1].row * dst[1].row, -dst[1].row }, // h21

        {-src[2].col, -src[2].row, -1,   0,   0,  0, src[2].col * dst[2].col, src[2].row * dst[2].col, -dst[2].col }, // h22
        {  0,   0,  0, -src[2].col, -src[2].row, -1, src[2].col * dst[2].row, src[2].row * dst[2].row, -dst[2].row }, // h23

        {-src[3].col, -src[3].row, -1,   0,   0,  0, src[3].col * dst[3].col, src[3].row * dst[3].col, -dst[3].col }, // h31
        {  0,   0,  0, -src[3].col, -src[3].row, -1, src[3].col * dst[3].row, src[3].row * dst[3].row, -dst[3].row }, // h32
    };

    gaussian_elimination((&P[0][0]), 9);

    // gaussian elimination gives the results of the equation system
    // in the last column of the original matrix.
    // opengl needs the transposed 4x4 matrix:
    double aux_H[] = { P[0][8], P[1][8], P[2][8], // h11  h21 0 h31
                       P[3][8], P[4][8], P[5][8],  // h12  h22 0 h32
                       P[6][8], P[7][8], 1
                     };       // h13  h23 0 h33

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            homography[i * 3 + j] = aux_H[i * 3 + j];
        }
    }

    return 0;
}

void get_affine_matrix(double m[3][5], int dim, double affineM[9]) {
    int index = 0;
    char res[1024] = {0};

    strcat(res, "\n");
    for(int j = 0; j < dim; j++) {
        char str1[128];
        sprintf(str1, "x%d' = ", j);
        strcat(res, str1);
        for(int i = 0; i < dim; i++) {
            char str2[128];
            sprintf(str2, "x%d * %f + ", i, m[i][j + dim + 1]);
            strcat(res, str2);

            affineM[index] = m[i][j + dim + 1];
            index++;
        }
        char str3[128];
        sprintf(str3, "%f\n", m[dim][j + dim + 1]);
        strcat(res, str3);

        affineM[index] = m[dim][j + dim + 1];
        index++;
    }

    affineM[8] = 1;

    LOGE_ORB("\n-------------------------------------\n");
    LOGE_ORB("%s", res);
}

bool gauss_jordan(double m[3][5], int length, int dim) {
    int h = length;
    int w = dim;
    /*
        LOGV_ORB("gauss1: \n[[%f, %f, %f, %f, %f]\n[%f, %f, %f, %f, %f]\n[%f, %f, %f, %f, %f]]\n",
            m[0][0], m[0][1], m[0][2], m[0][3], m[0][4],
            m[1][0], m[1][1], m[1][2], m[1][3], m[1][4],
            m[2][0], m[2][1], m[2][2], m[2][3], m[2][4]);
    */
    for(int y = 0; y < h; y++) {
        int maxrow = y;
        for(int y2 = y + 1; y2 < h; y2++) {
            if(fabs(m[y2][y]) > fabs(m[maxrow][y])) {
                maxrow = y2;
            }
        }

        //(m[y], m[maxrow]) = (m[maxrow], m[y])
        if (y != maxrow) {
            for(int i = 0; i < w; i++) {
                double temp = m[y][i];
                m[y][i] = m[maxrow][i];
                m[maxrow][i] = temp;
            }
        }

        if(fabs(m[y][y]) <= EPS) {
            return false;
        }

        for(int y2 = y + 1; y2 < h; y2++) {
            double c = m[y2][y] / m[y][y];
            for(int x = y; x < w; x++) {
                m[y2][x] -= m[y][x] * c;
            }
        }
    }

    for(int y = h - 1; y > -1; y--) {
        double c = m[y][y];
        for(int y2 = 0; y2 < y; y2++) {
            for(int x = w - 1; x > y - 1; x--) {
                m[y2][x] -= m[y][x] * m[y2][y] / c;
            }
        }
        m[y][y] /= c;
        for(int x = h; x < w; x++) {
            m[y][x] /= c;
        }
    }

    return true;
}

int affine_fit(double fpt[][2], double tpt[][2], int length, int dim, double affineM[9]) {
    if (length <= dim) {
        LOGE_ORB("number of points must bigger than dimension\n");
        return -1;
    }

    // dim+1 x dim
    double c[3][2] = {0};
    for(int j = 0; j < dim; j++) {
        for(int k = 0; k < (dim + 1); k++) {
            for(int i = 0; i < length; i++) {
                double qt[3] = {fpt[i][0], fpt[i][1], 1};
                c[k][j] += qt[k] * tpt[i][j];
            }
        }
    }

    // dim+1 x dim+1
    double Q[3][3] = {0};
    for(int k = 0; k < length; k++) {
        double qt[3] = {fpt[k][0], fpt[k][1], 1};
        for(int i = 0; i < (dim + 1); i++) {
            for(int j = 0; j < (dim + 1); j++) {
                Q[i][j] += qt[i] * qt[j];
            }
        }
    }

    double M[3][5];
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            M[i][j] = Q[i][j];
        }
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 3; j < 5; j++) {
            M[i][j] = c[i][j - 3];
        }
    }

    gauss_jordan(M, 3, 5);
    get_affine_matrix(M, 2, affineM);

    return 0;
}

int elimate_affine_transform(ORBList* matched_keypoints, double homography[9])
{
    int ret = 0;
    int i = 0, j = 0, k = 0;

    if (matched_keypoints->length < 3) {
        LOGE_ORB("matched keypoints num(%d) not enough", matched_keypoints->length);
        k = 0;
        for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
            orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
            LOGE_AORB("<%d>-[%d - %d]-[%d - %d]",
                      keypoint->score,
                      keypoint->col1, keypoint->row1,
                      keypoint->col2, keypoint->row2);
            k++;
        }
        return -1;
    }

    int max, M0, count = 0;
    int maxLength = 10;
    int subX, subY;
    int length = matched_keypoints->length > maxLength ? maxLength : matched_keypoints->length;
    int distance = -1, distanceAvg = 0;
    int* distanceArray = (int*)malloc(sizeof(int) * matched_keypoints->length);
    int* distanceSortedArray = (int*)malloc(sizeof(int) * matched_keypoints->length);
#if 0 // fix compile error for Android
    int (*M0_stats)[matched_keypoints->length] =
        (int (*)[matched_keypoints->length])malloc(sizeof(int) * matched_keypoints->length * 2);
    for (k = 0; k < matched_keypoints->length; k++) {
        M0_stats[0][k] = M0_stats[1][k] = -1;
    }
#else
    int* M0_stats[2];
    for (int i = 0; i < 2 ;i++)
        M0_stats[i] = (int*)malloc(sizeof(int) * matched_keypoints->length);
     for (k = 0; k < matched_keypoints->length; k++) {
         M0_stats[0][k] = M0_stats[1][k] = -1;
     }
#endif
    double (*from_pts)[2];
    from_pts = (double (*)[2])malloc(sizeof(double) * length * 2);

    double (*to_pts)[2];
    to_pts = (double (*)[2])malloc(sizeof(double) * length * 2);


    LOGD_ORB("matched points:");
    //work_begin();
    for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
        orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;

        subX = (keypoint->col1 - keypoint->col2);
        subY = (keypoint->row1 - keypoint->row2);
        distanceArray[i] = subX * subX + subY * subY;
        distanceSortedArray[i] = distanceArray[i];
        distanceAvg += distanceArray[i];
        i++;
    }

    distanceAvg /= matched_keypoints->length;

    //sort
    for (k = 0; k < matched_keypoints->length; k++) {
        for (j = k; j < matched_keypoints->length; j++) {
            if (distanceSortedArray[k] < distanceSortedArray[j]) {
                max = distanceSortedArray[j];
                distanceSortedArray[j] = distanceSortedArray[k];
                distanceSortedArray[k] = max;
            }
        }
    }

    //find mode
    for (k = 0, j = 0; k < matched_keypoints->length - 1; k++) {
        if (distanceSortedArray[k] == distanceSortedArray[k + 1]) {
            M0_stats[0][j] = distanceSortedArray[k];
            if (M0_stats[1][j] == -1) {
                M0_stats[1][j] = 2;
            } else {
                M0_stats[1][j] += 1;
            }
        } else {
            if(M0_stats[0][j] == -1) {
                M0_stats[0][j] = distanceSortedArray[k];
                M0_stats[1][j] = 1;
            }
            j++;

            if (k == matched_keypoints->length - 2) {
                M0_stats[0][j] = distanceSortedArray[k + 1];
                M0_stats[1][j] = 1;
            }
        }
    }

    for (k = 1; k < matched_keypoints->length; k++) {
        LOGE_AORB("distanceSortedArray[%d]: %d", k, distanceSortedArray[k]);

    }

    max = M0_stats[1][0];
    distance = M0_stats[0][0];
    for (k = 1; k < matched_keypoints->length; k++) {
        LOGE_AORB("M0_stats[%d]: %d, count: %d", k, M0_stats[0][k], M0_stats[1][k]);
        if (M0_stats[0][k] == -1) {
            break;
        }

        if (M0_stats[1][k] >= max) {
            max = M0_stats[1][k];
            distance = M0_stats[0][k];
        }
    }

    LOGE_AORB("mesured distance: %d, count: %d", distance, max);

    for (k = 0; k < matched_keypoints->length; k++) {
        if (abs(distanceArray[k] - distance) > distance * 10) {
            distanceArray[k] = -1;
        }
    }

    //work_end("calc-dist");
    //work_begin();

    i = 0;
    k = 0;
    for(Node* point = matched_keypoints->start; point != NULL && i < maxLength; point = point->next) {
        orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;

        if (distanceArray[k] == -1) {
            k++;
            continue;
        }

        from_pts[i][0] = keypoint->col1;
        from_pts[i][1] = keypoint->row1;
        to_pts[i][0] = keypoint->col2;
        to_pts[i][1] = keypoint->row2;
        i++;
        k++;
    }

    if (i < 3) {
        k = 0;
        for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
            orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
            LOGE_AORB("<%d>-[%d - %d]-[%d - %d]-<%d>",
                      keypoint->score,
                      keypoint->col1, keypoint->row1,
                      keypoint->col2, keypoint->row2,
                      distanceArray[k]);
            k++;
        }
        LOGE_ORB("valid matched keypoints %d less than 6", i);
        return -2;
    }

    ret = affine_fit(from_pts, to_pts, i, 2, homography);
    //work_end("calc-affine");

    free(from_pts);
    free(to_pts);
    free(distanceArray);
    free(distanceSortedArray);
#if 0 // fix compile error for Android
    free(M0_stats);
#else
    free(M0_stats[0]);
    free(M0_stats[1]);
#endif
    return ret;
}


void map_point(double mat[9], point_t* point_src, point_t* point_dst)
{
    // P[0], P[1], P[2],
    // P[3], P[4], P[5],
    // P[6], P[7], P[8]
    double D = point_src->col * mat[6] + point_src->row * mat[7] + mat[8];
    point_dst->col = (point_src->col * mat[0] + point_src->row * mat[1] + mat[2]) / D;
    point_dst->row = (point_src->col * mat[3] + point_src->row * mat[4] + mat[5]) / D;

}

orb_rect_t map_rect(double homography[9], orb_rect_t* rect_src)
{
    orb_rect_t rect_dst;
    point_t src, dst;
    src.col = rect_src->left;
    src.row = rect_src->top;
    map_point(homography, &src, &dst);
    rect_dst.left = dst.col;
    rect_dst.top = dst.row;

    src.col = rect_src->right;
    src.row = rect_src->bottom;
    map_point(homography, &src, &dst);
    rect_dst.right = dst.col;
    rect_dst.bottom = dst.row;

    rect_dst.width = rect_dst.right - rect_dst.left;
    rect_dst.height = rect_dst.bottom - rect_dst.top;

    return rect_dst;
}