#include "CasicIrisRec.h"
#include <math.h>
#include <fstream>

#include <Eigen/Dense>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include<opencv2/core/eigen.hpp>

using Eigen::MatrixXd;
using Eigen::MatrixXi;

namespace iristrt
{
CasicIrisRec::CasicIrisRec()
{

}

CasicIrisRec::CasicIrisRec(const char * gaborFilterFileName,const char * applicationPointsFileName)
{
    loadApplicationPoints(applicationPointsFileName);
    loadGaborFilters(gaborFilterFileName);
}

CasicIrisRec::~CasicIrisRec()
{

}

cv::Mat CasicIrisRec::normalize(cv::Mat image, int iris_x, int iris_y, int iris_r,int pupil_x, int pupil_y, int pupil_r)
{
    int width = NORM_WIDTH;
    int height = NORM_HEIGHT;
    int realHeight = height + 2;
    int angledivisions = width - 1;
    int rows = image.rows;
    int cols = image.cols;

    double ox = pupil_x - iris_x;
    double oy = pupil_y - iris_y;

    int sgn = ox <= 0 ? -1 : 1;
    if (ox == 0 && oy > 0) {
        sgn = 1;
    }

    double phi = ox == 0 ? M_PI / 2 : atan(oy / ox);

    MatrixXd a = MatrixXd::Ones(1, width) * (ox * ox + oy * oy);

    MatrixXd thetas(1, width);
    for (int i = 0; i < width; i++) {
        thetas(0, i) = (2 * M_PI / angledivisions) * i;
    }

    MatrixXd b = M_PI - phi - thetas.array();
    b = b.array().cos();
    b = sgn * b;

    MatrixXd item1 = a.array().sqrt() * b.array();
    MatrixXd item2 = a.array() * b.array().pow(2);
    MatrixXd item3 = a.array() - (iris_r * iris_r);
    MatrixXd r = item1 + (item2 - item3).array().sqrt().matrix();
    r = r.array() -  pupil_r;

    MatrixXd rMatrix = MatrixXd::Ones(1, realHeight).transpose() * r;
    MatrixXd tmp(1, realHeight);
    for (int i = 0; i < realHeight; i++) {
        tmp(0, i) = (double)(1.0 / realHeight) * i;
    }
    rMatrix = rMatrix.array() * (MatrixXd::Ones(angledivisions + 1, 1) * tmp).array().transpose();
    rMatrix = rMatrix.array() +  pupil_r;

    MatrixXd new_rMatrix = rMatrix.middleRows(1,realHeight-2);

    MatrixXd xcosMat = MatrixXd::Ones(height, 1) * thetas.array().cos().matrix();
    MatrixXd xsinMat = MatrixXd::Ones(height, 1) * thetas.array().sin().matrix();

    MatrixXd xo = pupil_x + new_rMatrix.array() * xcosMat.array();
    MatrixXd yo = pupil_y - new_rMatrix.array() * xsinMat.array();

    MatrixXi xo_i = xo.cast<int>();
    MatrixXi yo_i = yo.cast<int>();

    if(image.channels() == 3){
        cv::cvtColor(image,image,cv::COLOR_RGB2GRAY);
    }

    MatrixXi eigenImage;

    cv::cv2eigen(image, eigenImage);

    MatrixXi normMat = MatrixXi::Zero(xo_i.rows(), xo_i.cols());
    for (int i = 0; i < xo_i.rows(); i++) {
        for (int j = 0; j < xo_i.cols(); j++) {
            normMat(i, j) = eigenImage(yo_i(i, j), xo_i(i,j));
        }
    }

    cv::Mat normImage(image.size(),image.type());
    cv::eigen2cv(normMat, normImage);
    normImage.convertTo(normImage, CV_8UC1);

    return normImage;
}

std::vector<cv::Mat> CasicIrisRec::loadGaborFilters(const char * gaborFilterFileName)
{
    std::ifstream file(gaborFilterFileName, std::ios::in);
    if (!file)
    {
        std::cout << "Cannot load Gabor filters in file " << gaborFilterFileName << std::endl;
    }

    // Get the number of filters
    int n_filters;
    file >> n_filters;
    gaborFilters.resize(n_filters);

    //cout << "n_filters " << n_filters << endl;

    // Size of filter
    int rows, cols;

    // Loop on each filter
    for (int f = 0; f < n_filters; f++)
    {
        // Get the size of the filter
        file >> rows;
        file >> cols;
        //cout << rows << " " << cols << endl;
        // Temporary filter. Will be destroyed at the end of loop
        gaborFilters[f] = cv::Mat(rows, cols, CV_32FC1);

        // Set the value at coordinates r,c
        for (int r = 0; r < rows; r++)
        {
            for (int c = 0; c < cols; c++)
            {
                file >> gaborFilters[f].at<float>(r,c);
            }
        }

    } // Loop on each filter

    // Close the file
    file.close();

    return gaborFilters;
}

cv::Mat CasicIrisRec::loadApplicationPoints(const char * applicationPointsFileName)
{
    // Open text file containing the filters
    std::ifstream file(applicationPointsFileName, std::ios::in);
    if (!file)
    {
        std::cout << "Cannot load the application points in " << applicationPointsFileName << std::endl;
    }

    // Get the number of points
    int n_points = 0;
    file >> n_points;

    // Allocate memory for the matrix containing the points
    applicationPoints = cv::Mat(NORM_HEIGHT, NORM_WIDTH, CV_8UC1);
    // Initialize all pixels to "off"
    applicationPoints.setTo(cv::Scalar(0));

    // Local variables
    int i, j;

    // Loop on each point
    for (int p = 0; p < n_points; p++)
    {
        // Get the coordinates
        file >> i; file >> j;

        mvApplicationPoints.push_back(QPair<int, int>(i, j));

        // Set pixel to "on"
        if (i < 0 || i > applicationPoints.rows - 1 || j < 0 || j > applicationPoints.cols- 1)
        {
            std::cout << "Point (" << i << "," << j << ") ";
            std::cout << "exceeds size of normalized image : ";
            std::cout << applicationPoints.rows << "x" << applicationPoints.cols;
            std::cout << " while loading application points" << std::endl;
        }
        else
        {
            applicationPoints.at<uchar>(i,j) = 255;
        }
    }

    //cv::imshow("mpApplicationPoints", mpApplicationPoints);
    //cv::waitKey(0);

    // Close the file
    file.close();

    return applicationPoints;
}

cv::Mat CasicIrisRec::encodeToImage(cv::Mat normalizedImage)
{
    cv::Size size(normalizedImage.size());
    cv::Mat encodeImage(cv::Size(size.width, size.height * gaborFilters.size()), CV_32FC1);

    int max_width = 0;
    for (int f = 0; f < gaborFilters.size(); f++)
        if (gaborFilters[f].cols > max_width)
            max_width = gaborFilters[f].cols;
    max_width = (max_width - 1) / 2;

    cv::Mat resized = addBorders(normalizedImage, max_width);

//    std::cout << resized.size() << std::endl;
//    cv::imshow("resized", resized);
//    cv::waitKey(0);

    cv::Mat img1(resized.size(), CV_32F, 1);
    cv::Mat img2(resized.size(), encodeImage.depth(), 1);

    for (int f = 0; f < gaborFilters.size(); f++)
    {
        // Convolution
        cv::filter2D(resized, img1, img1.depth(), gaborFilters[f]);

        // Threshold : above or below 0
        cv::threshold(img1, img2, 0, 255, cv::THRESH_BINARY);

//        cv::imshow("img2", img2);
//        cv::waitKey(0);

        // Form the iris code
        cv::Mat roi = img2(cv::Rect(max_width, 0, normalizedImage.cols, normalizedImage.rows));
        cv::Mat dst = encodeImage(cv::Rect(0, f*normalizedImage.rows, normalizedImage.cols, normalizedImage.rows));
        roi.copyTo(dst);
        //cv::copyTo(img2, dst, NULL);
    }
    cv::Mat image8UC1;
    encodeImage.convertTo(image8UC1, CV_8UC1);
    return image8UC1;
}

QByteArray CasicIrisRec::extractFeature(cv::Mat irisCode, cv::Mat maskNorm)
{
    // count points width and height
    cv::Mat pointTemp(applicationPoints);
    cv::Mat rowTemp;

    std::vector<int> pointRows;
    for (int i = 0; i < applicationPoints.rows; i++) {
        rowTemp = pointTemp(cv::Rect(0, i, applicationPoints.cols, 1));
        if (cv::sum(rowTemp)[0] > 0) {
            pointRows.push_back(i);
        }
    }

    int width = irisCode.cols;
    int height = pointRows.size();
    rowTemp.release();
    pointTemp.release();

    int n_codes = irisCode.rows / applicationPoints.rows;

    int irisDataLength = (n_codes + 1) * width * height;
    uchar * irisData = new uchar[irisDataLength];
    int irisDataIndex = 0;

    uchar* codePixel = new uchar;

    // save iris code 512 * 8 * 6
    for (int n = 0; n < n_codes; n++) { // 6
        for (auto i : pointRows) { // 8
            i = n * maskNorm.rows + i;
            for (int j = 0; j < irisCode.cols; j++) { // 512
                codePixel = (uchar*)(irisCode.data + i * irisCode.step + j);
                if (*codePixel == 255) {
                    irisData[irisDataIndex++] = 1;
                }
                else {
                    irisData[irisDataIndex++] = *codePixel;
                }
            }
        }
    }

    // save iris mask 512 * 8 * 1
    uchar * maskPixel;
    for (auto i : pointRows) { // 8
        for (int j = 0; j < irisCode.cols; j++) { // 512
            maskPixel = (uchar*)(maskNorm.data + i * maskNorm.step + j);
            if (*maskPixel == 255) {
                irisData[irisDataIndex++] = 1;
            }
            else {
                irisData[irisDataIndex++] = *maskPixel;
            }
        }
    }


    uchar * irisFeaturePtr = new uchar[irisDataLength / 8 + 5];
    memset(irisFeaturePtr, 0, irisDataLength / 8 + 5);

    // first byte is width;
    irisFeaturePtr[0] = (uchar)width;
    irisFeaturePtr[1] = (uchar)(width >> 8);

    // second byte is height;
    irisFeaturePtr[2] = (uchar)height;

    // third byte is n_codes
    irisFeaturePtr[3] = (uchar)n_codes;

    // fourth byte is n_mask(1)
    irisFeaturePtr[4] = (uchar)1;
    for (int i = 0; i < irisDataLength; i++) {
        irisFeaturePtr[5 + i / 8] += irisData[i] << (7 - (i % 8));
    }

    QByteArray feature;
    for (int i = 0; i < irisDataLength / 8 + 5; i++) {
        feature.append((char) irisFeaturePtr[i]);
    }

    delete[] irisData;
    delete[] irisFeaturePtr;
    return feature;
}

float CasicIrisRec::matchImage(cv::Mat code1, cv::Mat code2, cv::Mat normalizedMask1, cv::Mat normalizedMask2)
{
    code1.convertTo(code1, CV_8UC1);
    code2.convertTo(code2, CV_8UC1);

    cv::Mat temp(applicationPoints.size(), CV_8UC1);
    temp.setTo(cv::Scalar(0));

    cv::bitwise_and(normalizedMask1, normalizedMask2, temp, applicationPoints);

    // Copy the mask f times, where f correspond to the number of codes (= number of filters)
    int n_codes = code1.rows / applicationPoints.rows;

    cv::Mat totalMask(code1.size(), CV_8UC1);
    for (int n = 0; n < n_codes; n++) {
        cv::Mat maskRoi = totalMask(cv::Rect(0, n*applicationPoints.rows, applicationPoints.cols, applicationPoints.rows));
        temp.copyTo(maskRoi);
    }

//    cv::imshow("totalMask", totalMask);
//    cv::waitKey(0);

    cv::Mat result(code1.size(), CV_8UC1);
    result.setTo(cv::Scalar(0));

    int shift = 10;
    cv::Mat shifted = addBorders(code1, shift);

    float score = 1;
    for (int s = -shift; s <= shift; s++) {
        cv::Mat roi = shifted(cv::Rect(shift + s, 0, code1.cols, code1.rows));
        cv::bitwise_xor(roi, code2, result, totalMask);

        float mean = (cv::sum(result).val[0]) / (cv::sum(totalMask).val[0]);
        // std::cout << "mean: " << mean << std::endl;
        score = std::min(score, mean);
    }

    // std::cout << "score: " << score << std::endl;
    return score;

}

float CasicIrisRec::matchFeatureCode(uchar *feature1, uchar *feature2)
{
    int width = feature1[0] | feature1[1] << 8; // 32
    int height = feature1[2]; // 8
    int nCode = feature1[3]; // 6
//    int nMask = feature1[4];

    uchar ** irisData1 = reduceIrisCode(feature1);
    uchar ** irisMask1 = reduceIrisMask(feature1);
    uchar ** irisData2 = reduceIrisCode(feature2);
    uchar ** irisMask2 = reduceIrisMask(feature2);

    int shift = 10;
    float score = 1.0;

    int minstep = 0 - shift;
    int maxstep = shift;

    for (int s = minstep; s <= maxstep; s++) {
        int count = 0;
        float mean = 0.0;
        for (int n = 0; n < nCode; n++) {
            for (auto pos : mvApplicationPoints) {
                int row = pos.first / mvApplicationPoints[0].first - 1;
                int col = (pos.second + s + width) % width;

                if (irisMask1[row][pos.second] == 1 && irisMask2[row][pos.second] == 1) {

                    uchar tmp1 = irisData1[row + height * n][col];
                    uchar tmp2 = irisData2[row + height * n][pos.second];
                    mean += tmp1 ^ tmp2;
                    count += 1;
                }
            }
        }

        //cout << "shift " << s << "  " << mean << "/" << count << " : " << mean / count << endl;
        mean = mean / count;
        score = mean < score ? mean : score;

    }

//    std::cout << "feature score: " << score << std::endl;


//    for (int i = 0; i < height * nCode; i++) {
//        delete irisData1[i];
//        delete irisData2[i];
//    }
//    delete irisData1;
//    delete irisData2;

//    for (int i = 0; i < height; i++) {
//        delete irisMask1[i];
//        delete irisMask2[i];
//    }
//    delete irisMask1;
//    delete irisMask2;
    return score;
}

cv::Mat CasicIrisRec::addBorders(cv::Mat pSrc, int width)
{
    cv::Mat result(cv::Size(pSrc.cols + 2 * width, pSrc.rows), pSrc.depth(), pSrc.channels());

    copyMakeBorder(pSrc, result, 0, 0, width, width, cv::BORDER_REPLICATE, 0);

    for (int i = 0; i < pSrc.rows; i++) {
        for (int j = 0; j < width; j++) {
            result.at<uchar>(i, j) = pSrc.at<uchar>(i, pSrc.cols - width + j);
            result.at<uchar>(i, result.cols - width + j) = pSrc.at<uchar>(i, j);
        }
    }
    return result;
}

uchar ** CasicIrisRec::reduceIrisCode(uchar *feature)
{
    int width = feature[0] | feature[1] << 8;
    int height = feature[2] * feature[3];

    uchar ** reduceCode = new uchar*[width*height];
    for (int i = 0; i < height; i++) {
        reduceCode[i] = new uchar[width];
        for (int j = 0; j < width; j++) {
            int tmp = i * width + j;
            reduceCode[i][j] = (uchar)feature[5 + tmp / 8] >> (7 - tmp % 8) & 1;
        }
    }

    return reduceCode;
}

uchar ** CasicIrisRec::reduceIrisMask(uchar *feature)
{
    int width = feature[0] | feature[1] << 8;
    int height = feature[2]; // 8
    int nCode = feature[3]; // 6

    uchar ** irisMask = new uchar*[width*height];
    for (int i = 0; i < height; i++) {
        irisMask[i] = new uchar[width];
        for (int j = 0; j < width; j++) {
            int tmp = width * height * nCode + i * width + j;
            irisMask[i][j] = (uchar)feature[5 + tmp / 8] >> (7 - tmp % 8) & 1;
        }
    }

    return irisMask;
}

}