// +-------------------------------- 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 <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>

#include "Easy.h"
#include "EasyImage.h"
using namespace Euresys::Open_eVision;


static const char* const deviceName = "/dev/video0";

// this part of the sample was tested on a jetson platform with a raspberry pi v2 camera.
// In this setting, using VIDIOC_S_FMT did not change the width/height of the video
// Solving this problem requires to modify the sensor_mode proprety of the camera.
// You can find out which sensor_mode corresponds to what width/height by using:
// v4l2-ctl --set-fmt-video=width=DESIRED_WIDTH,height=DESIRED_HEIGHT,pixelformat=DESIRED_PIXELFORMAT --set-ctrl=sensor_mode=SENSOR_MODE --verbose
// DESIRED_WIDTH, DESIRED_HEIGHT, DESIRED_PIXELFORMAT should be displayed by v4l2-ctl --list-formats-ext
// This might not be the case depending on the version of the OS or some other package.
#define SET_SENSOR_MODE_ON_JETSON

#ifdef SET_SENSOR_MODE_ON_JETSON
  #define TEGRA_CAMERA_CID_BASE (V4L2_CTRL_CLASS_CAMERA | 0x2000)
  #define TEGRA_CAMERA_CID_VI_BYPASS_MODE (TEGRA_CAMERA_CID_BASE+100)
  #define TEGRA_CAMERA_CID_SENSOR_MODE_ID (TEGRA_CAMERA_CID_BASE+8)
  #define TEGRA_SENSOR_MODE_ID 4
#endif

static int xioctl(int fh, int request, void *arg)
{
  int r;
  do
    r = ioctl(fh, request, arg);
  while (r == -1 && errno == EINTR);

  return r;
}

CameraHandler::CameraHandler(const std::string&): cameraFileDescriptor_(-1), cameraBuffers_(), abort_(false),
    buffer_(new EImageC24()), data_(nullptr), width_(0), height_(0), pitch_(0), cameraPitch_(0),
    processImageMutex_(), canProcessImage_(), tmpBayer8Bits_(new EImageBW8())
{}

CameraHandler::~CameraHandler()
{
  std::cout << "Close camera" << std::endl;
  if (cameraFileDescriptor_ != -1 && cameraThread_.joinable())
  {
    abort_ = true;
    cameraThread_.join();
    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(cameraFileDescriptor_, VIDIOC_STREAMOFF, &type) == -1)
    {
      std::cerr << "VIDIOC_STREAMOFF error: " << errno << ", " << strerror(errno) << std::endl;
      return;
    }

    for (struct buffer buffer: cameraBuffers_)
      if (munmap(buffer.start, buffer.length) == -1)
      {
        std::cerr << "munmap error: " << errno << ", " << strerror(errno) << std::endl;
        return;
      }

    if (close(cameraFileDescriptor_) == -1)
    {
      std::cerr << "close error: " << errno << ", " << strerror(errno) << std::endl;
      return;
    }
  }
}

