#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()) {
    return;
  }

  Pylon::CInstantCameraArray &cameras = wrapper->getCameraArray();
  wrapper->frameRate(static_cast<uint>(100), -1);

  wrapper->exposureTime(exposure);
  wrapper->gain(gain);
  wrapper->enableSoftwareTrigger(0);
  wrapper->enableSoftwareTrigger(1);

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

  auto before = high_resolution_clock::now();
  auto after = high_resolution_clock::now();
  auto after1 = high_resolution_clock::now();
  auto stitch = high_resolution_clock::now();
  auto stitchncopy = high_resolution_clock::now();
  auto grab_duration = duration_cast<microseconds>(after - before);
  auto grab1_duration = duration_cast<microseconds>(after1 - before);
  auto stitch_duration = duration_cast<microseconds>(stitch - after);
  auto stitchncopy_duration = duration_cast<microseconds>(stitchncopy - after);
  auto done = high_resolution_clock::now();
  auto total_duration = duration_cast<microseconds>(done - before);
  int expected_usecs = (int)(1./framerate * 1000000);

  while (cameras.IsGrabbing() && !stop_request && !failure) {
    if (counter > 0) {
      long delay = total_duration.count() - expected_usecs;
      if (delay > 0) {
        emit delayed(delay, counter-1);
      } else {
        // std::cerr << "expected ifi " << expected_usecs << " real ifi: " << total_duration.count() << " waiting for " << (-1*delay) << std::endl;
        usleep(-delay);
      }
    }
    before = high_resolution_clock::now();

    if (cameras[0].WaitForFrameTriggerReady(1000, Pylon::TimeoutHandling_ThrowException) &
        cameras[1].WaitForFrameTriggerReady(1000, Pylon::TimeoutHandling_ThrowException)) {
      // std::cerr << "executing software triggers" << std::endl;
      cameras[0].ExecuteSoftwareTrigger();
      cameras[1].ExecuteSoftwareTrigger();
    }

    try {
      cameras[0].RetrieveResult( 5000, frame0, Pylon::TimeoutHandling_ThrowException );
      after1 = high_resolution_clock::now();
      cameras[1].RetrieveResult( 5000, frame1, Pylon::TimeoutHandling_ThrowException );
      leftImage.AttachGrabResultBuffer( frame0 );
      rightImage.AttachGrabResultBuffer( frame1 );
    } catch( ... ) {
      qDebug() << "Grabbing frame failed! " << e.what();
      failure = true
    }
    after = high_resolution_clock::now();

    if (!failure && leftImage.IsValid() && rightImage.IsValid()) {
      try {
        StitchImage::StitchToRight(leftImage, rightImage, &stitchedImage, errorMessage);
        stitch = high_resolution_clock::now();
        MyImage *img = new MyImage(stitchedImage.GetWidth(), stitchedImage.GetHeight());
        img->setFrame(stitchedImage);
        buffer->push(img);
      } catch(const std::exception& e) {
        std::cerr << e.what() << '\n';
      }
    }

    stitchncopy = high_resolution_clock::now();
    grab_duration = duration_cast<microseconds>(after - before);
    grab1_duration = duration_cast<microseconds>(after1 - before);
    stitch_duration = duration_cast<microseconds>(stitch - after);
    stitchncopy_duration = duration_cast<microseconds>(stitchncopy - after);
    // std::cerr << "framecount: " << counter << " grab1_duration (us): " << grab1_duration.count() << " grab_all (us): " << grab_duration.count() << std::endl;
    // std::cerr << "\tpure stich (us): " << stitch_duration.count()  << " stich'n'copy (us): " << stitchncopy_duration.count() << std::endl;
    done = high_resolution_clock::now();
    total_duration = duration_cast<microseconds>(done - before);
    // double rate = (1./((double)total_duration.count()/1000000));
    // std::cerr << "total_duration (us): " << total_duration.count() << " rate: " << rate << std::endl;
    counter += 1;
  }
  cameras.StopGrabbing();
}