#include "dualcamgrabber.h"
#include <iostream>
#include <pylon/PylonIncludes.h>
#include <stitchimage.h>
#include <chrono>
using namespace std::chrono;
typedef high_resolution_clock Time;
typedef milliseconds ms;
typedef duration<float> fsec;

void DualcamGrabber::run() {
  stop_request = false;
  size_t counter = 0;

  if (wrapper->isOpen()) {
      Pylon::CInstantCameraArray &cameras = wrapper->getCameraArray();
      wrapper->frameRate(static_cast<uint>(framerate), -1);
      wrapper->exposureTime(exposure);
      wrapper->gain(gain);

      cameras.StartGrabbing();
      Pylon::CGrabResultPtr frame0, frame1;
      Pylon::CPylonImage leftImage;
      Pylon::CPylonImage rightImage;
      Pylon::CPylonImage stitchedImage;
      std::string errorMessage = "";

      // int ifi = 0;
      // int deviation = 0;
      // std::cerr << wrapper->frameRate(0) << "\t" << wrapper->frameRate(1) << "\t" << framerate << std::endl;
      // int desired_ifi = (1./wrapper->frameRate(0) * 1000000);
      // auto framestart = high_resolution_clock::now();
      while (cameras.IsGrabbing() && !stop_request) {
        // if (counter > 0) {
        //   deviation = desired_ifi - ifi;
        //   if (deviation > 0)
        //     // usleep(deviation);
        //     std::cerr << desired_ifi << "\t" << deviation << std::endl;
        // }
          // auto start = high_resolution_clock::now();
          MyImage *img = new MyImage();
          // auto stop1 = high_resolution_clock::now();
          cameras[0].RetrieveResult( 5000, frame0, Pylon::TimeoutHandling_ThrowException );
          // auto stop2 = high_resolution_clock::now();
          cameras[1].RetrieveResult( 5000, frame1, Pylon::TimeoutHandling_ThrowException );
          // auto stop3 = high_resolution_clock::now();
          leftImage.AttachGrabResultBuffer( frame0 );
          rightImage.AttachGrabResultBuffer( frame1 );
          if (leftImage.IsValid() && rightImage.IsValid()) {
            try {
              StitchImage::StitchToRight(leftImage, rightImage, &stitchedImage, errorMessage);
              img->setFrame(stitchedImage);
              buffer->push(img);
            } catch(const std::exception& e) {
              std::cerr << e.what() << '\n';
            }
          }
          // auto stop4 = high_resolution_clock::now();
          // auto duration1 = duration_cast<microseconds>(stop1 - start);
          // auto duration2 = duration_cast<microseconds>(stop2 - stop1);
          // auto duration3 = duration_cast<microseconds>(stop3 - stop2);
          // auto duration4 = duration_cast<microseconds>(stop4 - stop3);
          // std::cerr << "framecount: " << counter << " image constr: " << duration1.count() << "\t" << " retrieve1: " << duration2.count() << "\t" << " retrieve2: " << duration3.count() << "\t" << "conversion: " << duration4.count() << std::endl;
          // ifi = duration_cast<microseconds>(stop4 - framestart).count();
          // framestart = stop4;
          // if (counter > 0) {
          //   std::cerr << "frame " << counter << " inter frame interval: " << ifi << "microseconds" << std::endl;
          // } 
          counter += 1;
      }
      cameras.StopGrabbing();
    }
}