// +-------------------------------- 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 "Easy.h"
using namespace Euresys::Open_eVision;

extern "C"
{
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
#include <libavutil/version.h>
#include <libswscale/swscale.h>
}

// You need to change this value
#define DEFAULT_VIDEO_FILE "file:/path/to/videofile.mp4"

CameraHandler::CameraHandler(const std::string& params): data_(nullptr), width_(0),
  height_(0), pitch_(0), abort_(false)
{
  img_ = std::make_unique<EImageC24>();

  if (params.empty())
    videoFile_ = DEFAULT_VIDEO_FILE;
  else
    videoFile_ = params;
}

CameraHandler::~CameraHandler()
{
}

void CameraHandler::launchCamera()
{
  try
  {
    thread_ = std::thread(&CameraHandler::run, this);
  }
  catch (...) {
    std::cerr << "Unexpected error." << std::endl;
    abort_ = true;
  }
}

void CameraHandler::run()
{
  // FFMpeg context variables
  AVFormatContext* formatContext = nullptr;
  AVCodec* codec = nullptr;
  AVCodecContext* codecCtx = nullptr;
  AVFrame *frame = nullptr;
  AVPacket* pkt = nullptr;
  AVStream* stream = nullptr;
  SwsContext* swsCtx = nullptr;

  double frameDuration = 0.0;
  int ret = 0;

  if ((ret = avformat_open_input(&formatContext, videoFile_.c_str(), NULL, NULL)) < 0)
  {
    std::cout << "Could not open file '" << videoFile_ << "'." << std::endl;
    std::cout << "The video can be specified as the second command line argument or by changing the define DEFAULT_VIDEO_FILE." << std::endl;
    abort_ = true;
    return;
  }

  if ((ret = avformat_find_stream_info(formatContext, NULL)) < 0)
  {
    std::cout << "Cannot find stream information" << std::endl;
    return;
  }

#if LIBAVFORMAT_VERSION_INT >= AV_VERSION_INT(59, 0, 100)
  int streamIndex = av_find_best_stream(formatContext, AVMEDIA_TYPE_VIDEO, -1, -1, const_cast<const AVCodec**>(&codec), 0);
#else
  int streamIndex = av_find_best_stream(formatContext, AVMEDIA_TYPE_VIDEO, -1, -1, &codec, 0);
#endif

  if (streamIndex < 0)
  {
    std::cout << "Could not find a video stream or a decoder for the video stream." << std::endl;
    abort_ = true;
    return;
  }

  stream = formatContext->streams[streamIndex];

  codecCtx = avcodec_alloc_context3(codec);
  if (!codecCtx)
  {
    std::cout << "Could not allocate the codec context." << std::endl;
    abort_ = true;
    return;
  }

  ret = avcodec_parameters_to_context(codecCtx, stream->codecpar);
  if (ret < 0)
  {
    std::cout << "Failed to copy decoder parameters to input decoder context." << std::endl;
    abort_ = true;
    return;
  }

  codecCtx->framerate = av_guess_frame_rate(formatContext, stream, NULL);
  ret = avcodec_open2(codecCtx, codec, NULL);
  if (ret < 0)
  {
    std::cout << "Failed to open decoder." << std::endl;
    abort_ = true;
    return;
  }

  img_ = std::make_unique<EImageC24>(codecCtx->width, codecCtx->height);
  width_ = codecCtx->width;
  height_ = codecCtx->height;
  pitch_ = img_->GetRowPitch();
  data_ = static_cast<uint8_t*>(img_->GetImagePtr(0, 0));

  swsCtx = sws_getContext(codecCtx->width, codecCtx->height, codecCtx->pix_fmt, codecCtx->width, codecCtx->height, AV_PIX_FMT_BGR24, 0, nullptr, nullptr, nullptr);
  if (!swsCtx)
  {
    std::cout << "Failed to instanciate a converter to BGR." << std::endl;
    abort_ = true;
    return;
  }

  AVRational framerate = av_guess_frame_rate(formatContext, stream, NULL);
  double framerateDouble = av_q2d(framerate);
  frameDuration = 1.0 / framerateDouble;

  std::cout << "Emulating a camera with the video file '" << videoFile_ << "'..." << std::endl;
  std::cout << "- Image resolution: " << width_ << "x" << height_ << std::endl;
  std::cout << "- FPS: " << framerateDouble << std::endl;
  std::cout << "- Frame duration: " << 1000 * frameDuration  << "ms" << std::endl;

  // Allocate a frame and a packet.
  frame = av_frame_alloc();
  pkt = av_packet_alloc();

  // Timing stuff
  std::chrono::time_point<std::chrono::steady_clock> startTimeStamp;
  std::chrono::time_point<std::chrono::steady_clock> lastTimeStamp;
  int numFrames = 0;

  while (!abort_)
  {
    ret = av_read_frame(formatContext, pkt);
    if (ret == AVERROR_EOF)
    {
      std::cout << "=== End of video. Seeking back to the start... ===" << std::endl;
      av_seek_frame(formatContext, -1, 0, 0);
      startTimeStamp = std::chrono::time_point<std::chrono::steady_clock>();
      lastTimeStamp = std::chrono::time_point<std::chrono::steady_clock>();
      numFrames = 0;
      continue;
    }
    else if (ret < 0)
    {
      std::cout << "Failed to read a frame." << std::endl;
      abort_ = true;
      break;
    }

    // If we are decoding things too slowly, skip frame
    if (numFrames > 0 && std::chrono::duration<double>(std::chrono::steady_clock::now() - startTimeStamp).count() > frameDuration * numFrames)
    {
      // Skip frame.
      numFrames += 1;
      continue;
    }

    ret = avcodec_send_packet(codecCtx, pkt);
    if (ret < 0)
    {
      std::cout << "Failed to decode a frame" << std::endl;
      abort_ = true;
      break;
    }

    ret = avcodec_receive_frame(codecCtx, frame);
    if (ret < 0)
    {
      continue;
    }

    int stride = static_cast<int>(pitch_);
    uint8_t* dst[] = {data_};

    processImageMutex_.lock();
    sws_scale(swsCtx, frame->data, frame->linesize, 0, frame->height, &data_, &stride);

    // Sync the current frame w.r.t. the framerate
    auto t = std::chrono::steady_clock::now();
    auto timeSinceLastFrame = std::chrono::duration<double>(t - lastTimeStamp).count();
    auto timeSinceStart = std::chrono::duration<double>(t - startTimeStamp).count();

    if (timeSinceLastFrame < frameDuration || timeSinceStart < numFrames * frameDuration)
    {
      std::chrono::duration<double> sleepFor(std::min(frameDuration - timeSinceLastFrame, numFrames * frameDuration - timeSinceStart));
      std::this_thread::sleep_for(sleepFor);
    }

    canProcessImage_.notify_all();
    processImageMutex_.unlock();

    t = std::chrono::steady_clock::now();
    if (numFrames == 0)
      startTimeStamp = t;

    lastTimeStamp = t;
    numFrames += 1;

#ifndef NDEBUG
    printf("Decoded %d frames at %f fps\n", numFrames, numFrames / std::chrono::duration<double>(t - startTimeStamp).count());
#endif

    av_packet_unref(pkt);
  }

  av_packet_free(&pkt);
  avformat_close_input(&formatContext);
  avcodec_free_context(&codecCtx);
  av_frame_free(&frame);
  sws_freeContext(swsCtx);
}