void CameraHandler::launchCamera()
{
  // adapted from https://gist.github.com/maxlapshin/1253534
  try
  {
    struct stat st;
    if (stat(deviceName, &st) == -1)
    {
      std::cerr << "Cannot identify '" << deviceName << "': " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }

    if (!S_ISCHR(st.st_mode))
    {
      std::cerr << deviceName << " is no device" << std::endl;
      abort_ = true;
      return;
    }

    cameraFileDescriptor_ = open(deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);

    if (cameraFileDescriptor_ == -1)
    {
      std::cerr << "Cannot open '" << deviceName << "': " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
    }

    struct v4l2_capability cap;
    if (xioctl(cameraFileDescriptor_, VIDIOC_QUERYCAP, &cap) == -1)
    {
      if (errno == EINVAL)
        std::cerr << deviceName << " is no V4L2 device" << std::endl;
      else
        std::cerr << "VIDIOC_QUERYCAP error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
    {
      std::cerr << deviceName << " is no video capture device" << std::endl;
      abort_ = true;
      return;
    }

    if (!(cap.capabilities & V4L2_CAP_STREAMING))
    {
      std::cerr << deviceName << " does not support streaming i/o" << std::endl;
      abort_ = true;
      return;
    }

    /* Select video input, video standard and tune here. */
    struct v4l2_cropcap cropcap;
    memset(&(cropcap), 0, sizeof(cropcap));

    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (xioctl(cameraFileDescriptor_, VIDIOC_CROPCAP, &cropcap) == 0)
    {
      struct v4l2_crop crop;
      crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      crop.c = cropcap.defrect; /* reset to default */

      if (xioctl(cameraFileDescriptor_, VIDIOC_S_CROP, &crop) == -1)
      {
        switch (errno)
        {
        case EINVAL:
          /* Cropping not supported. */
          break;
        default:
          /* Errors ignored. */
          break;
        }
      }
    }
    /* else Errors ignored. */

#ifdef SET_SENSOR_MODE_ON_JETSON
    std::cout << "If you are not running on a jetson, please comment SET_SENSOR_MODE_ON_JETSON in CameraHandler.cpp" << std::endl;
    v4l2_ext_controls ctrls;
    v4l2_ext_control ctrl;

    memset(&ctrls, 0, sizeof(ctrls));
    memset(&ctrl, 0, sizeof(ctrl));

    ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(TEGRA_CAMERA_CID_SENSOR_MODE_ID);
    ctrls.count = 1;
    ctrls.controls = &ctrl;

    ctrl.id = TEGRA_CAMERA_CID_SENSOR_MODE_ID;
    ctrl.value64 = TEGRA_SENSOR_MODE_ID;

    if (xioctl(cameraFileDescriptor_, VIDIOC_S_EXT_CTRLS, &ctrls) == -1)
    {
      std::cerr << "could not modify sensor mode" << std::endl;
      abort_ = true;
      return;
    }
#endif

    struct v4l2_format fmt;
    memset(&(fmt), 0, sizeof(fmt));

    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    // we set the format to this valu because the camera we use (Raspberry-Pi camera v2.1 only supports this format)
    // we will then have to  manually convert it to grayscale, if your camera supports grayscale, it is better to use it directly
    fmt.fmt.pix.width       = 1280;
    fmt.fmt.pix.height      = 720;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SRGGB10;
    fmt.fmt.pix.field       = V4L2_FIELD_ANY;
    std::cout << "try to set format to: " << fmt.fmt.pix.width << "*" << fmt.fmt.pix.height << " " << fmt.fmt.pix.pixelformat << std::endl;

    if (xioctl(cameraFileDescriptor_, VIDIOC_S_FMT, &fmt) == -1)
    {
      std::cerr << "selected format not supported by camera, please try another one" << std::endl;
      abort_ = true;
      return;
    }

    if (xioctl(cameraFileDescriptor_, VIDIOC_G_FMT, &fmt) == -1)
    {
      std::cerr << "VIDIOC_G_FMT, error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }

    std::cout << "format is: " << fmt.fmt.pix.width << "*" << fmt.fmt.pix.height << " " << fmt.fmt.pix.pixelformat << std::endl;

    /* Buggy driver paranoia. */
    unsigned int min = fmt.fmt.pix.width * 2;
    if (fmt.fmt.pix.bytesperline < min)
      fmt.fmt.pix.bytesperline = min;
    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
    if (fmt.fmt.pix.sizeimage < min)
      fmt.fmt.pix.sizeimage = min;

    width_ = fmt.fmt.pix.width;
    height_ = fmt.fmt.pix.height;
    cameraPitch_ = fmt.fmt.pix.bytesperline;
    tmpBayer8Bits_->SetSize(width_, height_);
    buffer_->SetSize(width_, height_);
    pitch_ = buffer_->GetRowPitch();
    data_ = (uint8_t*) buffer_->GetImagePtr();

    // init_mmap
    struct v4l2_requestbuffers req;
    memset(&(req), 0, sizeof(req));
    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (xioctl(cameraFileDescriptor_, VIDIOC_REQBUFS, &req) == -1)
    {
      if (errno == EINVAL)
        std::cerr << deviceName << "does not support memory mapping" << std::endl;
      else
        std::cerr << "VIDIOC_REQBUFS, error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }

    if (req.count < 2)
    {
      std::cerr << "Insufficient buffer memory on " << deviceName << std::endl;
      abort_ = true;
      return;
    }

    cameraBuffers_.resize(req.count);

    unsigned int numberOfBuffers = 0;
    for (; numberOfBuffers < req.count; ++numberOfBuffers)
    {
      struct v4l2_buffer buf;
      memset(&(buf), 0, sizeof(buf));
      buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory      = V4L2_MEMORY_MMAP;
      buf.index       = numberOfBuffers;

      if (xioctl(cameraFileDescriptor_, VIDIOC_QUERYBUF, &buf) == -1)
      {
        std::cerr << "VIDIOC_QUERYBUF, error: " << errno << ", " << strerror(errno) << std::endl;
        abort_ = true;
        return;
      }

      cameraBuffers_[numberOfBuffers].length = buf.length;
      cameraBuffers_[numberOfBuffers].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
                          MAP_SHARED /* recommended */, cameraFileDescriptor_, buf.m.offset);

      if (cameraBuffers_[numberOfBuffers].start == MAP_FAILED)
      {
        std::cerr << "mmap error: " << errno << ", " << strerror(errno) << std::endl;
        abort_ = true;
        return;
      }
    }

    //start capturing
    enum v4l2_buf_type type;
    unsigned int i;
    for (i = 0; i < numberOfBuffers; ++i)
    {
      struct v4l2_buffer buf;
      memset(&(buf), 0, sizeof(buf));
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index = i;

      if (xioctl(cameraFileDescriptor_, VIDIOC_QBUF, &buf) == -1)
      {
        std::cerr << "VIDIOC_QBUF, error: " << errno << ", " << strerror(errno) << std::endl;
        abort_ = true;
        return;
      }
    }
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(cameraFileDescriptor_, VIDIOC_STREAMON, &type) == -1)
    {
      std::cerr << "VIDIOC_STREAMON, error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }

    // run main loop in a separate thread
    cameraThread_ = std::thread(&CameraHandler::run, this);
  }
  catch (EException e)
  {
    std::cerr << "EException caught: " << e.what() << std::endl;
    abort_ = true;
  }
}

void CameraHandler::run()
{
  while (!abort_)
  {
    fd_set fds;
    struct timeval tv;
    int r;

    FD_ZERO(&fds);
    FD_SET(cameraFileDescriptor_, &fds);

    /* Timeout. */
    tv.tv_sec = 5;
    tv.tv_usec = 0;

    r = select(cameraFileDescriptor_ + 1, &fds, NULL, NULL, &tv);

    if (r == -1)
    {
      if (errno == EINTR)
        continue;
      std::cerr << "select, error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }
    else if (r == 0)
    {
      std::cerr << "select timeout" << std::endl;
      abort_ = true;
      return;
    }

    struct v4l2_buffer buf;
    memset(&(buf), 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    unsigned int i;

    if (xioctl(cameraFileDescriptor_, VIDIOC_DQBUF, &buf) == -1)
    {
      if (errno == EAGAIN)
        continue;
      else
      {
        std::cerr << "VIDIOC_DQBUF, error: " << errno << ", " << strerror(errno) << std::endl;
        abort_ = true;
        return;
      }
    }

    // converts RG10 to RG8
    uint8_t* dstRow = (uint8_t*)tmpBayer8Bits_->GetImagePtr();
    uint8_t* srcRow = (uint8_t*)cameraBuffers_[buf.index].start;
    int dstPitch = tmpBayer8Bits_->GetRowPitch();
    for(int i = 0; i < height_; i++)
    {
      for(int j = 0; j < width_; j++)
      {
        dstRow[j] = (srcRow[2*j] >> 2) | (srcRow[2*j+1] << 6);
      }
      dstRow += dstPitch;
      srcRow += cameraPitch_;
    }

    processImageMutex_.lock();
    EasyColor::BayerToC24(tmpBayer8Bits_.get(), buffer_.get(), EBayerConfiguration_RG);  // converts RG8 to C24
    canProcessImage_.notify_one();
    processImageMutex_.unlock();

    if (xioctl(cameraFileDescriptor_, VIDIOC_QBUF, &buf) == -1)
    {
      std::cerr << "VIDIOC_QBUF, error: " << errno << ", " << strerror(errno) << std::endl;
      abort_ = true;
      return;
    }
  }
}
