// +-------------------------------- DISCLAIMER ---------------------------------+
// |                                                                             |
// | This program is provided to you free of charge "as such".                   |
// | You should not assume that this program is error-free or suitable for any   |
// | purpose whatsoever.                                                         |
// |                                                                             |
// | EURESYS does not give any representation, warranty or undertaking that this |
// | program is free of any defect or error or suitable for any purpose. EURESYS |
// | shall not be liable, in contract, in torts or otherwise, for any damages,   |
// | loss, costs, expenses or other claims for compensation, including those     |
// | asserted by third parties, arising out of or in connection with the use of  |
// | this program.                                                               |
// +-----------------------------------------------------------------------------+

#include "CameraHandler.h"
#include <pylon/PylonIncludes.h>
#include "EasyColor.h"
using namespace Euresys::Open_eVision;


CameraHandler::CameraHandler(): data_(nullptr), width_(0),
  height_(0), pitch_(0), image_(new EImageBW8()), abort_(false)
{}

CameraHandler::~CameraHandler()
{
  std::cout << "Close camera" << std::endl;
  if (cameraThread_.joinable())
  {
    abort_ = true;
    // waits for ProcessingThread::run to end
    cameraThread_.join();
  }
}

void CameraHandler::launchCamera()
{
  try
  {
    // everything must be done in the thread because of PylonAutoInitTerm
    cameraThread_ = std::thread(&CameraHandler::run, this);
  }
  catch (const EException &e) {
    std::cerr << "Unexpected Open eVision error: " << e.what() << std::endl;
    abort_ = true;
  }
  catch (...) {
    std::cerr << "Unexpected error" << std::endl;
    abort_ = true;
  }
}

void CameraHandler::run()
{
  Pylon::PylonAutoInitTerm autoInitTerm;

  // we chose to use the generic CInstantCamera for portability but you can have a finer control
  // over your camera with other classes such as CBaslerGigECamera or CBaslerUsbCamera
  Pylon::CInstantCamera mycamera(Pylon::CTlFactory::GetInstance().CreateFirstDevice());
  mycamera.Open();

  Pylon::CImageFormatConverter formatConverter;
  formatConverter.OutputPixelFormat = Pylon::PixelType_BGR8packed;

  Pylon::CPylonImage pylonImage;

  mycamera.StartGrabbing(Pylon::GrabStrategy_LatestImageOnly);

  Pylon::CGrabResultPtr ptrGrabResult;

  while(mycamera.IsGrabbing() && !abort_)
  {
    mycamera.RetrieveResult(5000, ptrGrabResult, Pylon::TimeoutHandling_ThrowException);

    if (ptrGrabResult->GrabSucceeded())
    {
      formatConverter.Convert(pylonImage, ptrGrabResult);

      if (width_ == 0)  // when using the pylon generic camera type, the width and height aren't avaiable
        // before here
      {
        width_ = pylonImage.GetWidth();
        height_ = pylonImage.GetHeight();
        image_->SetSize(width_, height_);
        pitch_ = image_->GetRowPitch();
        data_ = (uint8_t*)image_->GetImagePtr();
      }

      EImageC24 colorImage;
      colorImage.UseExternalBuffer(width_, height_, pylonImage.GetBuffer(),
                             (width_ * 3 + pylonImage.GetPaddingX()));

      EColorLookup lookup;
      lookup.ConvertFromRgb(EColorSystem_Lsh);

      processImageMutex_.lock();
      EasyColor::GetComponent(&colorImage, image_.get(), 0, &lookup);
      canProcessImage_.notify_one();
      processImageMutex_.unlock();
    }
  }
  abort_ = true;
}

void CameraHandler::stop()
{
  abort_ = true;
  canProcessImage_.notify_all();
}

void CameraHandler::getImage(EImageBW8& image)
{
  std::unique_lock<std::mutex> lock(processImageMutex_);
  canProcessImage_.wait(lock);
  image.InitializeFromUnalignedBuffer(width_, height_, data_, pitch_);
}

bool CameraHandler::isAborted()
{
  return abort_;
}

