// +-------------------------------- 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 <iostream>
#include <memory>
#include <cassert>
#include <sys/mman.h>
#include <libcamera/libcamera.h>
#include <cstring>


std::vector<uint8_t> CameraHandler::buffer_;
std::mutex CameraHandler::processImageMutex_;
std::condition_variable CameraHandler::canProcessImage_;
std::shared_ptr<libcamera::Camera> CameraHandler::camera_ = nullptr;


CameraHandler::CameraHandler(const std::string&): data_(nullptr), width_(0), height_(0), pitch_(0), stream_(nullptr),
    cameraManager_(nullptr), allocator_(nullptr), requests_(), abort_(false)

{
}

CameraHandler::~CameraHandler()
{
  if (camera_ != nullptr)
  {
    std::cout << "Close camera" << std::endl;
    camera_->stop();
    allocator_->free(stream_);
    delete allocator_;
    camera_->release();
    camera_.reset();
    cameraManager_->stop();
  }
  std::cout << "Destroyed CameraHandler" << std::endl;
}

void CameraHandler::requestComplete(libcamera::Request *request)
{
  if (request->status() == libcamera::Request::RequestCancelled)
    return;

  const libcamera::Request::BufferMap &buffers = request->buffers();
  assert(buffers.size() == 1u);

  auto bufferPair = *(buffers.begin());
  libcamera::FrameBuffer* buffer = bufferPair.second;

  size_t size = buffer->metadata().planes().begin()->bytesused;
  const libcamera::FrameBuffer::Plane &plane = buffer->planes().front();
  void* memory = mmap(NULL, plane.length, PROT_READ, MAP_SHARED, plane.fd.get(), 0);

  processImageMutex_.lock();
  memcpy(buffer_.data(), memory, size);
  canProcessImage_.notify_one();
  processImageMutex_.unlock();

  munmap(memory, plane.length);

  /* Re-queue the Request to the camera. */
  request->reuse(libcamera::Request::ReuseBuffers);
  camera_->queueRequest(request);
}

