// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#include "ximeaReader.hpp"

#include "libvideostitch/logging.hpp"
#include "libvideostitch/parse.hpp"

#include <cstdint>
#include <sstream>

// GPU//#include <cuda_runtime.h>

using namespace VideoStitch;
using namespace Input;

DualBuffer::DualBuffer(int64_t widthParam, int64_t heightParam)
    : readPtr(0), writePtr(0), size(widthParam * heightParam), empty(true), full(false), stopLoop(false) {
  for (uint8_t i = 0; i < 2; i++) {
    buff[i].second = new unsigned char[size];
    std::memset(buff[i].second, 0, size * sizeof(unsigned char));
  }

  wCond.notify_one();
  wCond.notify_one();
}

DualBuffer::~DualBuffer() {
  for (uint8_t i = 0; i < 2; i++) {
    delete buff[i].second;
  }
}

bool DualBuffer::read(XFrame* data) {
  std::unique_lock<std::mutex> lock(mtx);
  rCond.wait(lock, [this]() { return !empty || stopLoop; });

  if (stopLoop) {
    return false;
  }

  data->first = buff[readPtr].first;
  std::memcpy(data->second, buff[readPtr].second, size);
  if (readPtr == 0) {
    readPtr = 1;
  } else
    readPtr = 0;
  full = false;

  if (readPtr == writePtr) {
    empty = true;
  }

  wCond.notify_one();

  return true;
}

bool DualBuffer::write(XFrame* data) {
  std::unique_lock<std::mutex> lock(mtx);
  wCond.wait(lock, [this]() { return !full || stopLoop; });

  if (stopLoop) {
    return false;
  }

  buff[writePtr].first = data->first;
  std::memcpy(buff[writePtr].second, data->second, size);
  if (writePtr == 0) {
    writePtr = 1;
  } else
    writePtr = 0;
  empty = false;

  if (readPtr == writePtr) {
    full = true;
  }

  rCond.notify_one();

  return true;
}

XimeaReader::XimeaReader(readerid_t id, const int64_t width, const int64_t height, int deviceIndex,
                         const bool withAudio, FrameRate fps, bool interlaced, int bw, int frameRateLimit)
    : Reader(id),
      VideoReader(width, height, width * height, Grayscale, Host, fps,
                  // GPU//3 * width * height, RGB, Device, fps,
                  0, NO_LAST_FRAME, false /* not a procedural reader */, nullptr),
      devIdx(deviceIndex),
      bandwidth(bw),
      fpsLimit(frameRateLimit),
      stop(false),
      dBuff(new DualBuffer(width, height)) {
  thr = new std::thread(&XimeaReader::ximeaThread, this);
}

XimeaReader::~XimeaReader() {
  stop = true;
  dBuff->stop();
  thr->join();
  delete thr;
  thr = nullptr;

  delete dBuff;
  dBuff = nullptr;
}

XimeaReader* XimeaReader::create(readerid_t id, const Ptv::Value* config, const int64_t width, const int64_t height) {
  // const UWord deviceIndex,
  const bool withAudio = false;
  bool interlaced = false;

  int deviceIndex = 0;
  if (Parse::populateInt("Ximea reader", *config, "device", deviceIndex, false) !=
      VideoStitch::Parse::PopulateResult_Ok) {
    Logger::get(Logger::Error) << "[Ximea] Cam device index (\"device\") couldn't be retrieved. Aborting." << std::endl;
    return nullptr;
  }

  FrameRate frameRate;
  if (!config->has("frame_rate")) {
    Logger::get(Logger::Error) << "Frame rate (\"frame_rate\") couldn't be retrieved. Aborting." << std::endl;
    return nullptr;
  } else {
    const Ptv::Value* fpsConf = config->has("frame_rate");
    if ((Parse::populateInt("Ximea reader", *fpsConf, "num", frameRate.num, false) !=
         VideoStitch::Parse::PopulateResult_Ok) ||
        (Parse::populateInt("Ximea reader", *fpsConf, "den", frameRate.den, false) !=
         VideoStitch::Parse::PopulateResult_Ok)) {
      Logger::get(Logger::Error) << "[Ximea] Frame rate (\"frame_rate\") couldn't be retrieved. Aborting." << std::endl;
      return nullptr;
    }
  }

  int bw = 0;
  if (Parse::populateInt("Ximea reader", *config, "bandwidth", bw, false) != VideoStitch::Parse::PopulateResult_Ok) {
    Logger::get(Logger::Warning)
        << "[Ximea] Cam bandwidth limit (\"bandwidth\") couldn't be retrieved. Automatic bandwidth calculus used."
        << std::endl;
  }

  int fpsLim = 0;
  if (Parse::populateInt("Ximea reader", *config, "fps_limit", fpsLim, false) !=
      VideoStitch::Parse::PopulateResult_Ok) {
    Logger::get(Logger::Warning)
        << "[Ximea] Cam framerate limit (\"fps_limit\") couldn't be retrieved. Automatic framerate calculus used."
        << std::endl;
  }

  return new XimeaReader(id, width, height, deviceIndex, withAudio, frameRate, interlaced, bw, fpsLim);
  ;
}

