#include "ActiveContour.hpp"

void computeExternalEnergyImage(Mat input, Mat& energyImage)
{
    energyImage.create(input.rows, input.cols, CV_64FC1);

    //declaring a mat for the gaussian blur
    Mat blurredIMG;
    //decaring sobel and abs mats
    Mat sobelXImage;
    Mat sobelYImage;
    Mat ABSX;
    Mat ABSY;
    //declaring gradient mats
    Mat gradient;
    Mat LargeGradient;
    Mat SmallGradient;
    Mat MedGradient;

    GaussianBlur(input,blurredIMG,Size(7,7),0);

    Sobel(blurredIMG,sobelXImage,CV_64F,1,0,1,1,0,BORDER_DEFAULT);

    Sobel(blurredIMG, sobelYImage, CV_64F, 0, 1, 1, 1, 0, BORDER_DEFAULT);


    ABSY = cv::abs(sobelYImage);
    ABSX = cv::abs(sobelXImage);

    gradient = ABSY;
    gradient += ABSX;

    GaussianBlur(gradient,LargeGradient,Size(301,301),0);
    GaussianBlur(gradient,MedGradient,Size(41,41),0);
    GaussianBlur(gradient,SmallGradient,Size(13,13),0);

    energyImage = SmallGradient + MedGradient + LargeGradient + gradient;

}
double computeEnergyForCurve(vector<Point>& allPoints, Mat& energyImage, double iaw, double ibw, double ew)
{
    //ew is the external energy weight, iaw is the interal first - derivative weight, and ibw is the internal second - derivative weight.
    //Compute the internal energy as the ((first derivative) * iaw) + ((second derivative) * ibw)
    double tmp = 0.0, exEn = 0.0, inEn = 0.0;
    double fdiv = 0.0, sdiv = 0.0;
    for (int i = 0; i < allPoints.size(); i++) {
        exEn += energyImage.at<double>(allPoints.at(i));
    }

    exEn = (exEn * (-1)) * ew;
    //cout << exEn << "got the EWEWEWE" << endl;

    double p0, p1, p2;
    double length;

    for (int i = 1; i < allPoints.size(); i++) {
        p1 = energyImage.at<double>(allPoints.at((int)i - 1));
        p2 = energyImage.at<double>(allPoints.at(i));
        fdiv = cv::norm(p1 - p2);

        if (i > 1)
        {
            p0 = energyImage.at<double>(allPoints.at((int)i - 2));
            sdiv = cv::norm(p0 - 2.0 * p1 + p2);
        }
    }
    inEn = (fdiv * iaw) + (sdiv * ibw);
    tmp = inEn + exEn;
    return tmp;
}
bool updateCurve(vector<Point>& allPoints, Mat& energyImage, double iaw, double ibw, double ew)
{
    bool updated = false;
    Point tmp;
    double inten;
    int x, y, n = 0;

    for (int i = 0; i < allPoints.size(); i++) {
        inten = energyImage.at<double>(allPoints.at(i));
        tmp = allPoints.at(i);
        for (int j = 0; j < DIR_CNT; j++) {
            x = dirs[j].x;
            y = dirs[j].y;
            if (allPoints[i].x+x < energyImage.rows && allPoints[i].y+y < energyImage.cols)
            {
                if (inten < energyImage.at<double>(tmp.x + x, tmp.y + y))
                {
                    inten = energyImage.at<double>(tmp.x + x, tmp.y + y);
                    n++;
                    updated = true;
                }
                else
                {
                    updated = false;
                }
            }
        }
        if (updated)
        {
            allPoints[i].x += dirs[n].x;
            allPoints[i].y += dirs[n].y;
        }
        n = 0;
    }

    return updated;
}