#include "lfr.h"
#include <opencv2/core/utils/logger.hpp>
 
int main(void)
{
    //Disable opencv logging messages
    //cv::utils::logging::setLogLevel(cv::utils::logging::LOG_LEVEL_WARNING);

    const int thresholdBinary = 140;
    const int videoHeight = 720;
    const int videoWidth = 1280;
    const int gaussKernelSize = 11;

    std::mutex mutex;

    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
        return false;
    });

    //To calculate the frame rate
    std::chrono::milliseconds last = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    
    cv::Mat img;
    lfr.addListener([&](LFR_Result result)
    {
        std::unique_lock<std::mutex> lock(mutex);
        if (!result.rawImage.empty())
        {
            img = result.rawImage;
            //Resize to minimize latency using raspi over ssh
            //cv::resize(result.rawImage, img, cv::Size(128, 64+32));

            //Calculate frame rate
            now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
            unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
            double delta = static_cast<double>(deltaMs) / 1000.0;
            double frameRate = 1.0 / static_cast<double>(delta);

            if (result.validLane)
            {
                std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
                    << " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
                    << " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
            }
            else
            {
                std::cout << "No lane found." << std::endl;;
            }
            last = now;
        }
    }, &mutex);

    lfr.startLoop();
    
    for(int finished = false; finished != 'q';){
        finished = std::tolower(cv::waitKey(1));
        std::unique_lock<std::mutex> lock(mutex);
        if(!img.empty()){
            cv::imshow("frame", img);
        }
    }
}