#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <iostream>
#include <opencv2/features2d/features2d.hpp>

using namespace std;
using namespace cv;

#define THRESHOLD 100


/*
** Compares intensity of pixels 1,5,9,13 of the circle surrounding a pixel at (i,j) of an image with its intensity ip.
** If 3 out of 4 satisfy the threshold FAST constraints : bright (i>ip+t) & dark (i<ip-t),
** the pixel at (i,j) is considered a possible key point
*/
bool couldBeKeyPoint(Mat imageIn, int i, int j, int threshold) {
    uchar ip   = imageIn.at<unsigned char>(i, j); //intensity of the potential key point
    uchar ip9  = imageIn.at<unsigned char>(i, j - 3); //intensity of pixel 1 of the surrounding circle
    uchar ip1  = imageIn.at<unsigned char>(i, j + 3); //intensity of pixel 9 of the surrounding circle
    uchar ip5  = imageIn.at<unsigned char>(i + 3, j); //intensity of pixel 5 of the surrounding circle
    uchar ip13 = imageIn.at<unsigned char>(i - 3, j); //intensity of pixel 13 of the surrounding circle

    //checking FAST bright constraints on these 4 surrounding pixels
    bool b1 = (ip1 >= ip +  threshold);
    bool b9 = (ip9 >= ip +  threshold);
    bool b5 = (ip5 >= ip +  threshold);
    bool b13 = (ip13 >= ip +  threshold);

        //cout << b1+b9+b5+b13 ;
    //at least three of these must all be brighter than I_p + t.
    if (b1+b9+b5+b13 >=3)
        return true;

    bool d1 = (ip1 <= ip - threshold);
    bool d9 = (ip9 <= ip -  threshold);
    bool d5 = (ip5 <= ip - threshold);
    bool d13 = (ip13 <= ip -  threshold);
    //cout << d1+d9+d5+d13 << "n" ;
    //at least three of these must all be darker than I_p − t.
    if (d1+d9+d5+d13 >=3)
        return true;

    return false;
}

bool isKeyPoint(Mat imageIn, int i, int j, int threshold, int numberPixelsToCheck){
    cout << "iskeypoint";
    vector<unsigned char> pixelSurroundings;

    pixelSurroundings.push_back(imageIn.at<unsigned char>(i, j));//the potential key point
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i, j + 3));//pixel 1
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 1, j - 3));//pixel 2
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 2, j + 2));//pixel 3
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 3, j + 1));//pixel 4
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 3, j));//pixel 5
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 3, j - 1));//pixel 6
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 2, j - 2));//pixel 7
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i + 1, j - 3));//pixel 8
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i, j - 3));//pixel 9
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 1, j - 3));//pixel 10
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 2, j - 2));//pixel 11
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 3, j - 1));//pixel 12
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 3, j));//pixel 13
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 3, j + 1));//pixel 14
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 2, j + 2));//pixel 15
    pixelSurroundings.push_back(imageIn.at<unsigned char>(i - 1, j + 3));//pixel 16

    if (numberPixelsToCheck > 16){
        numberPixelsToCheck = 12; //The author have used N=12 in the first version of the algorithm
        cout <<  "Error number of surrounding pixels to check should not exceed 16! Value 12 was used instead. " << std::endl ;
    }

    unsigned char ip = pixelSurroundings[0];
    int  brightScore = 0;
    int  darkScore = 0;
    bool d = false,e=false;
    for(int j=1;j<pixelSurroundings.size();j++){
        unsigned char i = pixelSurroundings[j];
        d = (i >= ip + (unsigned char ) threshold);
        e = (i <= ip - (unsigned char ) threshold);

        brightScore += d;
        darkScore +=  e;
    }
        cout << darkScore << " DARKSCORE n";
        cout << brightScore << " BRIGHTSCORE n";
    if (darkScore >= numberPixelsToCheck || brightScore >= numberPixelsToCheck){
        //cout << darkScore << " DARKSCORE n";
        //cout << brightScore << " BRIGHTSCORE n";
        return true; //the pixel is a key point
    }

    return false;
}

//renvoit un ensemble de détections
//inputarray image
vector<KeyPoint> FAST(Mat imageIn, vector<KeyPoint> keypoints, int threshold){
    if(!imageIn.data )                              // Check for invalid input
    {
        cout <<  "Could not open or find the image" << std::endl ;
        //return {};
    }
    keypoints.clear();
    int i, j, count =0;
    for (i = 3; i < imageIn.rows - 3; i++)
    {
        for (j = 3; j < imageIn.cols - 3; j++)
        {
            if (couldBeKeyPoint(imageIn, i, j, threshold)){
                if (isKeyPoint(imageIn, i, j, threshold, 0)){
                        keypoints.push_back(KeyPoint(j, i ,1));
                        count++;
                        cout << "keypoint found at " << i << " " << j << "n";
                    }
            }
        }
    }
    cout << "NUMBER OF KEYPOINTS :" << keypoints.size() << "n";
    return keypoints;
}


int main(int argc, char** argv){
    vector<KeyPoint> keypointsMyFast;
    vector<KeyPoint> keypointsOpenCvFast;
    Mat src, destMyFast, destOpenCvFast;
    //src= imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    //src= imread(argv[1], CV_8UC3);
    src= imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    imshow( "ORGINAL",src);
    waitKey(1);

    keypointsMyFast = FAST(src, keypointsMyFast,THRESHOLD);
    drawKeypoints(src, keypointsMyFast, destMyFast, Scalar(255,0,0));
    imshow( "MYFAST",destMyFast);
    waitKey(1);
    FAST(src,keypointsOpenCvFast,THRESHOLD,false);
    cout << "NUMBER OF open cv KEYPOINTS :" << keypointsOpenCvFast.size() << "n";
    drawKeypoints(src, keypointsOpenCvFast, destOpenCvFast, Scalar(255,0,0));
    imshow( "Display window",destOpenCvFast);
    waitKey(0);


}