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

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

DartOpenMvg::Frame::Frame(const DartOpenMvg::Frame &frame)
{
  cFrame = frame.cFrame;
  // mRegions = std::unique_ptr<openMVG::features::Regions>(frame.mRegions.get());
}

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::add_frame(FILE *handle, int w, int h, int depth)
{
  CFrame *f = new_frame_from_handle(handle, w, h, depth);
  mFrames.push_back(
      Frame(f));
}

void DartOpenMvg::Frames::computeMatches()
{

  // 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 < mFrames.size() - 1; ++i)
  {
    const Frame &frame1 = mFrames[i];
    const Frame &frame2 = mFrames[i + 1];
    auto regions1 = frame1.mRegions.get();
    auto regions2 = frame2.mRegions.get();
    if (!regions1)
    {
      std::string msg = std::string("Did you remember to calculate regions for frame ") + std::to_string(i) + "?";
      throw std::out_of_range(msg);
    }

    // regions1->Load();
    std::cerr << "Loading " << std::to_string(regions1->RegionCount()) << " regions" << std::endl;

    auto is_binary = regions1->IsBinary();

    // Initialize the matching interface
    const std::unique_ptr<openMVG::matching::RegionsMatcher> matcher =
        openMVG::matching::RegionMatcherFactory(openMVG::matching::EMatcherType::HNSW_L2, *regions1);
    if (!matcher)
      continue;
    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;
  MvgOverride::AKAZE_OCV_Image_describer desc;
  auto im = imageFromFrame(cFrame);
  mRegions = std::make_shared<Regions>(std::move(desc.Describe(im)));

  if (!mRegions.get()) {
    throw std::range_error("Could not set regions member.");
  }
}

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

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

#endif // __cplusplus