#include "frame.h"
#include <iostream>

FILE *make_buffer(const uint8_t *data, const size_t data_len)
{
  FILE *file = fmemopen((void *)data, data_len, "r+");

  if (file == NULL)
  {
    perror("Error opening file");
    return NULL;
  }

  // Seek to the beginning of the file
  rewind(file);

  // Write the data to the file
  fwrite(data, sizeof(uint8_t), data_len, file);

  return file;
}

int read_buffer(FILE *file, uint8_t **buffer, size_t *length)
{
  if (file != NULL)
  {
    // Get the file size
    fseek(file, 0, SEEK_END);
    (*length) = ftell(file);
    fseek(file, 0, SEEK_SET);

    // Allocate memory
    *buffer = (uint8_t *)malloc((*length) * sizeof(char));
    if (buffer == NULL)
    {
      printf("Error: Unable to allocate memory.\n");
      return 1;
    }

    // Read file
    size_t newLen = fread(*buffer, sizeof(char), *length, file);
    if (newLen == 0)
    {
      if (feof(file))
      {
        printf("Error: End of file reached.\n");
        return 2;
      }
      printf("Error: Reading file.\n");
      return 3;
    }
    return 4;
  }
  return 5;
}

Frame *new_frame_from_data(const uint8_t *data, const size_t data_len, int w, int h, int depth)
{
  FILE *buf = make_buffer(data, data_len);
  return new_frame_from_handle(buf, w, h, depth);
}

Frame *new_frame_from_handle(FILE *stream, int w, int h, int depth)
{
  Frame *f = (Frame *)malloc(sizeof(Frame));
  f->stream = stream;
  f->w = w;
  f->h = h;
  f->depth = depth;
  return f;
}

void clean_frame(Frame *f) {
  fclose(f->stream);
  free(f);
}

#ifdef __cplusplus

openMVG::image::Image<u_char> DartOpenMvg::imageFromFrame(const CFrame *frame)
{
  rewind(frame->stream);
  std::vector<unsigned char> imageData;
  if (!openMVG::image::ReadPngStream(frame->stream, &imageData, (int *)&(frame->w), (int *)&(frame->h), (int *)&(frame->depth)))
  {
    throw std::runtime_error("Could not read stream");
  };
  int w = frame->w, h = frame->h;
  Eigen::Matrix<u_char, Eigen::Dynamic, Eigen::Dynamic, 1> eigenMatrix(frame->w, frame->h);
  for (int i = 0; i < w * h; ++i)
  {
    eigenMatrix(i / h, i % h) = imageData[i];
  }

  return openMVG::image::Image<uint8_t>(eigenMatrix);
}

DartOpenMvg::Frames::Frames(openMVG::sfm::Bundle_Adjustment_Ceres::BA_Ceres_options &options)
{
  mAdjustment = openMVG::sfm::Bundle_Adjustment_Ceres(options);
}

void DartOpenMvg::Frames::computeMatches()
{
#ifdef OPENMVG_USE_OPENMP
  OPENMVG_LOG_INFO << "Using the OPENMP thread interface";
  const bool b_multithreaded_pair_search = (eMatcherType_ == CASCADE_HASHING_L2);
  // -> set to true for CASCADE_HASHING_L2, since OpenMP instructions are not used in this matcher
#endif

  // Match the pairs as a sequence
  // Within openMvg::matching_image_collection::Matcher
  // they matcha gainst *ALL* images. However, since we know the order of the
  // image sequences (video frames), we'll stick to the sequence order.
  for (size_t i = 0; i < this->size() - 1; ++i)
  {
    const Frame &frame1 = this->at(i);
    const Frame &frame2 = this->at(i + 1);
    auto *regions1 = frame1.mRegions.get();
    auto *regions2 = frame2.mRegions.get();

    // Initialize the matching interface
    const std::unique_ptr<openMVG::matching::RegionsMatcher> matcher =
        openMVG::matching::RegionMatcherFactory(openMVG::matching::EMatcherType::HNSW_L2, *regions1);
    if (!matcher)
      continue;

#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel for schedule(dynamic) if (b_multithreaded_pair_search)
#endif
    openMVG::matching::IndMatches vec_putative_matches;
    matcher->MatchDistanceRatio(0.5, *regions2, vec_putative_matches);
    if (vec_putative_matches.empty())
    {
      return;
    }
    mFeatureMap[std::make_pair(i, i + 1)] = vec_putative_matches;
  }
};

void DartOpenMvg::Frames::buildTracks()
{
  openMVG::tracks::TracksBuilder trackBuilder;

  openMVG::matching::PairWiseMatches matches;

  trackBuilder.Build(mFeatureMap);
  trackBuilder.Filter();
  trackBuilder.ExportToSTL(mTracks);
}

void DartOpenMvg::Frames::adjust()
{
  const openMVG::sfm::Optimize_Options ba_refine_options(openMVG::cameras::Intrinsic_Parameter_Type::ADJUST_ALL,
                                                         openMVG::sfm::Extrinsic_Parameter_Type::ADJUST_ALL, // Adjust camera motion
                                                         openMVG::sfm::Structure_Parameter_Type::ADJUST_ALL, // Adjust scene structure
                                                         openMVG::sfm::Control_Point_Parameter(),
                                                         false);
  mAdjustment.Adjust(mSfmData, ba_refine_options);
}

void DartOpenMvg::Frame::calculateFeatures()
{
  using namespace openMVG::image;
  using namespace openMVG::features;
  auto desc = SIFT_Anatomy_Image_describer(SIFT_Anatomy_Image_describer::Params());
  auto image = imageFromFrame(cFrame);
  mRegions = desc.Describe(image);
}

void DartOpenMvg::Frames::resection() {
  // openMVG::tracks::TracksUtilsMap::GetFeatIndexPerViewAndTrackId(
  //   mTracks;
  // )
}

openMVG::image::Image<u_char> DartOpenMvg::Frame::getMvgImage()
{
  return imageFromFrame(this->cFrame);
}

#endif // __cplusplus