#include "slamdunk/include/camera_tracker.h"
#include "slamdunk/include/pretty_printer.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/timer/timer.hpp>

#include <android/log.h>

#include <time.h>

#define  LOG_TAG2	"slamdunk_app"
#define  LOGI2(...)	__android_log_print(ANDROID_LOG_INFO, LOG_TAG2, __VA_ARGS__)

/////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::signalMovedFrames(const VertexPoseSet& vset)
{
  // update the tree
  if(vset.empty())
    return;

  bool ok = true;
  for(VertexPoseSet::const_iterator it = vset.begin(); it != vset.end(); ++it)
    ok &= m_tree.update(*it, Eigen::Vector2d((*it)->estimate().translation().x(), (*it)->estimate().translation().z()));

  if(!ok && m_verbose)
    std::cout << SLAM_DUNK_ERROR_STR("At least one frame has moved out of quad tree's bounds") << std::endl;

  // update the feature matcher
  calcActiveWindow();
}

/////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::setFrames(const VertexPoseSet& vset)
{
  m_tree.clear();
  if(vset.empty())
  {
    m_reference_vxs.clear();
    return;
  }

  bool ok = true;
  for(VertexPoseSet::const_iterator it = vset.begin(); it != vset.end(); ++it)
    ok &= m_tree.insert(*it, Eigen::Vector2d((*it)->estimate().translation().x(), (*it)->estimate().translation().z()));

  if(!ok && m_verbose)
    std::cout << SLAM_DUNK_ERROR_STR("At least one frame is out of quad tree's bounds") << std::endl;

  // update the feature matcher
  calcActiveWindow();
}