void CameraHandler::launchCamera()
{
  try
  {
    // adapted from https://github.com/kbingham/simple-cam/blob/master/simple-cam.cpp

    /*
    * --------------------------------------------------------------------
    * Create a Camera Manager.
    *
    * The Camera Manager is responsible for enumerating all the Camera
    * in the system, by associating Pipeline Handlers with media entities
    * registered in the system.
    *
    * The CameraManager provides a list of available Cameras that
    * applications can operate on.
    *
    * When the CameraManager is no longer to be used, it should be deleted.
    * We use a unique_ptr here to manage the lifetime automatically during
    * the scope of this function.
    *
    * There can only be a single CameraManager constructed within any
    * process space.
    */
    cameraManager_ = std::make_unique<libcamera::CameraManager>();
    cameraManager_->start();

    /*
    * --------------------------------------------------------------------
    * Camera
    *
    * Camera are entities created by pipeline handlers, inspecting the
    * entities registered in the system and reported to applications
    * by the CameraManager.
    *
    * In general terms, a Camera corresponds to a single image source
    * available in the system, such as an image sensor.
    *
    * Application lock usage of Camera by 'acquiring' them.
    * Once done with it, application shall similarly 'release' the Camera.
    *
    * As an example, use the first available camera in the system after
    * making sure that at least one camera is available.
    *
    * Cameras can be obtained by their ID or their index, to demonstrate
    * this, the following code gets the ID of the first camera; then gets
    * the camera associated with that ID (which is of course the same as
    * cm->cameras()[0]).
    */
    if (cameraManager_->cameras().empty())
    {
      std::cerr << "No cameras were identified on the system." << std::endl;
      cameraManager_->stop();
      abort_ = true;
      return;
    }

    std::string cameraId = cameraManager_->cameras()[0]->id();
    camera_ = cameraManager_->get(cameraId);
    camera_->acquire();

    /*
    * Stream
    *
    * Each Camera supports a variable number of Stream. A Stream is
    * produced by processing data produced by an image source, usually
    * by an ISP.
    *
    *   +-------------------------------------------------------+
    *   | Camera                                                |
    *   |                +-----------+                          |
    *   | +--------+     |           |------> [  Main output  ] |
    *   | | Image  |     |           |                          |
    *   | |        |---->|    ISP    |------> [   Viewfinder  ] |
    *   | | Source |     |           |                          |
    *   | +--------+     |           |------> [ Still Capture ] |
    *   |                +-----------+                          |
    *   +-------------------------------------------------------+
    *
    * The number and capabilities of the Stream in a Camera are
    * a platform dependent property, and it's the pipeline handler
    * implementation that has the responsibility of correctly
    * report them.
    */

    /*
    * --------------------------------------------------------------------
    * Camera Configuration.
    *
    * Camera configuration is tricky! It boils down to assign resources
    * of the system (such as DMA engines, scalers, format converters) to
    * the different image streams an application has requested.
    *
    * Depending on the system characteristics, some combinations of
    * sizes, formats and stream usages might or might not be possible.
    *
    * A Camera produces a CameraConfigration based on a set of intended
    * roles for each Stream the application requires.
    */
    std::unique_ptr<libcamera::CameraConfiguration> config = camera_->generateConfiguration( { libcamera::StreamRole::Viewfinder } );

    /*
    * The CameraConfiguration contains a StreamConfiguration instance
    * for each StreamRole requested by the application, provided
    * the Camera can support all of them.
    *
    * Each StreamConfiguration has default size and format, assigned
    * by the Camera depending on the Role the application has requested.
    */
    libcamera::StreamConfiguration &streamConfig = config->at(0);
    std::cout << "Default viewfinder configuration is: " << streamConfig.toString()
              << ", buffer count is " << streamConfig.bufferCount << ", frame size is " << streamConfig.frameSize
              << ", stride is " << streamConfig.stride << std::endl;

    /*
    * Each StreamConfiguration parameter which is part of a
    * CameraConfiguration can be independently modified by the
    * application.
    *
    * In order to validate the modified parameter, the CameraConfiguration
    * should be validated -before- the CameraConfiguration gets applied
    * to the Camera.
    *
    * The CameraConfiguration validation process adjusts each
    * StreamConfiguration to a valid value.
    */
    streamConfig.size.width = 800;
    streamConfig.size.height = 600;
    streamConfig.pixelFormat = libcamera::PixelFormat(libcamera::formats::RGB888);

    /*
    * Validating a CameraConfiguration -before- applying it will adjust it
    * to a valid configuration which is as close as possible to the one
    * requested.
    */
    config->validate();

    if (streamConfig.pixelFormat != libcamera::formats::RGB888)
    {
      // for any format whose first plane is a grayscale 8bit image, you can simply modify the "if" above
      // other formats will require changes in the requestComplete method
      std::cerr << "Only the BGR888 format is supported by the sample" << std::endl;
      abort_ = true;
      return;
    }

    std::cout << "Validated viewfinder configuration is: " << streamConfig.toString()
              << ", buffer count is " << streamConfig.bufferCount << ", frame size is " << streamConfig.frameSize
              << ", stride is " << streamConfig.stride << "  " << std::endl;

    // allocate memory
    buffer_.resize(streamConfig.stride * streamConfig.size.height);
    data_ = buffer_.data();
    width_ = streamConfig.size.width;
    height_ = streamConfig.size.height;
    pitch_ = streamConfig.stride;

    /*
    * Once we have a validated configuration, we can apply it to the
    * Camera.
    */
    camera_->configure(config.get());

    /*
    * --------------------------------------------------------------------
    * Buffer Allocation
    *
    * Now that a camera has been configured, it knows all about its
    * Streams sizes and formats. The captured images need to be stored in
    * framebuffers which can either be provided by the application to the
    * library, or allocated in the Camera and exposed to the application
    * by libcamera.
    *
    * An application may decide to allocate framebuffers from elsewhere,
    * for example in memory allocated by the display driver that will
    * render the captured frames. The application will provide them to
    * libcamera by constructing FrameBuffer instances to capture images
    * directly into.
    *
    * Alternatively libcamera can help the application by exporting
    * buffers allocated in the Camera using a FrameBufferAllocator
    * instance and referencing a configured Camera to determine the
    * appropriate buffer size and types to create.
    */
    allocator_ = new libcamera::FrameBufferAllocator(camera_);

    for (libcamera::StreamConfiguration &cfg : *config)
    {
      if (allocator_->allocate(cfg.stream()) < 0)
      {
          std::cerr << "Can't allocate buffers" << std::endl;
          abort_ = true;
          return;
      }

      size_t allocated = allocator_->buffers(cfg.stream()).size();
      std::cout << "Allocated " << allocated << " buffers for stream" << std::endl;
    }

    /*
    * --------------------------------------------------------------------
    * Frame Capture
    *
    * libcamera frames capture model is based on the 'Request' concept.
    * For each frame a Request has to be queued to the Camera.
    *
    * A Request refers to (at least one) Stream for which a Buffer that
    * will be filled with image data shall be added to the Request.
    *
    * A Request is associated with a list of Controls, which are tunable
    * parameters (similar to v4l2_controls) that have to be applied to
    * the image.
    *
    * Once a request completes, all its buffers will contain image data
    * that applications can access and for each of them a list of metadata
    * properties that reports the capture parameters applied to the image.
    */
    stream_ = streamConfig.stream();
    const std::vector<std::unique_ptr<libcamera::FrameBuffer>>& buffers = allocator_->buffers(stream_);
    for (unsigned int i = 0; i < buffers.size(); ++i)
    {
      std::unique_ptr<libcamera::Request> request = camera_->createRequest();
      if (!request)
      {
        std::cerr << "Can't create request" << std::endl;
        abort_ = true;
        return;
      }

      const std::unique_ptr<libcamera::FrameBuffer> &buffer = buffers[i];
      int ret = request->addBuffer(stream_, buffer.get());
      if (ret < 0)
      {
        std::cerr << "Can't set buffer for request" << std::endl;
        abort_ = true;
        return;
      }

      requests_.push_back(std::move(request));
    }

    /*
    * --------------------------------------------------------------------
    * Signal&Slots
    *
    * libcamera uses a Signal&Slot based system to connect events to
    * callback operations meant to handle them, inspired by the QT graphic
    * toolkit.
    *
    * Signals are events 'emitted' by a class instance.
    * Slots are callbacks that can be 'connected' to a Signal.
    *
    * A Camera exposes Signals, to report the completion of a Request and
    * the completion of a Buffer part of a Request to support partial
    * Request completions.
    *
    * In order to receive the notification for request completions,
    * applications shall connecte a Slot to the Camera 'requestCompleted'
    * Signal before the camera is started.
    */
    camera_->requestCompleted.connect(CameraHandler::requestComplete);

    /*
    * --------------------------------------------------------------------
    * Start Capture
    *
    * In order to capture frames the Camera has to be started and
    * Request queued to it. Enough Request to fill the Camera pipeline
    * depth have to be queued before the Camera start delivering frames.
    *
    * For each delivered frame, the Slot connected to the
    * Camera::requestCompleted Signal is called.
    */
    camera_->start();
    for (std::unique_ptr<libcamera::Request> &request : requests_)
      camera_->queueRequest(request.get());
  }
  catch (...)
  {
    std::cerr << "Unexpected error" << std::endl;
    abort_ = true;
  }
}
