include "opencv2/opencv.hpp"

include "opencv2/core/core.hpp"

include "opencv2/highgui/highgui.hpp"

include "opencv2/videoio.hpp"

include

include

include

include

include

int main() { // line tracking class Scanline scanline1, scanline2, scanline3; bool debug = false;

// open the video file
cv::VideoCapture capture("/home/jameschinkt/Pictures/4feb.mp4");
if(!capture.isOpened()) {
    return 1;
}

// get the frame rate and delay period
double rate = capture.get(cv::CAP_PROP_FPS);
int delay = static_cast<int>(1000/rate);
std::cout << "Frame rate: " << rate << std::endl;

// create output and resized video frame
cv::Mat frame, frame_resized;
cv::Size size(960,540); // original image is 1920 x 1080

// shared fixed variables
int cx_intensity = 128;
int cx_width_min = 5;
int cx_width_max = 30;
int scan_line_small = 80;
int scan_line_large = 300;
// individual variables
int roi_line1 = 700;
int cy1 = 350;
int roi_line2 = 660;
int cy2 = 320;
int roi_line3 = 600;
int cy3 = 290;

// line tracking results
bool track1, track2, track3;
int cx1, cx2, cx3;

int64 start = 0;    // for performance measurement
int64 stop = 0;

scanline1.set_new_cx_intensity_threshold(cx_intensity);     // how much is white for lane color?
scanline1.set_new_cx_widths(cx_width_min, cx_width_max);    // min and max width for a lane
scanline1.set_canny_thresholds(85, 250);                    // canny thresholds
scanline1.set_scan_widths(80, 300);                         // scan line width for track and non-track
scanline1.set_roi_line_cx(roi_line1);                       // x vertical point of interest
scanline1.set_scanline_cy(cy1);                             // y horizontal line of interest

scanline2.set_new_cx_intensity_threshold(cx_intensity);     // how much is white for lane color?
scanline2.set_new_cx_widths(cx_width_min, cx_width_max);    // min and max width for a lane
scanline2.set_canny_thresholds(85, 250);                    // canny thresholds
scanline2.set_scan_widths(80, 300);                         // scan line width for track and non-track
scanline2.set_roi_line_cx(roi_line2);                       // x vertical point of interest
scanline2.set_scanline_cy(cy2);                             // y horizontal line of interest

scanline3.set_new_cx_intensity_threshold(cx_intensity);     // how much is white for lane color?
scanline3.set_new_cx_widths(cx_width_min, cx_width_max);    // min and max width for a lane
scanline3.set_canny_thresholds(85, 250);                    // canny thresholds
scanline3.set_scan_widths(80, 300);                         // scan line width for track and non-track
scanline3.set_roi_line_cx(roi_line3);                       // x vertical point of interest
scanline3.set_scanline_cy(cy3);                             // y horizontal line of interest


while(true)
{
    // performance measurement - start
    start = cv::getTickCount();

    if(!capture.read(frame)) {
        break;
    }

    if(frame.channels() == 3 ) {
        cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);         // convert to grayscale
    }

    cv::resize(frame,frame_resized,size,cv::INTER_CUBIC);       // resize image

    // =================================================       perfom scanline
    track1 = scanline1.track_line(frame_resized, debug);
    cx1= scanline1.get_new_cx();

    track2 = scanline2.track_line(frame_resized, debug);
    cx2= scanline2.get_new_cx();

    track3 = scanline3.track_line(frame_resized, debug);
    cx3= scanline3.get_new_cx();

    // =================================================        draw the results on the same frame_resized
    if(track1)
    {
        // line is white when tracking
        cv::line(frame_resized,cv::Point(cx1 - scan_line_small,cy1),cv::Point(cx1 + scan_line_small,cy1),cv::Scalar(255,255,255),3,9);
    }
    else
    {
        // line is black when unable to track and using previous cx
        cv::line(frame_resized,cv::Point(cx1 - scan_line_large,cy1),cv::Point(cx1 + scan_line_large,cy1),cv::Scalar(0,0,0),3,9);
    }

    if(track2)
    {
        // line is white when tracking
        cv::line(frame_resized,cv::Point(cx2 - scan_line_small,cy2),cv::Point(cx2 + scan_line_small,cy2),cv::Scalar(255,255,255),3,9);
    }
    else
    {
        // line is black when unable to track and using previous cx
        cv::line(frame_resized,cv::Point(cx2 - scan_line_large,cy2),cv::Point(cx2 + scan_line_large,cy2),cv::Scalar(0,0,0),3,9);
    }

    if(track3)
    {
        // line is white when tracking
        cv::line(frame_resized,cv::Point(cx3 - scan_line_small,cy3),cv::Point(cx3 + scan_line_small,cy3),cv::Scalar(255,255,255),3,9);
    }
    else
    {
        // line is black when unable to track and using previous cx
        cv::line(frame_resized,cv::Point(cx3 - scan_line_large,cy3),cv::Point(cx3 + scan_line_large,cy3),cv::Scalar(0,0,0),3,9);
    }

    //*****************************************************************************
    //      GENERATING QUADRATIC EQUATION Y = AX^2 + BX + C
    //*****************************************************************************

    double denom = ((cx1 - cx2) * (cx1 - cx3) * (cx2 - cx3));

    double A = ((cx3 * (cy2 - cy1)) + (cx2 * (cy1 - cy3)) + (cx1 * (cy3 - cy2))) / denom;
    double B = ((cx3 * cx3 * (cy1 - cy2)) + (cx2 * cx2 * (cy3 - cy1)) + (cx1 * cx1 * (cy2 - cy3))) / denom;
    double C = ((cx2 * cx3 * (cx2 - cx3) * cy1) + (cx3 * cx1 * (cx3 - cx1) * cy2) + (cx1 * cx2 * (cx1 - cx2) * cy3)) / denom;

    double x_vertex = (-B / (2 * A));
    double y_vertex = ((C - B * B) / (4 * A));


    // Display the equation of the 3 points only when 3 points are detected
    if (track1 && track2 && track3)
    {
        char text1[255];
        char text2[255];
        sprintf(text1, "Eqn: y = Ax^2  +  Bx  +  C");
        sprintf(text2, "Eqn: y = %fx^2 + %fx + %f",A, B, C);

        cv::putText(frame_resized, text1, cv::Point(30,500), 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(255,255,1));
        cv::putText(frame_resized, text2, cv::Point(30,520), 1, cv::FONT_HERSHEY_PLAIN, cv::Scalar(255,255,1));
    }

    cv::imshow("Output Video",frame_resized);

    // performance measurement - stop
    stop = cv::getTickCount();
    double time = (stop - start)/ cv::getTickFrequency() * 1000;
    std::cout << " Processed time: " << time << std::endl;

    // press key to pause and resume
    if (cv::waitKey(delay) >= 0)
    {
        while (true)
        {
            if (cv::waitKey(delay) >= 0)
                break;
        }
    }

}

return 1;

}