ReadStatus XimeaReader::readFrame(mtime_t& date, unsigned char* video) {
  XFrame tmp;
  std::memset(video, 0, getWidth() * getHeight());
  // GPU//cudaMemset(video, 0, getWidth()*getHeight()*3);

  date = tmp.first;
  tmp.second = video;
  if (!dBuff->read(&tmp)) {
    return ReadStatus::fromCode<ReadStatusCode::EndOfFile>();
  }

  /*GPU
  if(!imgQueue.empty()){
                unsigned char* frame = imgQueue.front();
                imgQueue.pop();
                cudaMemcpy(video, frame, getWidth()*getHeight(), cudaMemcpyDeviceToDevice );
                cudaMemcpy(video, frame, getWidth()*getHeight(), cudaMemcpyDeviceToDevice );
                cudaMemcpy(video, frame, getWidth()*getHeight(), cudaMemcpyDeviceToDevice );

  }*/

  return ReadStatus::OK();
}

Status XimeaReader::seekFrame(frameid_t) { return Status::OK(); }

void XimeaReader::ximeaThread(XimeaReader* XR) {
  HANDLE xHdl;
  XI_IMG xImg;
  int param = 0;

  memset(&xImg, 0, sizeof(xImg));
  xImg.size = sizeof(XI_IMG);
  // GPU//xImg.frm = XI_FRM_TRANSPORT_DATA;
  xImg.bp_size = (DWORD)(XR->getWidth() * XR->getHeight());

  XI_RETURN stat = xiOpenDevice(XR->deviceIndex(), &xHdl);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error opening cam : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }

  // stupid flanders, black image otherwise
  stat = xiSetParamInt(xHdl, XI_PRM_EXPOSURE, 20000);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error setting exposition : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }

  std::stringstream msg;
  msg << "[Ximea] cam  number : " << XR->deviceIndex();  // << std::endl;

  /*
   * GPU
   *//*
stat = xiSetParamInt(xHdl,  XI_PRM_TRANSPORT_DATA_TARGET, XI_TRANSPORT_DATA_TARGET_GPU_RAM);
if(stat != XI_OK) {
	Logger::get(Logger::Error) << "[Ximea] Error setting GPU memory : " << XR->deviceIndex() << " error : " << stat << std::endl;
}

stat = xiSetParamInt(xHdl, XI_PRM_IMAGE_DATA_FORMAT, XI_FRM_TRANSPORT_DATA);
if(stat != XI_OK) {
	Logger::get(Logger::Error) << "[Ximea] Error setting data format : " << XR->deviceIndex() << " error : " << stat << std::endl;
}
*/

  /*
   * FRAMERATE
   */
  if (XR->fpsLim() != 0) {
    stat = xiSetParamInt(xHdl, XI_PRM_ACQ_TIMING_MODE, XI_ACQ_TIMING_MODE_FRAME_RATE_LIMIT);
    if (stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error setting framerate accuracy : " << XR->deviceIndex()
                                 << " error : " << stat << std::endl;
    }

    param = (int)XR->fpsLim();
    stat = xiSetParamInt(xHdl, XI_PRM_FRAMERATE, param);
    if (stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error setting framerate  : " << XR->deviceIndex() << " error : " << stat
                                 << std::endl;
    }
    msg << " Framerate set : " << param;
  } else {
    Logger::get(Logger::Info) << "[Ximea] Using automatic framerate calculation" << std::endl;
  }

  stat = xiGetParamInt(xHdl, XI_PRM_FRAMERATE, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting framerate " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Framerate get : " << param << std::endl;

  /*
   * BANDWIDTH
   */
  int bwMax = 0;
  int bwMin = 0;

  stat = xiGetParamInt(xHdl, XI_PRM_AUTO_BANDWIDTH_CALCULATION, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth calc : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Auto bandwidth calculation : " << param;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH_MODE, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth mode : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth limit mode : " << param;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth limit  : " << param << std::endl;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH XI_PRM_INFO_MIN, &bwMin);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth min : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth min : " << bwMin;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH XI_PRM_INFO_MAX, &bwMax);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth max : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth max : " << bwMax << std::endl;

  Logger::get(Logger::Error) << msg.str();

  if (XR->bw() != 0) {
    if ((XR->bw() > bwMin) && (XR->bw() < bwMax)) {
      stat = xiSetParamInt(xHdl, XI_PRM_AUTO_BANDWIDTH_CALCULATION, XI_OFF);
      if (stat != XI_OK) {
        Logger::get(Logger::Error) << "[Ximea] Error setting bandwidth calc : " << XR->deviceIndex()
                                   << " error : " << stat << std::endl;
      }

      stat = xiSetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH_MODE, XI_ON);
      if (stat != XI_OK) {
        Logger::get(Logger::Error) << "[Ximea] Error setting bandwidth mode : " << XR->deviceIndex()
                                   << " error : " << stat << std::endl;
      }

      stat = xiSetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH, (int)XR->bw());
      if (stat != XI_OK) {
        Logger::get(Logger::Error) << "[Ximea] Error setting bandwidth : " << XR->deviceIndex() << " error : " << stat
                                   << std::endl;
      }
      msg << " bandwidth limit set : " << XR->bw();
    } else {
      Logger::get(Logger::Error) << "[Ximea] Error settings bandwidth, user value outside bond : " << XR->bw()
                                 << " is not between " << bwMin << " and " << bwMax << std::endl;
    }
  } else {
    Logger::get(Logger::Warning) << "[Ximea] Using automatic bandwidth calculation" << std::endl;
  }

  stat = xiGetParamInt(xHdl, XI_PRM_AUTO_BANDWIDTH_CALCULATION, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth calc : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Auto bandwidth calculation : " << param;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH_MODE, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth mode : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth limit mode : " << param;

  stat = xiGetParamInt(xHdl, XI_PRM_LIMIT_BANDWIDTH, &param);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error getting bandwidth : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
  msg << " Bandwidth limit  : " << param << std::endl;

  /*
   * Buffer policy
   *//*
stat = xiSetParamInt(xHdl, XI_PRM_BUFFER_POLICY, XI_BP_SAFE);
if(stat != XI_OK) {
	Logger::get(Logger::Error) << "[Ximea] Error settin buffer policy : " << XR->deviceIndex() << " error : " << stat << std::endl;
}


stat = xiGetParamInt(xHdl, XI_PRM_BUFFER_POLICY, &param);
if(stat != XI_OK) {
	Logger::get(Logger::Error) << "[Ximea] Error getting buffer policy : " << XR->deviceIndex() << " error : " << stat << std::endl;
}
msg << " buffer policy : " << param << std::endl;
*/

  /*
   * Cam sync
   *//*
  if(XR->deviceIndex() == 0){
    stat = xiSetParamInt(xHdl, XI_PRM_GPO_SELECTOR, XI_GPO_PORT1);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error setting GPO for main cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }

    stat = xiSetParamInt(xHdl, XI_PRM_GPO_MODE, XI_GPO_FRAME_ACTIVE);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error set GPO mode for main cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }
  }else{
    stat = xiSetParamInt(xHdl, XI_PRM_GPI_SELECTOR, XI_GPI_PORT1);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error setting GPI for slave cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }

    stat = xiSetParamInt(xHdl, XI_PRM_GPI_MODE, XI_GPI_TRIGGER);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error set GPI mode for slave cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }

    stat = xiSetParamInt(xHdl, XI_PRM_TRG_SOURCE, XI_TRG_EDGE_RISING);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error set trigger source for slave cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }

    stat = xiSetParamInt(xHdl, XI_PRM_TRG_SELECTOR, XI_TRG_SEL_FRAME_START);
    if(stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error set trigger selector for slave cam : " << XR->deviceIndex() << " error : " << stat << std::endl;
    }

  }*/

  Logger::get(Logger::Error) << msg.str();

  std::this_thread::sleep_for(std::chrono::milliseconds(10));  // allow other camera to start

  stat = xiStartAcquisition(xHdl);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error starting acquisition : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }

  /*
   * MAIN LOOP
   */
  XFrame tmpFrame;
  while (!XR->stopThread()) {
    /*GPU
    unsigned char* frameBuffer;
      cudaMalloc((void**)&frameBuffer, getWidth()*getHeight());
    */

    stat = xiGetImage(xHdl, 500, &xImg);
    if (stat != XI_OK) {
      Logger::get(Logger::Error) << "[Ximea] Error getting image from cam : " << XR->deviceIndex()
                                 << " error : " << stat << std::endl;
    } else {
      tmpFrame.first = (mtime_t)xImg.tsSec * 1000000 + (mtime_t)xImg.tsUSec;
      tmpFrame.second = (unsigned char*)xImg.bp;

      XR->dBuff->write(&tmpFrame);

      // cudaMemcpy(frameBuffer, xImg.bp, getWidth()*getHeight(), cudaMemcpyDeviceToDevice );
      // GPU//XR->imgQueue.push(frameBuffer);
    }
  }

  stat = xiStopAcquisition(xHdl);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error stopping acquisition : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }

  stat = xiCloseDevice(xHdl);
  if (stat != XI_OK) {
    Logger::get(Logger::Error) << "[Ximea] Error closing cam : " << XR->deviceIndex() << " error : " << stat
                               << std::endl;
  }
}