#include "CLM_core.h"

#include <fstream>
#include <sstream>

#include <boost/accumulators/accumulators.hpp>


using namespace std;
using namespace cv;

int main (int argc, char **argv)
{

    // Assuming 640x490 images
    float fx = 500, fy = 500, cx = 320, cy = 245;

        //Image sequence read from a file
    std::ifstream imageFile("Path_Images.txt");
    if(!imageFile.good())
    {
        std::cout << "Image list not found." << std::endl;
        return 0;
    }

    CLMTracker::PDM pdm;
    pdm.Read("./bin/model/pdms/In-the-wild_aligned_PDM_68.txt");

    string line;
    cv::Vec6d pose_params = 0;
    while(getline(imageFile, line))
    {
        Mat im = cv::imread(line, CV_LOAD_IMAGE_COLOR);
        line.replace(line.size()-3, 3, "pts");

        cv::Mat_<double> landmarks(136, 1);
        std::ifstream landmarkFile(line.c_str());
        if(!landmarkFile.good())
        {
            std::cout << "Landmark file: " << line << " not readable" << std::endl;
            return 0;
        }
        std::cout << "Processing Image: " << line << std::endl;
        string temp;
        landmarkFile >> temp;
        landmarkFile >> temp;
        landmarkFile >> temp;
        for(int i=0; i< 68; i++)
        {
            landmarkFile >> landmarks.at<double>(i, 0);
            landmarkFile >> landmarks.at<double>(i+68, 0);
        }

        cv::Mat_<double> local_params(34, 1);
        local_params.setTo(0);
        cv::Vec3d rotation;
        rotation[0] = pose_params[3]; rotation[1] = pose_params[4]; rotation[2] = pose_params[5];
        auto t1 = std::chrono::high_resolution_clock::now();
        pdm.CalcParams( pose_params, local_params, landmarks, rotation);
        auto t2 = std::chrono::high_resolution_clock::now();
        auto timeTaken = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
        std::cout << pose_params << std::endl;

        int thickness = 2;
        CLMTracker::DrawBox(im, pose_params, Scalar(0, 0, 255), thickness, fx, fy, cx, cy);
        imshow("tracking_result", im);
        waitKey(1);
    }

    return 0;
}