#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;

        // 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);

    //cout << resized.size() << 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);
    }
    return encodeImage;
}

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;

}

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;
}

}