#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 = "";

      while (cameras.IsGrabbing() && !stop_request) {
          auto before = high_resolution_clock::now();
          try {
            cameras[0].RetrieveResult( 5000, frame0, Pylon::TimeoutHandling_ThrowException );
            cameras[1].RetrieveResult( 5000, frame1, Pylon::TimeoutHandling_ThrowException );
            leftImage.AttachGrabResultBuffer( frame0 );
            rightImage.AttachGrabResultBuffer( frame1 );
          } catch(const std::exception& e) {
            qDebug() << "Grabbing frame failed! " << e.what();
          }
          auto after = high_resolution_clock::now();
          if (leftImage.IsValid() && rightImage.IsValid()) {
            try {
              StitchImage::StitchToRight(leftImage, rightImage, &stitchedImage, errorMessage);
              MyImage *img = new MyImage(stitchedImage.GetWidth(), stitchedImage.GetHeight());
              img->setFrame(stitchedImage);
              buffer->push(img);
            } catch(const std::exception& e) {
              std::cerr << e.what() << '\n';
            }
          }
          auto stitch = high_resolution_clock::now();
          auto grab_duration = duration_cast<microseconds>(after - before);
          auto stitch_duration = duration_cast<microseconds>(stitch - after);
          std::cerr << "framecount: " << counter << " grab_duration (us): " << grab_duration.count() << "\t" << " stitching (us): " << stitch_duration.count() << std::endl;
          counter += 1;
      }
      cameras.StopGrabbing();
    }
}