/////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::calcActiveWindow()
{
  boost::timer::cpu_timer stopwatch;

  QuadTreeType::ElementList elems;
  m_tree.query( m_last_tracked.m_pose.translation().x() - m_half_active_win_length,
                m_last_tracked.m_pose.translation().z() - m_half_active_win_length,
                2.*m_half_active_win_length, 2.*m_half_active_win_length, elems);
  m_active_win_center = Eigen::Vector2d(m_last_tracked.m_pose.translation().x(), m_last_tracked.m_pose.translation().z());
  m_active_win_pose = m_last_tracked.m_pose.cast<double>();

  m_reference_vxs.clear();
  std::vector<cv::Mat> frame_descriptors;

  Eigen::Vector4f homv(0,0,0,1);
  for(QuadTreeType::ElementList::const_iterator it = elems.begin(); it != elems.end(); ++it)
    //if(m_last_tracked.m_pose.rotation().col(2).dot(it->m_data->estimate().rotation().col(2)) > m_min_pov_cosangle_th)
    //{
      //m_reference_vxs[frame_descriptors.size()] = it->m_data;
      //frame_descriptors.push_back(static_cast<const FrameData*>(it->m_data->userData())->m_descriptors);
    //}
  {
    const FrameData* fdata = static_cast<const FrameData*>(it->m_data->userData());
    // Count features within the frustum
    unsigned nfeats = 0;
    for(std::vector<Eigen::Vector3f>::const_iterator kpit = fdata->m_keypoints.begin(); kpit < fdata->m_keypoints.end(); ++kpit)
    {
      homv.head<3>() = *kpit;
      if(((m_frustum_mtx * homv).array() < 0).all())
        ++nfeats;
    }
    if((float)nfeats/(float)fdata->m_keypoints.size() > m_perc_feat_overlap)
    {
      m_reference_vxs[frame_descriptors.size()] = it->m_data;
      frame_descriptors.push_back(fdata->m_descriptors);
    }
  }
  stopwatch.stop();

  if(!frame_descriptors.empty())
    m_matcher->setModels(frame_descriptors);
  else
    std::cout << SLAM_DUNK_ERROR_STR("Active window is empty: using last frame") << std::endl;

  if(m_verbose)
    std::cout << SLAM_DUNK_INFO_STR("Active window updated with " << m_reference_vxs.size() << " frames >>> ")
              << stopwatch.format() << std::endl;

  if(m_deb_func)
  {
    StampedPoseVector poses;
    for(std::map<int, VertexPoseConstPtr>::const_iterator it = m_reference_vxs.begin(); it != m_reference_vxs.end(); ++it)
      poses.push_back(std::make_pair(static_cast<const FrameData*>(it->second->userData())->m_timestamp, it->second->estimate()));
    m_deb_func(poses);
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::setFrustum(const Eigen::Matrix3f& inverse_kcam, int width, int height,
                                        float near_plane, float far_plane)
{
  const Eigen::Vector3f& tl = inverse_kcam.col(2);
  Eigen::Vector3f tr = tl; tr[0] += width*inverse_kcam(0,0);
  Eigen::Vector3f bl = tl; bl[1] += height*inverse_kcam(1,1);
  const Eigen::Vector3f br(tr[0], bl[1], 1.f);
  const Eigen::Vector3f npl(0,0,near_plane);
  const Eigen::Vector3f fpl(0,0,far_plane);

  m_frustum_mtx.row(0).head<3>() = Eigen::Vector3f::UnitZ();
                    m_frustum_mtx(0,3) = -1 * m_frustum_mtx.row(0).head<3>().dot(fpl);  // far
  m_frustum_mtx.row(1).head<3>() = -1 * m_frustum_mtx.row(0).head<3>();
                    m_frustum_mtx(1,3) = m_frustum_mtx.row(0).head<3>().dot(npl);       // near
  m_frustum_mtx.row(2).head<3>() = tr.cross(tl).normalized(); m_frustum_mtx(2,3) = 0.f; // top
  m_frustum_mtx.row(3).head<3>() = bl.cross(br).normalized(); m_frustum_mtx(3,3) = 0.f; // bottom
  m_frustum_mtx.row(4).head<3>() = tl.cross(bl).normalized(); m_frustum_mtx(4,3) = 0.f; // left
  m_frustum_mtx.row(5).head<3>() = br.cross(tr).normalized(); m_frustum_mtx(5,3) = 0.f; // right
}

/////////////////////////////////////////////////////////////////////////////////////////////////
namespace
{
  enum { SLAM_DUNK_FEATURE_GRID_SIZE = 4 };
}

bool slamdunk::FeatureTracker::track(const RGBDFrame& frame)
{
  boost::timer::cpu_timer stopwatch;

  std::vector<cv::KeyPoint> kpts;
  extractFrameData(frame, m_last_tracked.m_frame_data, &kpts);
  if(m_last_tracked.m_frame_data.m_keypoints.size() < m_min_matches)
  {
    if(m_verbose) std::cout << SLAM_DUNK_WARNING_STR("Too few valid visual features...track failure") << std::endl;
    return false;
  }

  // frame-frame matching
  std::vector<FMatch> ff_matches;
  stopwatch.start();
  m_matcher->match(m_last_tracked.m_frame_data.m_descriptors, ff_matches);
  stopwatch.stop();
  if(m_verbose) std::cout << SLAM_DUNK_INFO_STR(ff_matches.size() << " matches found with previous frames >>> " << stopwatch.format()) << std::endl;
#ifndef NDEBUG
  if(m_debug)
  {
    std::map<int, std::vector<cv::DMatch> > ref2matches;
    for(unsigned i = 0; i < ff_matches.size(); ++i)
    {
      std::vector<cv::DMatch>& dmatches = ref2matches[ff_matches[i].m_model_idx];
      dmatches.push_back(cv::DMatch(ff_matches[i].m_feat_idx, ff_matches[i].m_query_idx, 0));
    }
    for(std::map<int, std::vector<cv::DMatch> >::const_iterator dmit = ref2matches.begin(); dmit != ref2matches.end(); ++dmit)
    {
      VertexPoseConstPtr frame_vx = m_reference_vxs[dmit->first];
      std::cout << SLAM_DUNK_DEBUG_STR(dmit->second.size() << " matches with reference vx #" << frame_vx->id()) << std::endl;
      const FrameData* fd = static_cast<const FrameData*>(frame_vx->userData());
      cv::Mat outimg;
      cv::drawMatches(fd->m_image, fd->m_kpts2d,
                      m_last_tracked.m_frame_data.m_image, m_last_tracked.m_frame_data.m_kpts2d,
                      dmit->second, outimg);
      cv::imshow("debug", outimg);
      cv::waitKey();
    }
  }
#endif
  if(ff_matches.size() < m_min_matches)
  {
    if(m_verbose) std::cout << SLAM_DUNK_WARNING_STR("Too few feature matches...track failure") << std::endl;
    return false;
  }

  //TODO prova matches
  /*std::vector<std::vector<cv::DMatch> > allMatches;
  for(unsigned i = 0; i < m_reference_vxs.size(); ++i)
  {
	  std::vector<cv::DMatch> matches;
	  allMatches.push_back(matches);
  }
  for(unsigned i = 0; i < ff_matches.size(); ++i)
  {
	  cv::DMatch match;
	  match.imgIdx = ff_matches[i].m_model_idx;
	  match.queryIdx = ff_matches[i].m_query_idx;
	  match.trainIdx = ff_matches[i].m_feat_idx;
	  allMatches[ff_matches[i].m_model_idx].push_back(match);
  }
  for(unsigned i = 0; i < m_reference_vxs.size(); ++i)
  {
	  cv::Mat imgOut;
	  const FrameData* data = static_cm_overlapping_featuresast<const FrameData*>(m_reference_vxs[i]->userData());
	  cv::drawMatches(m_last_tracked.m_frame_data.m_image, m_last_tracked.m_frame_data.m_kpts2d, data->m_image, data->m_kpts2d, allMatches[i], imgOut);
	  std::stringstream ssf;
	  ssf << "/sdcard/testImages/image" << i << ".png";
	  cv::imwrite(ssf.str(), imgOut);

	  cv::Mat imgOutF;
	  cv::drawKeypoints(data->m_image, data->m_kpts2d, imgOutF, cv::Scalar(0, 0, 255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
	  std::stringstream ssf2;
	  ssf2 << "/sdcard/testImages/imageF" << i << ".png";
	  cv::imwrite(ssf2.str(), imgOutF);
  }*/

  // outlier rejection
  std::vector<Eigen::Vector3f> ff_ref_kpts(ff_matches.size());
  std::vector< std::pair<unsigned, unsigned> > ref_query_ids(ff_matches.size());
  std::vector<VertexPoseConstPtr> ref_vxs(ff_matches.size());
  for(unsigned i = 0; i < ff_matches.size(); ++i)
  {
    VertexPoseConstPtr frame_vx = m_reference_vxs[ff_matches[i].m_model_idx];

    ff_ref_kpts[i] = frame_vx->estimate().cast<float>() *
                      (static_cast<const FrameData*>(frame_vx->userData())->m_keypoints[ff_matches[i].m_feat_idx]);
    ref_query_ids[i] = std::pair<unsigned, unsigned>(i, ff_matches[i].m_query_idx);
    ref_vxs[i] = frame_vx;
  }

  /*for (int i = 0; i < ff_matches.size(); i++)
  {
	  std::stringstream ss;
	  ss << "ffRefKey: (" << ff_ref_kpts[i].x() << "," << ff_ref_kpts[i].y() << "," <<
			ff_ref_kpts[i].z() << "); refQueryIds: (" << ref_query_ids[i].first << "," <<
			ref_query_ids[i].second << "); refVxs: (" << ref_vxs[i]->id() << ")";
	  LOGI2("%s", ss.str().data());
  }*/

  stopwatch.start();
  clock_t start = clock();
  m_outlier_rejection->findInliers(ff_ref_kpts, m_last_tracked.m_frame_data.m_keypoints, ref_query_ids);
  stopwatch.stop();
  clock_t end = clock();
  std::stringstream ss;
  ss << "RANSAC: " << (1000.0F * (float)(end - start) / (float)CLOCKS_PER_SEC);
  LOGI2("%s", ss.str().data());
  const std::vector<unsigned>& rqi_inliers = m_outlier_rejection->getRefQueryInliers();
  if(m_verbose) std::cout << SLAM_DUNK_INFO_STR(rqi_inliers.size() << " inliers survived >>> " << stopwatch.format()) << std::endl;
#ifndef NDEBUG
  if(m_debug)
  {
    std::map<VertexPoseConstPtr, std::vector<cv::DMatch> > ref2matches;
    for(unsigned i = 0; i < rqi_inliers.size(); ++i)
    {
      const std::pair<unsigned, unsigned>& inlier_pair = ref_query_ids[rqi_inliers[i]];
      std::vector<cv::DMatch>& dmatches = ref2matches[ ref_vxs[inlier_pair.first] ];
      dmatches.push_back(cv::DMatch(ff_matches[inlier_pair.first].m_feat_idx, inlier_pair.second, 0));
    }
    for(std::map<VertexPoseConstPtr, std::vector<cv::DMatch> >::const_iterator dmit = ref2matches.begin(); dmit != ref2matches.end(); ++dmit)
    {
      const VertexPoseConstPtr frame_vx = dmit->first;
      std::cout << SLAM_DUNK_DEBUG_STR(dmit->second.size() << " matches with reference vx #" << frame_vx->id()) << std::endl;
      const FrameData* fd = static_cast<const FrameData*>(frame_vx->userData());
      cv::Mat outimg;
      cv::drawMatches(fd->m_image, fd->m_kpts2d,
                      m_last_tracked.m_frame_data.m_image, m_last_tracked.m_frame_data.m_kpts2d,
                      dmit->second, outimg);
      cv::imshow("debug", outimg);
      cv::waitKey();
    }
  }
#endif
  if(rqi_inliers.size() < m_min_matches)
  {
    if(m_verbose) std::cout << SLAM_DUNK_WARNING_STR("Too few feature matches...track failure") << std::endl;
    return false;
  }

  // get the inliers
  bool inlier_grid[SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE];
  //bool feature_grid[SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE];
  std::memset(inlier_grid, false, SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE*sizeof(bool));
  //std::memset(feature_grid, false, SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE*sizeof(bool));

  //int min_x = frame.m_color_image.cols, max_x = 0, min_y = frame.m_color_image.rows, max_y = 0;

  m_last_tracked.m_ff_matches.resize(rqi_inliers.size());
  std::vector< std::pair<unsigned, unsigned> > ref_query_ids_inliers;

  m_last_tracked_keyframes.clear();
  for(unsigned i = 0; i < rqi_inliers.size(); ++i)
  {
    const std::pair<unsigned, unsigned>& inlier_pair = ref_query_ids[rqi_inliers[i]];
    ref_query_ids_inliers.push_back(inlier_pair);

    FrameToFrameMatch& ffm = m_last_tracked.m_ff_matches[i];
    ffm.m_matching_frame_id = ref_vxs[inlier_pair.first]->id();
    ffm.m_matching_frame_feat = ff_matches[inlier_pair.first].m_feat_idx;
    ffm.m_ref_frame_feat = inlier_pair.second;
    ffm.m_score = ff_matches[inlier_pair.first].m_match_score;

    m_last_tracked_keyframes.insert(ffm.m_matching_frame_id);

    const cv::KeyPoint& kp = kpts[ ffm.m_ref_frame_feat ];
    //if(kp.pt.x < min_x) min_x = kp.pt.x;
    //if(kp.pt.x > max_x) max_x = kp.pt.x;
    //if(kp.pt.y < min_y) min_y = kp.pt.y;
    //if(kp.pt.y > max_y) max_y = kp.pt.y;

    const int ug = (int)(kp.pt.x * (float)SLAM_DUNK_FEATURE_GRID_SIZE / (float)frame.m_color_image.cols);
    const int vg = (int)(kp.pt.y * (float)SLAM_DUNK_FEATURE_GRID_SIZE / (float)frame.m_color_image.rows);
    assert(ug >= 0 && ug < SLAM_DUNK_FEATURE_GRID_SIZE);
    assert(vg >= 0 && vg < SLAM_DUNK_FEATURE_GRID_SIZE);
    inlier_grid[ug + vg*SLAM_DUNK_FEATURE_GRID_SIZE] = true;
  }
  //for(unsigned i = 0; i < m_last_tracked.m_frame_data.m_keypoints.size(); ++i)
  //{
    //const cv::KeyPoint& kp = kpts[i];
    //const int ug = (int)(kp.pt.x * (float)SLAM_DUNK_FEATURE_GRID_SIZE / (float)frame.m_color_image.cols);
    //const int vg = (int)(kp.pt.y * (float)SLAM_DUNK_FEATURE_GRID_SIZE / (float)frame.m_color_image.rows);
    //assert(ug >= 0 && ug < SLAM_DUNK_FEATURE_GRID_SIZE);
    //assert(vg >= 0 && vg < SLAM_DUNK_FEATURE_GRID_SIZE);
    //feature_grid[ug + vg*SLAM_DUNK_FEATURE_GRID_SIZE] = true;
  //}

  // pose estimation
  stopwatch.start();
  slamdunk::estimateTransformationSVD(ff_ref_kpts, m_last_tracked.m_frame_data.m_keypoints, ref_query_ids_inliers, m_last_tracked.m_pose);
  stopwatch.stop();
  const Eigen::Isometry3d incremental_pose = m_last_tracked.m_pose.inverse() * m_active_win_pose;
  const double cosTheta = (incremental_pose.rotation().trace()-1.)*0.5;
  const float angleScore = (float)(cosTheta < 0.707106781 ? 0. : std::pow(2.*cosTheta*cosTheta -1., 2));
  // overlapping score computation
  m_last_tracked.m_overlapping_features = (float)std::count(inlier_grid, inlier_grid+SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE, true)
  	  	  	  	  	  	  	  	  	  	  / (float)(SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE);
  m_last_tracked.m_overlapping_score = 0.33333333f * (
		  m_last_tracked.m_overlapping_features
		  + angleScore
		  + (float)(1. - std::min(1., incremental_pose.translation().norm()*2.5/m_half_active_win_length))
                                                      );

  if(m_verbose)
    std::cout << SLAM_DUNK_INFO_STR("Pose estimated by SVD (overlapping score: ")
              << ((float)std::count(inlier_grid, inlier_grid+SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE, true)
                    / (float)(SLAM_DUNK_FEATURE_GRID_SIZE*SLAM_DUNK_FEATURE_GRID_SIZE))/3. << "+"
              << angleScore/3.f << "+"
              << (1. - std::min(1., incremental_pose.translation().norm()*2.5/m_half_active_win_length))/3. << "="
              << m_last_tracked.m_overlapping_score << ") >>> " << stopwatch.format() << std::endl;

  return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::updateOverlappingScore(Eigen::Isometry3d& new_estimated_pose)
{
	m_last_tracked.m_pose = new_estimated_pose;
	const Eigen::Isometry3d incremental_pose = m_last_tracked.m_pose.inverse() * m_active_win_pose;
	const double cosTheta = (incremental_pose.rotation().trace() - 1.) * 0.5;
	const float angleScore = (float)(cosTheta < 0.707106781 ? 0. : std::pow(2. * cosTheta * cosTheta - 1., 2));
	// overlapping score computation
	m_last_tracked.m_overlapping_score = 0.33333333f * (
			m_last_tracked.m_overlapping_features
			+ angleScore
			+ (float)(1. - std::min(1., incremental_pose.translation().norm() * 2.5 / m_half_active_win_length)));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::updateMap()
{
  // should we move the active window?
  if( (((Eigen::Vector2d(m_last_tracked.m_pose.translation().x(), m_last_tracked.m_pose.translation().z()) -
          m_active_win_center).cwiseAbs() - m_win_movement_step).array() > 0).any() )
  {
    if(m_verbose) std::cout << SLAM_DUNK_INFO_STR("Moving active window") << std::endl;
    calcActiveWindow();
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
void slamdunk::FeatureTracker::extractFrameData(const RGBDFrame& frame, FrameData& frame_data) const
{
  extractFrameData(frame, frame_data, NULL);
}

void slamdunk::FeatureTracker::extractFrameData(const RGBDFrame& frame, FrameData& frame_data, std::vector<cv::KeyPoint>* kpts_ptr) const
{
  boost::timer::cpu_timer stopwatch;

  frame_data.m_timestamp = frame.m_timestamp;
  // feature extraction
  std::vector<cv::KeyPoint> kpts;
  cv::Mat cvdesc;
  if (m_use_detector_extractor)
  {
	  stopwatch.start();
	  clock_t start = clock();
	  (*m_feature_detector_extractor)(frame.m_color_image, cv::Mat(), kpts, cvdesc, false);
	  clock_t end = clock();
	  stopwatch.stop();
	  std::stringstream ss;
	  ss << "KEYPOINT DETECTION AND EXTRACTION: " << (1000.0F * (float)(end - start) / (float)CLOCKS_PER_SEC) << " >>> " << cvdesc.rows;
	  LOGI2("%s", ss.str().data());
  }
  else
  {
	  stopwatch.start();
	  clock_t start = clock();
	  m_feature_detector->detect(frame.m_color_image, kpts);
	  clock_t end = clock();
	  std::stringstream ss;
	  ss << "KEYPOINT DETECTION: " << (1000.0F * (float)(end - start) / (float)CLOCKS_PER_SEC) << " >>> " << kpts.size();
	  LOGI2("%s", ss.str().data());
	  start = clock();
	  m_feature_extractor->compute(frame.m_color_image, kpts, cvdesc);
	  stopwatch.stop();
	  end = clock();
	  stopwatch.stop();
	  std::stringstream ss2;
	  ss2 << "FEATURE EXTRACTION: " << (1000.0F * (float)(end - start) / (float)CLOCKS_PER_SEC) << " >>> " << cvdesc.rows;
	  LOGI2("%s", ss2.str().data());
  }
  if(m_verbose) std::cout << SLAM_DUNK_INFO_STR(cvdesc.rows << " features extracted >>> " << stopwatch.format()) << std::endl;
  if(m_debug)
  {
    std::cout << SLAM_DUNK_DEBUG_STR("Extracted Keypoints") << std::endl;
    cv::Mat outimg;
    if(frame.m_color_image.channels() == 3) outimg = frame.m_color_image.clone();
    else cv::cvtColor(frame.m_color_image, outimg, CV_GRAY2BGR);
    cv::drawKeypoints(outimg, kpts, outimg, cv::Scalar::all(-1),
                      cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    cv::imshow("debug", outimg);
    cv::waitKey();
  }

  // keep only feats with a valid depth value
  cv::Mat valid_desc(cvdesc.rows, cvdesc.cols, cvdesc.type());
  unsigned feat_with_depth = 0;
  frame_data.m_keypoints.clear();
//#ifndef NDEBUG
  frame_data.m_kpts2d.clear();
  frame_data.m_image = frame.m_color_image;
//#endif
  for(unsigned i = 0; i < kpts.size() && feat_with_depth < m_max_feats_per_frame; ++i)
  {
    assert(kpts[0].response >= kpts[i].response);
    const float& z = frame.m_depth_image(kpts[i].pt);
    if(z > 0)
    {
      const Eigen::Vector3f pt3d = m_inverse_kcam * Eigen::Vector3f(kpts[i].pt.x*z, kpts[i].pt.y*z, z);
      assert(std::isfinite(pt3d.x()) && std::isfinite(pt3d.y()) && std::isfinite(pt3d.z()) && std::abs(pt3d.z()) > 0.2);
      frame_data.m_keypoints.push_back(pt3d);
      if(kpts_ptr != NULL)
        kpts_ptr->push_back(kpts[i]);
    //#ifndef NDEBUG
      frame_data.m_kpts2d.push_back(kpts[i]);
    //#endif
      cvdesc.row(i).copyTo(valid_desc.row(feat_with_depth));

      ++feat_with_depth;
    }
  }
  frame_data.m_descriptors = valid_desc.rowRange(0, feat_with_depth);

  if(m_verbose) std::cout << SLAM_DUNK_INFO_STR(frame_data.m_descriptors.rows << " features have a valid depth value") << std::endl;
#ifndef NDEBUG
  if(m_debug)
  {
    std::cout << SLAM_DUNK_DEBUG_STR("Valid Keypoints") << std::endl;
    cv::Mat imgleft, imgright, outimg;
    if(frame.m_color_image.channels() == 3)
    {
      imgleft = frame.m_color_image.clone();
      imgright = frame.m_color_image.clone();
    } else
    {
      cv::cvtColor(frame.m_color_image, imgleft, CV_GRAY2BGR);
      cv::cvtColor(frame.m_color_image, imgright, CV_GRAY2BGR);
    }
    cv::drawKeypoints(imgleft, kpts, imgleft, cv::Scalar::all(-1),
                      cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    cv::drawKeypoints(imgright, frame_data.m_kpts2d, imgright, cv::Scalar::all(-1),
                      cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

    outimg.create(imgleft.rows, imgleft.cols+imgright.cols, imgleft.type());
    imgleft.copyTo(outimg.colRange(0, imgleft.cols));
    imgright.copyTo(outimg.colRange(imgleft.cols, outimg.cols));

    cv::imshow("debug", outimg);
    cv::waitKey();
  }
#endif
}


/**
 * @file
 * @brief
 *
 * (c) 2010 the IceCube Collaboration
 *
 * $Id: I3SuperDST.cxx 173719 2019-06-04 16:18:41Z david.schultz $
 * @version $Revision: 173719 $
 * @date $Date: 2019-06-04 10:18:41 -0600 (Tue, 04 Jun 2019) $
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 */

#include <cassert>

#include <serialization/binary_object.hpp>
#include <boost/static_assert.hpp>
#include <boost/utility.hpp>
#include <boost/foreach.hpp>

#include "icetray/OMKey.h"
#include "icetray/I3Units.h"

#include "dataclasses/payload/I3SuperDST.h"
#include "dataclasses/payload/I3SuperDSTUtils.h"

/* Maximum bit widths used in discretization */
#define I3SUPERDSTCHARGESTAMP_TIME_BITS_V0   8
#define I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0 6
#define I3SUPERDSTCHARGESTAMP_BYTESIZE_V0    2

#define I3SUPERDST_DOMID_BITS_V0     13
#define I3SUPERDST_SLOP_BITS_V0       3
#define I3SUPERDST_HEADER_BYTESIZE_V0 2

/* Earliest representable time (relative to the readout window) */
const double I3SuperDST::tmin_ = -512.0;

using namespace I3SuperDSTUtils;

I3SuperDSTChargeStamp::I3SuperDSTChargeStamp(double time, double charge, double width,
    bool hlc, Discretization format) : timecode_(0), chargecode_(0), charge_overflow_(0),
    version_(i3superdst_version_), kind_(hlc ? HLC : SLC), charge_format_(format)
{
	i3_assert( time >= 0.0 );
	i3_assert( charge >= 0.0 );

	/* Don't truncate integer codes, since we need to relativize the times later. */
	timecode_ = I3SuperDST::EncodeTime(time, 31, version_);
	widthcode_ = I3SuperDST::EncodeWidth(width, 31, version_);
	chargecode_ = I3SuperDST::EncodeCharge(charge, 31, version_, charge_format_);
}

void
I3SuperDSTChargeStamp::SetTimeReference(const I3SuperDSTChargeStamp &other)
{
	i3_assert(timecode_ >= other.timecode_);

	timecode_ -= other.timecode_;
}

double
I3SuperDSTChargeStamp::GetTime() const
{
	return I3SuperDST::DecodeTime(timecode_, version_);
}

double
I3SuperDSTChargeStamp::GetWidth() const
{
	return I3SuperDST::DecodeWidth(widthcode_, version_);
}

double
I3SuperDSTChargeStamp::GetCharge() const
{
	return I3SuperDST::DecodeCharge(chargecode_ + charge_overflow_,
	    version_, charge_format_);
}

bool
I3SuperDSTChargeStamp::operator==(const I3SuperDSTChargeStamp& rhs) const {
  return (this->timecode_ == rhs.timecode_ &&
          this->widthcode_ == rhs.widthcode_ &&
          this->chargecode_ == rhs.chargecode_ &&
          this->charge_overflow_ == rhs.charge_overflow_ &&
          this->version_ == rhs.version_ &&
          this->kind_ == rhs.kind_ &&
          this->charge_format_ == rhs.charge_format_);
}

I3SuperDSTReadout::I3SuperDSTReadout(const OMKey &om, bool hlc,
    const std::list<I3RecoPulse>::const_iterator &start,
    const std::list<I3RecoPulse>::const_iterator &end, double t0)
{
	std::list<I3RecoPulse>::const_iterator pulse_it = start;

	Discretization format = (om.GetOM() > 60) ? LOG : LINEAR;

	for ( ; pulse_it != end; pulse_it++) {
		stamps_.push_back(I3SuperDSTChargeStamp(pulse_it->GetTime()-t0,
		    std::max(pulse_it->GetCharge(), 0.0f),
		    std::max(pulse_it->GetWidth(), 1.0f), hlc, format));
	}

	time_overflow_ = 0;
	om_ = om;
	kind_ = hlc ? I3SuperDSTChargeStamp::HLC : I3SuperDSTChargeStamp::SLC;
	i3_assert(start != end);
	start_time_ = start->GetTime()-t0;
}

void
I3SuperDSTReadout::SetTimeReference(const I3SuperDSTReadout &other)
{
	const I3SuperDSTChargeStamp &other_stamp = other.stamps_.front();

	i3_assert(start_time_ >= other.start_time_);
	i3_assert(other.stamps_.size() > 0);

	/*
	 * Set the times of any secondary stamp relative to the one prior to
	 * it, then set the time of the first stamp relative to first
	 * stamp in the other readout.
	 */
	Relativize();
	stamps_.front().SetTimeReference(other_stamp);
}

void
I3SuperDSTReadout::Relativize()
{
	std::vector<I3SuperDSTChargeStamp>::reverse_iterator stamp_rit = stamps_.rbegin();
	/*
	 * Set the time of each secondary stamp relative to the one
	 * preceding it. (Reverse iterators are fun!)
	 */
	for ( ; stamp_rit != stamps_.rend()-1; stamp_rit++) {
		i3_assert(stamp_rit->GetTimeCode() >= (stamp_rit+1)->GetTimeCode());
		stamp_rit->SetTimeReference(*(stamp_rit+1));
	}
}

double
I3SuperDSTReadout::GetTime() const
{
	i3_assert(stamps_.size() != 0);

	return (stamps_.front().GetTime() +
	    I3SuperDST::DecodeTime(time_overflow_, i3superdst_version_));
}

bool
I3SuperDSTReadout::operator==(const I3SuperDSTReadout& rhs) const {
  return (this->om_ == rhs.om_ &&
          this->start_time_ == rhs.start_time_ &&
          this->stamps_ == rhs.stamps_ &&
          this->kind_ == rhs.kind_ &&
          this->time_overflow_ == rhs.time_overflow_);
}

namespace I3SuperDSTRecoPulseUtils {
	static bool
	TimeOrdering(const I3RecoPulse &p1, const I3RecoPulse &p2)
	{ return p1.GetTime() < p2.GetTime(); }

	static bool
	IsBorked(const I3RecoPulse &p1)
	{
		return (!std::isfinite(p1.GetTime()) || !std::isfinite(p1.GetCharge()));
	}

	static bool
	HasLC(const I3RecoPulse &p1)
	{
		return (p1.GetFlags() & I3RecoPulse::LC);
	}

	static bool
	ShouldSplit(const OMKey &key, const I3RecoPulse &current,
	    const I3RecoPulse &previous)
	{
		/*
		 * Split if pulses are not from the same launch.
		 */
		static const double tmax = 6.45e3;

		return (key.GetOM() > 60 || HasLC(current) != HasLC(previous)
		    || current.GetTime() - previous.GetTime() > tmax);
	}
};

void
I3SuperDST::AddPulseMap(const I3RecoPulseSeriesMap &pulses, double t0)
{
	I3RecoPulseSeriesMap::const_iterator map_iter;
	std::list<I3RecoPulse> pulse_list;
	std::list<I3RecoPulse>::const_iterator pulse_head, pulse_tail;

	unpacked_.reset();

	/*
	 * Convert the pulse series map to a list of "readouts"
	 * (pulses spaced closely in time in a single DOM)
	 */
	for (map_iter = pulses.begin(); map_iter != pulses.end(); map_iter++) {
		/*
		 * Get a time-ordered representation of the pulse series,
		 * except those pulses with nonsensical charges or times.
		 */
		using namespace I3SuperDSTRecoPulseUtils;
		pulse_list.clear();
		std::remove_copy_if(map_iter->second.begin(),
		    map_iter->second.end(), std::back_inserter(pulse_list),
		    IsBorked);
		if (pulse_list.size() == 0) continue;
		pulse_list.sort(TimeOrdering);

		pulse_head = pulse_list.begin();
		pulse_tail = boost::next(pulse_head);

		for ( ; pulse_tail != pulse_list.end(); pulse_tail++) {
			/*
			 * Split the preceding run off into its own readout
			 * if it is from a different kind of launch or this is
			 * and IceTop DOM (where we assume exactly one pulse
			 * per launch)
			 */
			if (ShouldSplit(map_iter->first, *pulse_tail,
			    *(pulse_head))) {
				readouts_.push_back(I3SuperDSTReadout(
				    map_iter->first, HasLC(*pulse_head),
				    pulse_head, pulse_tail, t0));
				pulse_head = pulse_tail;
			}
		}

		/* Push the remaining pulses into a readout. */
		readouts_.push_back(I3SuperDSTReadout(
		    map_iter->first, HasLC(*pulse_head), pulse_head, pulse_tail, t0));
	}
}

void
I3SuperDST::AddPulseMap(const I3RecoPulseSeriesMap &pulses)
{
	/*
	 * Get the earliest leading-edge time present in the input.
	 * This must be >= -512 ns to be representable.
	 */
	const double t0 = FindStartTime(pulses);
	if (t0 < tmin_)
		log_fatal("First pulse time %g ns < %3.0f ns is unrepresentable! "
		    "Is this un-timeshifted simulation?", t0, tmin_);

	/*
	 * Convert the pulse series map to a list of "readouts"
	 * (pulses spaced closely in time in a single DOM)
	 */
	AddPulseMap(pulses, tmin_);

	/* Sort the readouts by start time */
	readouts_.sort();

	/* Convert the absolute times in each readout to time deltas. */
	std::list<I3SuperDSTReadout>::reverse_iterator list_rit = readouts_.rbegin();
	if (list_rit != readouts_.rend()) {
		for ( ; boost::next(list_rit) != readouts_.rend(); list_rit++)
			list_rit->SetTimeReference(*boost::next(list_rit));
		list_rit->Relativize(); /* Relativize the first readout as well. */
		i3_assert(boost::next(list_rit).base() == readouts_.begin());
	}
}

std::list<I3SuperDSTReadout>
I3SuperDST::GetReadouts(bool hlc) const
{
	std::list<I3SuperDSTReadout> filtered;
	std::list<I3SuperDSTReadout>::const_iterator list_it;

	for (list_it = readouts_.begin(); list_it != readouts_.end(); list_it++)
		if (hlc == (list_it->kind_ == I3SuperDSTChargeStamp::HLC))
			filtered.push_back(*list_it);

	return filtered;
}

I3SuperDST::I3SuperDST(const I3RecoPulseSeriesMap &pulses)
    : version_(i3superdst_version_)
{
	AddPulseMap(pulses);

	InitDebug();
}

/* Expand charge stamps into fake I3RecoPulses */
I3RecoPulseSeriesMapConstPtr
I3SuperDST::Unpack() const
{
	if (unpacked_)
		return unpacked_;

	std::vector<I3SuperDSTChargeStamp>::const_iterator stamp_it;
	std::list<I3SuperDSTReadout>::const_iterator readout_it;
	double t_ref;

	unpacked_ = I3RecoPulseSeriesMapPtr(new I3RecoPulseSeriesMap);

	t_ref = tmin_;

	for (readout_it = readouts_.begin(); readout_it != readouts_.end(); readout_it++) {
		int flags = (readout_it->kind_ == I3SuperDSTChargeStamp::HLC) ?
		    I3RecoPulse::ATWD | I3RecoPulse::FADC | I3RecoPulse::LC : I3RecoPulse::FADC;
		I3RecoPulseSeries &target = unpacked_->operator[](readout_it->om_);

		t_ref += readout_it->GetTime();
		double t_ref_internal = t_ref;

		stamp_it = readout_it->stamps_.begin();

		if (stamp_it != readout_it->stamps_.end()) {
			I3RecoPulse pulse;

			pulse.SetTime(t_ref);
			pulse.SetCharge(stamp_it->GetCharge());
			pulse.SetWidth(stamp_it->GetWidth());
			pulse.SetFlags(flags);
			target.push_back(pulse);

			stamp_it++;
		}

		for ( ; stamp_it != readout_it->stamps_.end(); stamp_it++) {
			I3RecoPulse pulse;

			t_ref_internal += stamp_it->GetTime();
			pulse.SetTime(t_ref_internal);
			pulse.SetCharge(stamp_it->GetCharge());
			pulse.SetWidth(stamp_it->GetWidth());
			pulse.SetFlags(flags);
			target.push_back(pulse);
		}
	}

	BOOST_FOREACH(I3RecoPulseSeriesMap::value_type &target, *unpacked_) {
		std::sort(target.second.begin(), target.second.end(),
		    I3SuperDSTRecoPulseUtils::TimeOrdering);

		I3RecoPulseSeries::iterator prev, current, next;
		prev = target.second.end();
		current = target.second.begin();
		next = current+1;
		bool merge = false;
		/* Ensure that pulses do not overlap. */
		while (next < target.second.end()) {
			current->SetWidth(std::min(current->GetWidth(),
			    float(next->GetTime()-current->GetTime())));
			if (current->GetWidth() == 0 || merge) {
				if (prev != target.second.end()) {
					/* Merge widths with previous pulse */
					if (prev->GetWidth() == 0 && current->GetWidth() > 0)
						prev->SetWidth(current->GetWidth());
					current->SetWidth(prev->GetWidth()/2.);
					prev->SetWidth(prev->GetWidth()/2.);
					current->SetTime(prev->GetTime()+prev->GetWidth());
					merge = false;
				} else {
					merge = true;
				}
			} else {
				merge = false;
			}
			prev = current;
			current++;
			next = current+1;
		}
		if (merge && prev != target.second.end()) {
			if (prev->GetWidth() == 0 && current->GetWidth() > 0)
				prev->SetWidth(current->GetWidth());
			current->SetWidth(prev->GetWidth()/2.);
			prev->SetWidth(prev->GetWidth()/2.);
			current->SetTime(prev->GetTime()+prev->GetWidth());
		}
	}

	return unpacked_;
}

uint32_t
I3SuperDST::EncodeOMKey(const OMKey &key, unsigned int maxbits, unsigned int version)
{
	uint32_t encoded(0);
	int string = key.GetString();
	int om = key.GetOM();

	i3_assert(string >= 0);
	i3_assert(om > 0);
	i3_assert(om <= 64);
	i3_assert(maxbits <= 31);

	/*
	 * NB: this is the same encoding scheme as the DAQ:
	 * upper 7 bits: string number, lower 6 bits: OM number
	 */
	encoded |= ((unsigned(string) << 6) & ~((1<<6)-1));
	encoded |= ((unsigned(om)-1) & ((1<<6)-1));
	i3_assert( encoded <= uint32_t((1<<maxbits)-1) );

	return (encoded);
}

OMKey
I3SuperDST::DecodeOMKey(uint32_t dom_id, unsigned int version)
{
	int string(0), om(0);

	string = unsigned((dom_id >> 6) & ((1<<7)-1));
	om = unsigned(dom_id & ((1<<6)-1))+1;

	return OMKey(string, om);
}

inline uint32_t
truncate(double val, unsigned int maxbits)
{
	uint32_t code(lround(val)); /* Round to nearest integer */
	i3_assert(maxbits <= 31);

	/* Truncate if greater than 2^(maxbits)-1 */
	if (code > uint32_t((1<<maxbits)-1))
		code = uint32_t((1<<maxbits)-1);

	return (code);
}

uint32_t
I3SuperDST::EncodeTime(double time, unsigned int maxbits,
    unsigned int version)
{
	uint32_t encoded(0);

	i3_assert( time >= 0.0 );
	switch (version) {
		case 0:
			encoded = truncate(time / (4.0*I3Units::ns), maxbits);
			break;
		case 1:
			encoded = truncate(time / (1.0*I3Units::ns), maxbits);
			break;
		default:
			break;
	}

	return (encoded);
}

double
I3SuperDST::DecodeTime(uint32_t dt, unsigned int version)
{
	double decoded(0);

	switch (version) {
		case 0:
			decoded = dt*4.0*I3Units::ns;
			break;
		case 1:
			decoded = dt*1.0*I3Units::ns;
			break;
		default:
			break;
	}

	return (decoded);
}

inline unsigned
GetExponent(uint64_t i, unsigned mbits, unsigned ebits)
{
	using namespace I3SuperDSTUtils;
	i3_assert(mbits > 0);
	i3_assert(ebits > 0);
	int e = std::max(findlastset(i)-int(mbits-1), 0);
	return std::min(unsigned(e), (1u << ebits)-1);
}

uint32_t
I3SuperDST::EncodeWidth(double width, unsigned int maxbits,
    unsigned int version)
{
	i3_assert(width > 0 && width < double(std::numeric_limits<unsigned>::max()));
	unsigned rounded = unsigned(ceil(width/1.0));
	unsigned code = std::min(unsigned(findlastset(rounded)), (1u << maxbits)-1);
	return (rounded == 1u << (code-1)) ? code-1 : code;
}

double
I3SuperDST::DecodeWidth(uint32_t dt, unsigned int version)
{
	i3_assert(dt <= 31);
	return double(1u << dt);
}

uint32_t
I3SuperDST::EncodeCharge(double charge, unsigned int maxbits,
    unsigned int version, Discretization mode)
{
	uint32_t encoded(0);

	i3_assert( charge >= 0 );

	if (mode == LOG) {
		i3_assert(maxbits >= 14);
		double logq = std::max(0.0, log10(charge) + 2.0);
		double step = 9.0/double((1<<14)-1);
		encoded = truncate(logq/step, maxbits);
	} else {
		if (version == 0) {
			encoded = truncate(charge/0.15, maxbits);
		} else {
			encoded = uint32_t(floor(std::max(0., charge)/0.05));
		}
	}

	return (encoded);
}

double
I3SuperDST::DecodeCharge(uint32_t chargecode, unsigned int version,
    Discretization mode)

{
	double decoded(0);

	if (mode == LOG) {
		double step = 9.0/double((1<<14)-1);
		decoded = pow(10., chargecode*step - 2.0);
	} else {
		if (version == 0)
			decoded = chargecode*0.15;
		else {
			// Decode at bin center
			decoded = chargecode*0.05 + 0.025;
		}
	}

	return (decoded);
}

double
I3SuperDST::FindStartTime(const I3RecoPulseSeriesMap &pmap)
{
	double tmin = std::numeric_limits<double>::max();
	I3RecoPulseSeriesMap::const_iterator map_it = pmap.begin();
	I3RecoPulseSeries::const_iterator pulse_it;

	for ( ; map_it != pmap.end() ; map_it++)
		for (pulse_it = map_it->second.begin();
		    pulse_it != map_it->second.end(); pulse_it++)
			if (std::isfinite(pulse_it->GetTime()) &&
			    pulse_it->GetTime() < tmin)
				tmin = pulse_it->GetTime();

	return (tmin);
}

bool
I3SuperDST::operator==(const I3SuperDST& rhs) const {
  if (bool(this->unpacked_) != bool(rhs.unpacked_)) {
    return false;
  }
  if (this->unpacked_) {
    if (!(*(this->unpacked_) == *(rhs.unpacked_))) {
      return false;
    }
  }
  return (this->readouts_ == rhs.readouts_ &&
          this->tmin_ == rhs.tmin_ &&
          this->version_ == rhs.version_);
}

/*
 * NB: While the internal layout of bitfields is implementation-defined,
 * (see ISO 14882 [http://www-d0.fnal.gov/~dladams/cxx_standard.pdf] p. 159)
 * GCC >= 3.4.6 obeys the following conventions under __attribute((packed)):
 *
 * - on little-endian platforms, fields are laid out in native byte order
 *   starting from the least significant bit of the parent struct
 *
 * - on big-endian platforms, fields are laid out starting from the most
 *   significant bit of the parent struct
 *
 * To properly interpet the binary representation of a bitfield from a little-
 * endian platform on a big-endian one (and vice versa), it is necessary not
 * only to swap the order of the bytes in memory, but also to reverse the
 * order of the fields.
 */

namespace I3SuperDSTSerialization {

struct DOMHeader {
#if BYTE_ORDER == BIG_ENDIAN
	uint16_t slop      : I3SUPERDST_SLOP_BITS_V0; /* most significant 3 bits of first HLC time */
	uint16_t dom_id    : I3SUPERDST_DOMID_BITS_V0;
#else
	uint16_t dom_id    : I3SUPERDST_DOMID_BITS_V0;
	uint16_t slop      : I3SUPERDST_SLOP_BITS_V0; /* most significant 3 bits of first HLC time */
#endif
} __attribute((packed));

BOOST_STATIC_ASSERT( sizeof(DOMHeader) == I3SUPERDST_HEADER_BYTESIZE_V0 );

union ChargeStamp {
	struct {
#if BYTE_ORDER == BIG_ENDIAN
		uint16_t stop     : 1;
		uint16_t hlc_bit  : 1;
		uint16_t charge   : I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0;
		uint16_t rel_time : I3SUPERDSTCHARGESTAMP_TIME_BITS_V0;
#else
		uint16_t rel_time : I3SUPERDSTCHARGESTAMP_TIME_BITS_V0;
		uint16_t charge   : I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0;
		uint16_t hlc_bit  : 1;
		uint16_t stop     : 1;
#endif
	} stamp;
	struct {
#if BYTE_ORDER == BIG_ENDIAN
		uint16_t stop : 1; /* The stop bit is always valid. */
		uint16_t code : 15;
#else
		uint16_t code : 15;
		uint16_t stop : 1; /* The stop bit is always valid. */
#endif
	} overflow;
	uint16_t raw;
} __attribute((packed));

BOOST_STATIC_ASSERT( sizeof(ChargeStamp) == I3SUPERDSTCHARGESTAMP_BYTESIZE_V0 );

}

#ifndef NDEBUG
#include <sys/resource.h>
class I3SuperDSTTimer {
public:
	I3SuperDSTTimer(double *acc, unsigned *counter)
	: acc_(acc), counter_(counter)
	{
		if (acc_)
			err_ = getrusage(RUSAGE_SELF, &start_);
	}
	~I3SuperDSTTimer()
	{
		if ((acc_) && !err_ && !getrusage(RUSAGE_SELF, &end_)) {
			*acc_ += double(end_.ru_utime.tv_sec + end_.ru_stime.tv_sec
			    - start_.ru_utime.tv_sec - start_.ru_stime.tv_sec)
			    + double(end_.ru_utime.tv_usec + end_.ru_stime.tv_usec
			    - start_.ru_utime.tv_usec - start_.ru_stime.tv_usec)
			    *1e-6;
		}
		if (counter_)
			*counter_ += 1;
	}
private:
	double *acc_;
	unsigned *counter_;
	int err_;
	rusage start_, end_;
};
#endif

template <class T>
static void
swap_vector(std::vector<T> &vec)
{
#if BYTE_ORDER == BIG_ENDIAN
	BOOST_STATIC_ASSERT(sizeof(T) == 2);
	typename std::vector<T>::iterator it = vec.begin();
	for ( ; it != vec.end(); it++) {
		uint16_t *swapit = (uint16_t*)(&*it);
		*swapit = ((*swapit & 0x00FF) << 8) | ((*swapit & 0xFF00) >> 8);
	}
#endif
};

I3MapKeyVectorInt
I3SuperDST::GetEncodedSizes() const
{
	I3MapKeyVectorInt sizes;
	std::ostringstream oarchive_stream;
	icecube::archive::portable_binary_oarchive oarchive(oarchive_stream);

	/* Piggy-back on the current implementation of save() */
	this->save(oarchive, icecube::serialization::version<I3SuperDST>::value,
	    &sizes);

	return sizes;
}

namespace I3SuperDSTUtils {

	void RunCodec::EncodeRun(vector_t &codes, uint8_t val, unsigned len)
	{
		unsigned nblocks = (findlastset(len)-1)/4 + 1;
#if BYTE_ORDER == BIG_ENDIAN
		for (unsigned i=nblocks-1; i >= 0; i--) {
#else
		for (unsigned i=0; i < nblocks; i++) {
#endif
			uint8_t code = (val & 0xf);;
			code |= ((len >> (i*4u)) << 4) & 0xf0;
			codes.push_back(code);
		}
	}

	void RunCodec::DecodeRun(vector_t &target,
	    const vector_t::const_iterator &head,
	    const vector_t::const_iterator &tail)
	{
		uint8_t code = *head & 0xf;
		unsigned runlength = 0;
		unsigned offset = 0;
#if BYTE_ORDER == BIG_ENDIAN
		vector_t::const_iterator it = tail-1;
		for ( ; it >= head; it--, offset+=4u)
#else
		vector_t::const_iterator it = head;
		for ( ; it != tail; it++, offset+=4u)
#endif
			runlength |= (((*it >> 4) & 0xf) << offset);
		target.insert(target.end(), runlength, code);
	}

	void RunCodec::Encode(const vector_t &runs, vector_t &codes)
	{
		vector_t::const_iterator head, tail;
		head = runs.begin();
		tail = head+1;
		for ( ; tail < runs.end(); tail++)
			if (*tail != *head) {
				/* Start a new run */
				unsigned runlength = std::distance(head, tail);
				EncodeRun(codes, *head, runlength);
				head = tail;
			}

		/* True by construction, unless the vector has length 0 */
		if (tail == runs.end()) {
			unsigned runlength = std::distance(head, tail);
			EncodeRun(codes, *head, runlength);
		}
	}

	void RunCodec::Decode(const std::vector<uint8_t> &codes, vector_t &runs)
	{
		vector_t::const_iterator head, tail;
		runs.clear();
		head = codes.begin();
		tail = head+1;
		for ( ; tail < codes.end(); tail++)
			if ((*tail & 0xf) != (*head & 0xf)) {
				/* A run has ended. */
				DecodeRun(runs, head, tail);
				head = tail;
			}

		/* True by construction, unless the vector has length 0 */
		if (tail == codes.end()) {
			DecodeRun(runs, head, tail);
		}
	}

	template <class Archive>
	void SizeCodec::save(Archive &ar, unsigned version) const
	{
		if (size_ < 0xff-sizeof(size_type)) {
			uint8_t tag = size_;
			ar & make_nvp("Tag", tag);
		} else {
			uint8_t n_bytes = (findlastset(size_)-1)/8 + 1;
			uint8_t tag = 0xff - n_bytes;
			ar & make_nvp("Tag", tag);

			uint8_t bytes[8];
			uint64_t s = size_;
#if BYTE_ORDER == BIG_ENDIAN
			for (unsigned i=n_bytes-1; i >= 0; i--, s=s>>8)
#else
			for (unsigned i=0; i < n_bytes; i++, s=s>>8)
#endif
				bytes[i] = s & 0xff;

			ar & make_nvp("Bytes", icecube::serialization::make_binary_object(
			    bytes, n_bytes));
		}
	}

	template <class Archive>
	void SizeCodec::load(Archive &ar, unsigned version)
	{
		uint8_t tag = 0;
		size_ = 0;
		ar & make_nvp("Tag", tag);
		if (tag < 0xff-sizeof(size_type)) {
			size_ = tag;
		} else {
			unsigned n_bytes = 0xff - tag;
			uint8_t bytes[8];
			ar & make_nvp("Bytes", icecube::serialization::make_binary_object(
			    bytes, n_bytes));

#if BYTE_ORDER == BIG_ENDIAN
			for (unsigned i=n_bytes-1; i >= 0; i--)
#else
			for (unsigned i=0; i < n_bytes; i++)
#endif
				size_ |= (size_t(bytes[i]) << (i*8));
		}
	}

}

template <class Archive>
void
I3SuperDST::save(Archive& ar, unsigned version) const
{
	save(ar, version, NULL);
}

namespace {

size_t
encode_overflow(CompactVector<I3SuperDSTSerialization::ChargeStamp> &stamp_stream,
   unsigned code, unsigned max_overflow)
{
	size_t readout_bytes = 0;

	I3SuperDSTSerialization::ChargeStamp stamp;
	// emit overflow in blocks of max_overflow
	do {
		stamp.raw = std::min(code, max_overflow);
		code -= std::min(code, max_overflow);

		stamp_stream.push_back(stamp);
		readout_bytes += sizeof(stamp);

	} while (code > 0);

	// the decoder interprets stamps with value max_overflow as a signal
	// to continue. emit one with value 0 to make it stop.
	if (stamp.raw == max_overflow) {
		stamp.raw = 0;
		stamp_stream.push_back(stamp);
		readout_bytes += sizeof(stamp);
	}

	return readout_bytes;
}

}

template <class Archive>
void
I3SuperDST::save(Archive& ar, unsigned version,
    std::map<OMKey, std::vector<int > > *sizes) const
{
#ifndef NDEBUG
	I3SuperDSTTimer timer(serialization_time_, serialization_counter_);
#endif
	CompactVector<I3SuperDSTSerialization::DOMHeader> header_stream;
	CompactVector<I3SuperDSTSerialization::ChargeStamp> stamp_stream;
	CompactVector<uint8_t> ldr_stream;
	std::vector<uint8_t> widths[4];

	std::list<I3SuperDSTReadout>::const_iterator readout_it;
	std::vector<I3SuperDSTChargeStamp>::const_iterator stamp_it;

	const unsigned max_timecode_header = (1u << (I3SUPERDSTCHARGESTAMP_TIME_BITS_V0
	    + I3SUPERDST_SLOP_BITS_V0)) - 1;
	const unsigned max_timecode = (1u << I3SUPERDSTCHARGESTAMP_TIME_BITS_V0) - 1;
	const unsigned max_chargecode = (1u << I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0) - 1;
	const unsigned max_overflow = UINT16_MAX;

	for (readout_it = readouts_.begin();
	    readout_it != readouts_.end(); readout_it++) {

		I3SuperDSTSerialization::DOMHeader header;

		header.dom_id = EncodeOMKey(readout_it->om_, 13, version);
		size_t readout_bytes = sizeof(header);

		// Widths: {InIce SLC, InIce HLC, IceTop SLC, IceTop HLC}
		std::vector<uint8_t> &width =
		    widths[2*(readout_it->om_.GetOM() > 60) + readout_it->GetLCBit()];

		/*
		 * Special case for the first stamp in the series:
		 * use the slop space in the header to extend the time range.
		 */
		stamp_it = readout_it->stamps_.begin();
		Discretization charge_format = stamp_it->GetChargeFormat();
		if (stamp_it != readout_it->stamps_.end()) {
			unsigned timecode = stamp_it->GetTimeCode();
			unsigned chargecode = stamp_it->GetChargeCode();
			const bool stop = (boost::next(stamp_it) == readout_it->stamps_.end());
			header.slop = (std::min(timecode, max_timecode_header)
			    >> I3SUPERDSTCHARGESTAMP_TIME_BITS_V0)
			    & ((1 << I3SUPERDST_SLOP_BITS_V0)-1);

			I3SuperDSTSerialization::ChargeStamp stampytown; /* Population: 5! */
			stampytown.stamp.rel_time = std::min(timecode, max_timecode_header)
			    & ((1 << I3SUPERDSTCHARGESTAMP_TIME_BITS_V0)-1);
			stampytown.stamp.hlc_bit = stamp_it->GetLCBit();
			stampytown.stamp.stop = stop;

			if (charge_format == LINEAR) {
				stampytown.stamp.charge =
				    std::min(chargecode, max_chargecode);
			} else {
				/*
				 * Special case for floating-point scheme: pack lower 6
				 * bits in the charge stamp; upper 8 bits in an
				 * extra stream.
				 */
				stampytown.stamp.charge = chargecode &
				    ((1<<I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0)-1);
				uint8_t ldr = (chargecode >> I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0) &
				    ((1<<8)-1);
				ldr_stream.push_back(ldr);
				readout_bytes += sizeof(ldr);
			}

			width.push_back(stamp_it->GetWidthCode());
			stamp_stream.push_back(stampytown);
			readout_bytes += sizeof(stampytown);

			/*
			 * If we still have any time overflow, subtract it
			 * off in 16-bit chunks until it's gone. The saturated
			 * timecode serves as a sentinel for this case.
			 */
			if (timecode >= max_timecode_header) {
				readout_bytes += encode_overflow(stamp_stream,
				    timecode-max_timecode_header, max_overflow);
			}

			/*
			 * Repeat the same overflow procedure for charge in the
			 * linear scheme. The floating-point scheme has no overflow.
			 */
			if (charge_format == LINEAR && chargecode >= max_chargecode) {
				readout_bytes += encode_overflow(stamp_stream,
				    chargecode-max_chargecode, max_overflow);
			}

			stamp_it++;
		}

		header_stream.push_back(header);

		/* Only one stamp in the floating point scheme */
		i3_assert(charge_format != LOG || stamp_it == readout_it->stamps_.end());

		for ( ; stamp_it != readout_it->stamps_.end(); ) {
			I3SuperDSTSerialization::ChargeStamp stampytown;
			const bool stop = (boost::next(stamp_it)
			    == readout_it->stamps_.end());
			unsigned chargecode = stamp_it->GetChargeCode();
			unsigned timecode = stamp_it->GetTimeCode();

			stampytown.stamp.rel_time = std::min(timecode, max_timecode);
			stampytown.stamp.charge = std::min(chargecode, max_chargecode);
			stampytown.stamp.hlc_bit = stamp_it->GetLCBit();

			stampytown.stamp.stop = stop;
			width.push_back(stamp_it->GetWidthCode());
			stamp_stream.push_back(stampytown);
			readout_bytes += sizeof(stampytown);

			/* Encode any time overflow. */
			if (timecode >= max_timecode) {
				readout_bytes += encode_overflow(stamp_stream,
				    timecode-max_timecode, max_overflow);
			}

			/* Encode any charge overflow. */
			if (chargecode >= max_chargecode) {
				readout_bytes += encode_overflow(stamp_stream,
				    chargecode-max_chargecode, max_overflow);
			}

			stamp_it++;
		}

		if (sizes != NULL)
			(*sizes)[readout_it->om_].push_back(readout_bytes);
	}

	ar & make_nvp("I3FrameObject", base_object<I3FrameObject>(*this));

	swap_vector(stamp_stream);
	ar & make_nvp("ChargeStamps", stamp_stream);

	swap_vector(header_stream);
	ar & make_nvp("DOMHeaders", header_stream);

	CompactVector<uint8_t> width_runs;
	for (int i = 0; i < 4; i++) {
		width_runs.clear();
		RunCodec::Encode(widths[i], width_runs);
		ar & make_nvp("Widths", width_runs);
	}

	ar & make_nvp("ExtraBytes", ldr_stream);

	return;
}

template <typename Archive>
void I3SuperDST::load_v0(Archive &ar)
{
	uint16_t stream_size;
	ar & make_nvp("NRecords", stream_size);
	std::vector<I3SuperDSTSerialization::ChargeStamp> stamp_stream;
	stamp_stream.resize(stream_size);
	ar & make_nvp("ChargeStamps",
	    icecube::serialization::make_binary_object(&stamp_stream.front(),
	    stream_size*sizeof(I3SuperDSTSerialization::ChargeStamp)));
	swap_vector(stamp_stream);

	/* Count the number of distinct units in the stamp stream */
	unsigned n_headers = 0;
	BOOST_FOREACH(const I3SuperDSTSerialization::ChargeStamp &stamp, stamp_stream)
		if (stamp.stamp.stop)
			n_headers++;

	/* Now that we know the length of the header stream, resurrect it. */
	std::vector<I3SuperDSTSerialization::DOMHeader> header_stream;
	header_stream.resize(n_headers);
	ar & make_nvp("DOMHeaders",
	    icecube::serialization::make_binary_object(&header_stream.front(),
	    n_headers*sizeof(I3SuperDSTSerialization::DOMHeader)));
	swap_vector(header_stream);

	/* Saturation values */
	const unsigned max_timecode_header = (1 << (I3SUPERDSTCHARGESTAMP_TIME_BITS_V0
	    + I3SUPERDST_SLOP_BITS_V0)) - 1;
	const unsigned max_chargecode = (1u << I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0) - 1;

	std::vector<I3SuperDSTSerialization::ChargeStamp>::const_iterator stamp_it
	    = stamp_stream.begin();
	std::vector<I3SuperDSTSerialization::DOMHeader>::const_iterator header_it
	    = header_stream.begin();

	const uint32_t widthcode =
	    I3SuperDST::EncodeWidth(I3SuperDST::DecodeTime(1, 0), 0);

	for ( ; header_it != header_stream.end(); header_it++) {
		I3SuperDSTReadout readout;

		readout.om_ = I3SuperDST::DecodeOMKey(header_it->dom_id, 0);
		bool hlc = stamp_it->stamp.hlc_bit;

		/*
		 * Use the slop bits in the header as the most significant
		 * bits for the time of the first header.
		 */
		unsigned timecode = stamp_it->stamp.rel_time;
		unsigned chargecode = stamp_it->stamp.charge;
		timecode |= (unsigned(header_it->slop) <<
		    I3SUPERDSTCHARGESTAMP_TIME_BITS_V0);

		/* If the time is saturated, add the next 15 bits. */
		if (timecode == max_timecode_header) {
			stamp_it++;
			timecode += stamp_it->overflow.code;
			//readout.time_overflow_ = stamp_it->overflow.code;
		}

		/* If the time is saturated, add the next 15 bits. */
		if (chargecode == max_chargecode) {
			stamp_it++;
			chargecode += stamp_it->overflow.code;
		}

		I3SuperDSTChargeStamp stamp(timecode, chargecode, widthcode,
		    hlc, LINEAR, 0);

		readout.stamps_.push_back(stamp);

		/* Read in the remaining stamps until we hit a stop. */
		while (!stamp_it->stamp.stop) {
			stamp_it++;

			/* No funny schtuff, ja Lebovski? */
			i3_assert( stamp_it->stamp.hlc_bit == hlc );

			timecode = stamp_it->stamp.rel_time;
			chargecode = stamp_it->stamp.charge;

			/* Add charge overflow if we have it */
			if (chargecode == max_chargecode) {
				stamp_it++;
				chargecode += stamp_it->overflow.code;
			}

			I3SuperDSTChargeStamp stamp(timecode, chargecode, widthcode,
			    hlc, LINEAR, 0);

			readout.stamps_.push_back(stamp);
		}
		i3_assert( stamp_it != stamp_stream.end() );
		stamp_it++; /* Advance for the next readout */

		readout.kind_ = hlc ? I3SuperDSTChargeStamp::HLC : I3SuperDSTChargeStamp::SLC;
		readouts_.push_back(readout);
	}

	/* Populate the start_time_ fields of the readouts_ for consistency. */
	double t_ref = 0.0;
	BOOST_FOREACH(I3SuperDSTReadout &readout, readouts_) {
		i3_assert(readout.stamps_.size() > 0);
		t_ref += readout.GetTime();
		readout.start_time_ = t_ref;
	}
}

template <typename Archive>
void I3SuperDST::load_v1(Archive &ar)
{
	CompactVector<I3SuperDSTSerialization::ChargeStamp> stamp_stream;
	ar & make_nvp("ChargeStamps", stamp_stream);
	swap_vector(stamp_stream);

	CompactVector<I3SuperDSTSerialization::DOMHeader> header_stream;
	ar & make_nvp("DOMHeaders", header_stream);
	swap_vector(header_stream);

	std::vector<uint8_t> widths[4];
	std::vector<uint8_t>::const_iterator width_its[4];
	CompactVector<uint8_t> width_runs;
	for (int i = 0; i < 4; i++) {
		width_runs.clear();
		ar & make_nvp("Widths", width_runs);
		RunCodec::Decode(width_runs, widths[i]);
		width_its[i] = widths[i].begin();
	}

	CompactVector<uint8_t> byte_stream;
	ar & make_nvp("ExtraBytes", byte_stream);

	/* Saturation values */
	const unsigned max_timecode_header = (1 << (I3SUPERDSTCHARGESTAMP_TIME_BITS_V0
	    + I3SUPERDST_SLOP_BITS_V0)) - 1;
	const unsigned max_timecode = (1u << I3SUPERDSTCHARGESTAMP_TIME_BITS_V0) - 1;
	const unsigned max_chargecode = (1u << I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0) - 1;

	std::vector<I3SuperDSTSerialization::ChargeStamp>::const_iterator stamp_it
	    = stamp_stream.begin();
	std::vector<I3SuperDSTSerialization::DOMHeader>::const_iterator header_it
	    = header_stream.begin();
	std::vector<uint8_t>::const_iterator ldr_it = byte_stream.begin();

	for ( ; header_it != header_stream.end(); header_it++) {
		I3SuperDSTReadout readout;

		readout.om_ = I3SuperDST::DecodeOMKey(header_it->dom_id, 1);
		i3_assert(stamp_it < stamp_stream.end());
		bool hlc = stamp_it->stamp.hlc_bit;
		bool stop = stamp_it->stamp.stop;

		/* Widths: {InIce SLC, InIce HLC, IceTop SLC, IceTop HLC} */
		std::vector<uint8_t>::const_iterator &width_it =
		    width_its[2*(readout.om_.GetOM() > 60) + hlc];
		std::vector<uint8_t>::const_iterator width_end =
		    widths[2*(readout.om_.GetOM() > 60) + hlc].end();

		/*
		 * Use the slop bits in the header as the most significant
		 * bits for the time of the first stamp.
		 */
		unsigned timecode = stamp_it->stamp.rel_time;
		unsigned chargecode = stamp_it->stamp.charge;
		timecode |= (unsigned(header_it->slop) <<
		    I3SUPERDSTCHARGESTAMP_TIME_BITS_V0);

		Discretization charge_format = (readout.om_.GetOM() > 60)
		    ? LOG : LINEAR;

		if (timecode == max_timecode_header) {
			i3_assert(stamp_it < stamp_stream.end());
			do {
				timecode += (++stamp_it)->raw;
			} while (stamp_it->raw == UINT16_MAX);
		}
		if (charge_format == LINEAR && chargecode == max_chargecode) {
			i3_assert(stamp_it < stamp_stream.end());
			do {
				chargecode += (++stamp_it)->raw;
			} while (stamp_it->raw == UINT16_MAX);
		} else if (charge_format == LOG) {
			i3_assert(ldr_it < byte_stream.end());
			chargecode |= unsigned(*ldr_it) <<
			    I3SUPERDSTCHARGESTAMP_CHARGE_BITS_V0;
			ldr_it++;
		}

		i3_assert(width_it < width_end);
		I3SuperDSTChargeStamp stamp(timecode, chargecode, *width_it++,
		    hlc, charge_format, 1);

		readout.stamps_.push_back(stamp);

		/* Read in the remaining stamps until we hit a stop. */
		while (!stop) {
			stamp_it++;

			/* No funny schtuff, ja Lebovski? */
			i3_assert(charge_format == LINEAR);
			i3_assert( stamp_it->stamp.hlc_bit == hlc );

			timecode = stamp_it->stamp.rel_time;
			chargecode = stamp_it->stamp.charge;
			stop = stamp_it->stamp.stop;

			if (timecode == max_timecode) {
				i3_assert(stamp_it < stamp_stream.end());
				do {
					timecode += (++stamp_it)->raw;
				} while (stamp_it->raw == UINT16_MAX);
			}
			if (chargecode == max_chargecode) {
				i3_assert(stamp_it < stamp_stream.end());
				do {
					chargecode += (++stamp_it)->raw;
				} while (stamp_it->raw == UINT16_MAX);
			}

			i3_assert(width_it < width_end);
			I3SuperDSTChargeStamp stamp(timecode, chargecode, *width_it++,
			    hlc, LINEAR, 1);

			readout.stamps_.push_back(stamp);
		}
		i3_assert( stamp_it < stamp_stream.end() );
		stamp_it++; /* Advance for the next readout */

		readout.kind_ = hlc ? I3SuperDSTChargeStamp::HLC : I3SuperDSTChargeStamp::SLC;
		readouts_.push_back(readout);
	}

	i3_assert(stamp_it == stamp_stream.end());

	/* Populate the start_time_ fields of the readouts_ for consistency. */
	double t_ref = 0.0;
	BOOST_FOREACH(I3SuperDSTReadout &readout, readouts_) {
		i3_assert(readout.stamps_.size() > 0);
		t_ref += readout.GetTime();
		readout.start_time_ = t_ref;
	}
}

template <class Archive>
void
I3SuperDST::load(Archive& ar, unsigned version)
{
	ar & make_nvp("I3FrameObject", base_object<I3FrameObject>(*this));

	switch (version) {
		case 0:
			load_v0(ar);
			break;
		case 1:
			load_v1(ar);
			break;
		default:
			log_fatal("Foo!");
	}

	version_ = version;

	return;
}

I3_SPLIT_SERIALIZABLE(I3SuperDST);

// explicitly instantiate serialization functions to avoid linker errors
// when compiling superdst-test
template void I3SuperDSTUtils::SizeCodec::save(icecube::archive::portable_binary_oarchive&, unsigned) const;
template void I3SuperDSTUtils::SizeCodec::load(icecube::archive::portable_binary_iarchive&, unsigned);
template void I3SuperDSTUtils::SizeCodec::load(icecube::archive::xml_iarchive&, unsigned);
template void I3SuperDSTUtils::SizeCodec::save(icecube::archive::xml_oarchive&, unsigned) const;


// Copyright (c) 2021-2022, The iTw3 Project
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "multisig_kex_msg.h"
#include "multisig_kex_msg_serialization.h"

#include "common/base58.h"
#include "crypto/crypto.h"
extern "C"
{
#include "crypto/crypto-ops.h"
}
#include "cryptonote_basic/cryptonote_format_utils.h"
#include "include_base_utils.h"
#include "ringct/rctOps.h"
#include "serialization/binary_archive.h"
#include "serialization/serialization.h"

#include <boost/utility/string_ref.hpp>

#include <sstream>
#include <utility>
#include <vector>

#undef MONERO_DEFAULT_LOG_CATEGORY
#define MONERO_DEFAULT_LOG_CATEGORY "multisig"

const boost::string_ref MULTISIG_KEX_V1_MAGIC{"MultisigV1"};
const boost::string_ref MULTISIG_KEX_MSG_V1_MAGIC{"MultisigxV1"};
const boost::string_ref MULTISIG_KEX_MSG_V2_MAGIC_1{"MultisigxV2R1"};  //round 1
const boost::string_ref MULTISIG_KEX_MSG_V2_MAGIC_N{"MultisigxV2Rn"};  //round n > 1

namespace multisig
{
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_kex_msg: EXTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  multisig_kex_msg::multisig_kex_msg(const std::uint32_t round,
    const crypto::secret_key &signing_privkey,
    std::vector<crypto::public_key> msg_pubkeys,
    const crypto::secret_key &msg_privkey) :
      m_kex_round{round}
  {
    CHECK_AND_ASSERT_THROW_MES(round > 0, "Kex round must be > 0.");
    CHECK_AND_ASSERT_THROW_MES(sc_check((const unsigned char*)&signing_privkey) == 0 &&
      signing_privkey != crypto::null_skey, "Invalid msg signing key.");

    if (round == 1)
    {
      CHECK_AND_ASSERT_THROW_MES(sc_check((const unsigned char*)&msg_privkey) == 0 &&
        msg_privkey != crypto::null_skey, "Invalid msg privkey.");

      m_msg_privkey = msg_privkey;
    }
    else
    {
      for (const auto &pubkey : msg_pubkeys)
      {
        CHECK_AND_ASSERT_THROW_MES(pubkey != crypto::null_pkey && pubkey != rct::rct2pk(rct::identity()),
          "Pubkey for message was invalid.");
        CHECK_AND_ASSERT_THROW_MES((rct::scalarmultKey(rct::pk2rct(pubkey), rct::curveOrder()) == rct::identity()),
          "Pubkey for message was not in prime subgroup.");
      }

      m_msg_pubkeys = std::move(msg_pubkeys);
    }
    CHECK_AND_ASSERT_THROW_MES(crypto::secret_key_to_public_key(signing_privkey, m_signing_pubkey),
      "Failed to derive public key");

    // sets message and signing pub key
    construct_msg(signing_privkey);
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_kex_msg: EXTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  multisig_kex_msg::multisig_kex_msg(std::string msg) : m_msg{std::move(msg)}
  {
    parse_and_validate_msg();
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_kex_msg: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  crypto::hash multisig_kex_msg::get_msg_to_sign() const
  {
    ////
    // msg_content = kex_round | signing_pubkey | expand(msg_pubkeys) | OPTIONAL msg_privkey
    // sign_msg = versioning-domain-sep | msg_content
    ///

    std::string data;
    CHECK_AND_ASSERT_THROW_MES(MULTISIG_KEX_MSG_V2_MAGIC_1.size() == MULTISIG_KEX_MSG_V2_MAGIC_N.size(),
      "Multisig kex msg magic inconsistency.");
    data.reserve(MULTISIG_KEX_MSG_V2_MAGIC_1.size() + 4 + 32*(1 + (m_kex_round == 1 ? 1 : 0) + m_msg_pubkeys.size()));

    // versioning domain-sep
    if (m_kex_round == 1)
      data.append(MULTISIG_KEX_MSG_V2_MAGIC_1.data(), MULTISIG_KEX_MSG_V2_MAGIC_1.size());
    else
      data.append(MULTISIG_KEX_MSG_V2_MAGIC_N.data(), MULTISIG_KEX_MSG_V2_MAGIC_N.size());

    // kex_round as little-endian bytes
    for (std::size_t i{0}; i < 4; ++i)
    {
      data += static_cast<char>(m_kex_round >> i*8);
    }

    // signing pubkey
    data.append((const char *)&m_signing_pubkey, sizeof(crypto::public_key));

    // add msg privkey if kex_round == 1
    if (m_kex_round == 1)
      data.append((const char *)&m_msg_privkey, sizeof(crypto::secret_key));
    else
    {
      // only add pubkeys if not round 1

      // msg pubkeys
      for (const auto &key : m_msg_pubkeys)
        data.append((const char *)&key, sizeof(crypto::public_key));
    }

    // message to sign
    crypto::hash hash;
    crypto::cn_fast_hash(data.data(), data.size(), hash);

    return hash;
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_kex_msg: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  void multisig_kex_msg::construct_msg(const crypto::secret_key &signing_privkey)
  {
    ////
    // msg_content = kex_round | signing_pubkey | expand(msg_pubkeys) | OPTIONAL msg_privkey
    // sign_msg = versioning-domain-sep | msg_content
    // msg = versioning-domain-sep | serialize(msg_content | crypto_sig[signing_privkey](sign_msg))
    ///

    // sign the message
    crypto::signature msg_signature;
    crypto::hash msg_to_sign{get_msg_to_sign()};
    crypto::generate_signature(msg_to_sign, m_signing_pubkey, signing_privkey, msg_signature);

    // assemble the message
    m_msg.clear();

    std::stringstream serialized_msg_ss;
    binary_archive<true> b_archive(serialized_msg_ss);

    if (m_kex_round == 1)
    {
      m_msg.append(MULTISIG_KEX_MSG_V2_MAGIC_1.data(), MULTISIG_KEX_MSG_V2_MAGIC_1.size());

      multisig_kex_msg_serializable_round1 msg_serializable;
      msg_serializable.msg_privkey    = m_msg_privkey;
      msg_serializable.signing_pubkey = m_signing_pubkey;
      msg_serializable.signature      = msg_signature;

      CHECK_AND_ASSERT_THROW_MES(::serialization::serialize(b_archive, msg_serializable),
        "Failed to serialize multisig kex msg");
    }
    else
    {
      m_msg.append(MULTISIG_KEX_MSG_V2_MAGIC_N.data(), MULTISIG_KEX_MSG_V2_MAGIC_N.size());

      multisig_kex_msg_serializable_general msg_serializable;
      msg_serializable.kex_round      = m_kex_round;
      msg_serializable.msg_pubkeys    = m_msg_pubkeys;
      msg_serializable.signing_pubkey = m_signing_pubkey;
      msg_serializable.signature      = msg_signature;

      CHECK_AND_ASSERT_THROW_MES(::serialization::serialize(b_archive, msg_serializable),
        "Failed to serialize multisig kex msg");
    }

    m_msg.append(tools::base58::encode(serialized_msg_ss.str()));
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_kex_msg: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  void multisig_kex_msg::parse_and_validate_msg()
  {
    // check message type
    CHECK_AND_ASSERT_THROW_MES(m_msg.size() > 0, "Kex message unexpectedly empty.");
    CHECK_AND_ASSERT_THROW_MES(m_msg.substr(0, MULTISIG_KEX_V1_MAGIC.size()) != MULTISIG_KEX_V1_MAGIC,
      "V1 multisig kex messages are deprecated (unsafe).");
    CHECK_AND_ASSERT_THROW_MES(m_msg.substr(0, MULTISIG_KEX_MSG_V1_MAGIC.size()) != MULTISIG_KEX_MSG_V1_MAGIC,
      "V1 multisig kex messages are deprecated (unsafe).");

    // deserialize the message
    std::string msg_no_magic;
    CHECK_AND_ASSERT_THROW_MES(MULTISIG_KEX_MSG_V2_MAGIC_1.size() == MULTISIG_KEX_MSG_V2_MAGIC_N.size(),
      "Multisig kex msg magic inconsistency.");
    CHECK_AND_ASSERT_THROW_MES(tools::base58::decode(m_msg.substr(MULTISIG_KEX_MSG_V2_MAGIC_1.size()), msg_no_magic),
      "Multisig kex msg decoding error.");
    binary_archive<false> b_archive{epee::strspan<std::uint8_t>(msg_no_magic)};
    crypto::signature msg_signature;

    if (m_msg.substr(0, MULTISIG_KEX_MSG_V2_MAGIC_1.size()) == MULTISIG_KEX_MSG_V2_MAGIC_1)
    {
      // try round 1 message
      multisig_kex_msg_serializable_round1 kex_msg_rnd1;

      if (::serialization::serialize(b_archive, kex_msg_rnd1))
      {
        // in round 1 the message stores a private ancillary key component for the multisig account
        // that will be shared by all participants (e.g. a shared private view key)
        m_kex_round      = 1;
        m_msg_privkey    = kex_msg_rnd1.msg_privkey;
        m_signing_pubkey = kex_msg_rnd1.signing_pubkey;
        msg_signature    = kex_msg_rnd1.signature;
      }
      else
      {
        CHECK_AND_ASSERT_THROW_MES(false, "Deserializing kex msg failed.");
      }
    }
    else if (m_msg.substr(0, MULTISIG_KEX_MSG_V2_MAGIC_N.size()) == MULTISIG_KEX_MSG_V2_MAGIC_N)
    {
      // try general message
      multisig_kex_msg_serializable_general kex_msg_general;

      if (::serialization::serialize(b_archive, kex_msg_general))
      {
        m_kex_round      = kex_msg_general.kex_round;
        m_msg_privkey    = crypto::null_skey;
        m_msg_pubkeys    = std::move(kex_msg_general.msg_pubkeys);
        m_signing_pubkey = kex_msg_general.signing_pubkey;
        msg_signature    = kex_msg_general.signature;

        CHECK_AND_ASSERT_THROW_MES(m_kex_round > 1, "Invalid kex message round (must be > 1 for the general msg type).");
      }
      else
      {
        CHECK_AND_ASSERT_THROW_MES(false, "Deserializing kex msg failed.");
      }
    }
    else
    {
      // unknown message type
      CHECK_AND_ASSERT_THROW_MES(false, "Only v2 multisig kex messages are supported.");
    }

    // checks
    for (const auto &pubkey: m_msg_pubkeys)
    {
      CHECK_AND_ASSERT_THROW_MES(pubkey != crypto::null_pkey && pubkey != rct::rct2pk(rct::identity()),
        "Pubkey from message was invalid.");
      CHECK_AND_ASSERT_THROW_MES(rct::isInMainSubgroup(rct::pk2rct(pubkey)),
        "Pubkey from message was not in prime subgroup.");
    }

    CHECK_AND_ASSERT_THROW_MES(m_signing_pubkey != crypto::null_pkey && m_signing_pubkey != rct::rct2pk(rct::identity()),
      "Message signing key was invalid.");
    CHECK_AND_ASSERT_THROW_MES(rct::isInMainSubgroup(rct::pk2rct(m_signing_pubkey)),
      "Message signing key was not in prime subgroup.");

    // validate signature
    crypto::hash signed_msg{get_msg_to_sign()};
    CHECK_AND_ASSERT_THROW_MES(crypto::check_signature(signed_msg, m_signing_pubkey, msg_signature),
      "Multisig kex msg signature invalid.");
  }
  //----------------------------------------------------------------------------------------------------------------------
} //namespace multisig


/*! \file meshlist.cpp
    \brief アルゴンに対して、分子動力学シミュレーションを行うクラスの宣言

    Copyright ©  2017 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/

#include "meshlist.h"
#include <algorithm>        // for std::fill
#include <boost/assert.hpp> // for BOOST_ASSERT

namespace moleculardynamics {
    MeshList::MeshList(double periodiclen) : periodiclen_(periodiclen)
    {
        auto const SL = SystemParam::RCUTOFF + SystemParam::MARGIN;

        m_ = static_cast<std::int32_t>(periodiclen / SL) - 1;
        mesh_size_ = static_cast<double>(periodiclen) / m_;

        BOOST_ASSERT(m_ > 2);
        BOOST_ASSERT(mesh_size_ > SL);

        number_of_mesh_ = m_ * m_ * m_;
        count_.resize(number_of_mesh_);
        indexes_.resize(number_of_mesh_);
    }

    void MeshList::make_pair(SystemParam::myatomvector & atoms, SystemParam::mypairvector & pairs)
    {
        pairs.clear();

        auto const pn = atoms.size();

        std::vector<std::int32_t> particle_position(pn, 0);
        std::vector<std::int32_t> pointer(number_of_mesh_, 0);

        std::fill(count_.begin(), count_.end(), 0);

        auto const im = 1.0 / mesh_size_;
        for (auto i = 0; i < pn; i++) {
            auto ix = static_cast<std::int32_t>(atoms[i].r[0] * im);
            auto iy = static_cast<std::int32_t>(atoms[i].r[1] * im);
            auto iz = static_cast<std::int32_t>(atoms[i].r[2] * im);

            if (ix < 0) {
                ix += m_;
            }
            else if (ix >= m_) {
                ix -= m_;
            }

            if (iy < 0) {
                iy += m_;
            }
            else if (iy >= m_) {
                iy -= m_;
            }
            if (iz < 0) {
                iz += m_;
            }
            else if (iz >= m_) {
                iz -= m_;
            }

            auto const index = ix + iy * m_ + iz * m_ * m_;

            BOOST_ASSERT(index >= 0);
            BOOST_ASSERT(index < number_of_mesh_);

            count_[index]++;
            particle_position[i] = index;
        }

        indexes_[0] = 0;
        auto sum = 0;

        for (auto i = 0; i < number_of_mesh_ - 1; i++) {
            sum += count_[i];
            indexes_[i + 1] = sum;
        }

        for (auto i = 0; i < pn; i++) {
            auto const pos = particle_position[i];
            auto const j = indexes_[pos] + pointer[pos];
            sorted_buffer[j] = i;
            ++pointer[pos];
        }

        for (auto i = 0; i < number_of_mesh_; i++) {
            search(i, atoms, pairs);
        }
    }

    void MeshList::search_other(std::int32_t id, std::int32_t ix, std::int32_t iy, std::int32_t iz, SystemParam::myatomvector & atoms, SystemParam::mypairvector & pairs)
    {
        if (ix < 0) {
            ix += m_;
        }
        else if (ix >= m_) {
            ix -= m_;
        }

        if (iy < 0) {
            iy += m_;
        }
        else if (iy >= m_) {
            iy -= m_;
        }

        if (iz < 0) {
            iz += m_;
        }
        else if (iz >= m_) {
            iz -= m_;
        }

        auto const id2 = ix + iy * m_ + iz * m_ * m_;

        for (auto k = indexes_[id]; k < indexes_[id] + count_[id]; k++) {
            for (auto m_ = indexes_[id2]; m_ < indexes_[id2] + count_[id2]; m_++) {
                auto const i = sorted_buffer[k];
                auto const j = sorted_buffer[m_];

                Eigen::Vector4d d = atoms[j].r - atoms[i].r;

                SystemParam::adjust_periodic(d, periodiclen_);

                if (d.squaredNorm() <= SystemParam::ML2) {
                    pairs.push_back(std::make_pair(i, j));
                }
            }
        }
    }

    void MeshList::search(std::int32_t id, SystemParam::myatomvector & atoms, SystemParam::mypairvector & pairs)
    {
        auto const ix = id % m_;
        auto const iy = (id / m_) % m_;
        auto const iz = (id / m_ / m_);

        search_other(id, ix + 1, iy, iz, atoms, pairs);
        search_other(id, ix - 1, iy + 1, iz, atoms, pairs);
        search_other(id, ix, iy + 1, iz, atoms, pairs);
        search_other(id, ix + 1, iy + 1, iz, atoms, pairs);

        search_other(id, ix - 1, iy, iz + 1, atoms, pairs);
        search_other(id, ix, iy, iz + 1, atoms, pairs);
        search_other(id, ix + 1, iy, iz + 1, atoms, pairs);

        search_other(id, ix - 1, iy - 1, iz + 1, atoms, pairs);
        search_other(id, ix, iy - 1, iz + 1, atoms, pairs);
        search_other(id, ix + 1, iy - 1, iz + 1, atoms, pairs);

        search_other(id, ix - 1, iy + 1, iz + 1, atoms, pairs);
        search_other(id, ix, iy + 1, iz + 1, atoms, pairs);
        search_other(id, ix + 1, iy + 1, iz + 1, atoms, pairs);

        // Registration of self box
        auto const si = indexes_[id];
        auto const n = count_[id];

        for (auto k = si; k < si + n - 1; k++) {
            for (auto m_ = k + 1; m_ < si + n; m_++) {
                auto const i = sorted_buffer[k];
                auto const j = sorted_buffer[m_];

                Eigen::Vector4d d = atoms[j].r - atoms[i].r;

                SystemParam::adjust_periodic(d, periodiclen_);

                if (d.squaredNorm() <= SystemParam::ML2) {
                    pairs.push_back(std::make_pair(i, j));
                }
            }
        }
    }
}


/******************************
  Copyright (C) 2021 Icey Chiu
******************************/
#include <boost/lexical_cast.hpp>
#include "../include/marker_detection.h"

int main(int argc,char **argv){
//   if (argc != 2 ){ std::cerr<<"Usage: inimage"<<std::endl;return -1;}
     /* load parameters */
    const std::string h_matrix_dir = "/home/zebra/test/groundtruth/extrinsic_calibrationfile.yaml";
    std::string result_dir;
    const std::string setting_dir = "/home/zebra/test/groundtruth/intrinsic_calibrationfile.yaml";

    groundtruth gt;
    gt.intrinsic(setting_dir);
    aruco::MarkerDetector MDetector;
    MDetector.setDictionary("ARUCO");
    cv::String folder = "/home/zebra/Downloads/2020325/image/*.png";
    std::vector<cv::String> filenames;
    std::vector<cv::String> names;
    std::vector<double> name;
    cv::glob(folder, filenames, false);
    int nImages = filenames.size();
    cv::Mat image;
    float start_x;
    float start_y;
    /*
    for (int i = 0; i < nImages; ++i)
    {
        images.push_back(cv::imread(filenames[i]));
    }
    */
    for (int i = 0; i < nImages; ++i)
    {
        image = cv::imread(filenames[i]);
        if (image.empty())
        {
            std::cout << "frame: " << image << std::endl;
            break;
        }

        gt.undistort(image);
        gt.pnp(h_matrix_dir, i);
        gt.markers;
        for (size_t i = 0; i < gt.markers.size(); i++){
            int id = gt.markers[i].id;

            switch ( id )
            {
                case 0:
                    result_dir = "/home/zebra/test/groundtruth/result_0.txt";
                    break;
                case 1:
                    result_dir = "/home/zebra/test/groundtruth/result_1.txt";
                    break;
                case 10:
                    result_dir = "/home/zebra/test/groundtruth/result_10.txt";
                    break;
                case 16:
                    result_dir = "/home/zebra/test/groundtruth/result_16.txt";
                    break;
                case 23:
                    result_dir = "/home/zebra/test/groundtruth/result_23.txt";
                    break;
            }
        }
        if (i == 0)
        {
            start_x = gt.pt[i][0];
            start_y = gt.pt[i][1];
        }
        std::ofstream  groundtruth_file(result_dir, std::ofstream::app);
	std::cout << filenames[i] << std::endl;
        names.push_back(filenames[i].substr(36, 19));
        name.push_back(std::stod(names[i], NULL) / 1000000000.00);
        if (i > 0)
            if (((name[i] - name[i - 1]) < 0.06) && ((abs(gt.pt[i][0] - gt.pt[i - 1][0]) > 0.02) || (abs(gt.pt[i][1] - gt.pt[i - 1][1]) > 0.02)))
                continue;
        groundtruth_file << names[i] << " " << (gt.pt[i][0] - start_x) * 0.96595 - 0.0221 << " " << (gt.pt[i][1] - start_y) * 0.96595 + 0.001895 << " 0.0"
                << " 0.0" << " 0.0" << " 0.0" << " 0.0" << std::endl;

        imshow("video", image);
        //cv::waitKey(1);
   }
   return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_PPC_VMX_LIMITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_PPC_VMX_LIMITS_HPP_INCLUDED

#include <boost/simd/arch/ppc/tags.hpp>
#include <boost/simd/arch/common/limits.hpp>
#include <boost/simd/detail/dispatch/meta/make_integer.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <cstdint>

namespace boost { namespace simd
{
  template<> struct limits<boost::simd::vmx_>
  {
    using parent = boost::simd::simd_;

    struct largest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<4,Sign> {};
    };

    struct smallest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<1,Sign> {};
    };

    using largest_real   = float;
    using smallest_real  = float;

    enum { bits = 128, bytes = 16 };
  };
} }

#endif