#include "SobelFilter.hpp"

cv::Point2d SobelFilter::getSubPixelPoint(float dx, float dy, cv::Point edge, cv::Point delimiter,cv::Mat input){
    SampleStrip sample;

    double diffLength = sqrt ( dx*dx+dy*dy );
    int stripeLength = (int)(0.8* diffLength);

    if (stripeLength < 5)
        stripeLength = 5;

    sample.length = stripeLength;

    cv::Point2f p1;
    cv::Point2f p2;

    p1.x = dx / diffLength;
    p1.y = dy / diffLength;

    p2.x = p1.y;
    p2.y = -p1.x;

    sample.vecX = p1;
    sample.vecY = p2;

    int nStop = stripeLength / 2;
    int nStart = -nStop;

    cv::Size size(3,stripeLength);
    cv::Mat iplStripe(size, CV_8UC1);

    for (int m = -1; m <= 1; ++m) {
        for (int n = nStart; n <= nStop; ++n) {
            cv::Point2f subPixel;
            subPixel.x = sample.vecX.x * m + sample.vecY.x * n;
            subPixel.y = sample.vecX.y * m + sample.vecY.y * n;

            subPixel.x += delimiter.x;
            subPixel.y += delimiter.y;

            int pixel = samplePixel(input, subPixel);
            int w = m + 1;
            int h = n + (stripeLength >> 1);

            iplStripe.at<uchar>(h,w) = (uchar)pixel;
        }
    }

    cv::Sobel(iplStripe, iplStripe, CV_8U, 1, 0);

    return findSubPixelPoint(iplStripe, sample);
}

int SobelFilter::samplePixel(const cv::Mat &pSrc, const cv::Point2f &p){
    int x = int( floorf ( p.x ) );
    int y = int( floorf ( p.y ) );

    if ( x < 0 || x >= pSrc.cols - 1 || y < 0 || y >= pSrc.rows - 1 )
        return 127;

    int dx = int ( 256 * ( p.x - floorf ( p.x ) ) );
    int dy = int ( 256 * ( p.y - floorf ( p.y ) ) );

    unsigned char* i = ( unsigned char* ) ( ( pSrc.data + y * pSrc.step ) + x );
    int a = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
    i += pSrc.step;
    int b = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
    return a + ( ( dy * ( b - a) ) >> 8 );
}

cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip){

    double maxIntensity = -1;

    int maxIntensityIndex = 0;

    // Finding the max value

    for (size_t i = 0; i < iplStripe.cols; ++i) {
        uchar intensity = iplStripe.at<uchar>(i,1);
        if(intensity > maxIntensity){
            maxIntensity = intensity;
            maxIntensityIndex = i;
        }
    }

    double y0, y1, y2;

    // Point before and after
    unsigned int max1 = maxIntensityIndex - 1, max2 = maxIntensityIndex + 1;

    // TASK: Why do we need this if statement?
    y0 = (maxIntensityIndex <= 0) ? 0 : iplStripe.at<uchar>(max1,1);
    y1 = iplStripe.at<uchar>(maxIntensityIndex,1);
    // TASK: Why do we need this if statement?
    y2 = (maxIntensityIndex >= strip.length - 3) ? 0 : iplStripe.at<uchar>(max2,1);

    // Formula for calculating the x-coordinate of the vertex of a parabola, given 3 points with equal distances
    // (xv means the x value of the vertex, d the distance between the points):
    // xv = x1 + (d / 2) * (y2 - y0)/(2*y1 - y0 - y2)

    // d = 1 because of the normalization and x1 will be added later
    double pos = (y2 - y0) / (4 * y1 - 2 * y0 - 2 * y2);

    cv::Point2d edgeCenter;

    // TASK: What happens when there is no solution
    if(std::isnan(pos)){
        return edgeCenter;
    }

    edgeCenter.x = pos + (strip.vecX.x + strip.vecY.x);
    edgeCenter.y = pos + (strip.vecX.y + strip.vecY.y);

    return edgeCenter;
}

void SobelFilter::drawStrips(cv::Mat output){
    sampleStrips.clear();
    samples.clear();
}
