/* PREM NIRMAL
   Fordham RCV Lab
   Fordham University
   Bronx NY 10458
*/
/*
  07/2012
  An implemtation of Churchill and Vardy's VHiSS algorithm,
  using David Lowe's SIFT feature extractor.
*/

#include <cstdio>
#include <iostream>
#include <cv.h>
#include <highgui.h>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include "Aria.h"

#include "defs.h"

using namespace cv;
using namespace std;
using namespace Eigen;

#define HOMEIMAGE "data/home.pgm"
#define HOMEKEY "data/home.key"
#define IMG_WIDTH 1771
#define IMG_HEIGHT 270
#define DISTANCE 400
#define EPS 2
#define PERCENT 60

/* -------------------- Local function prototypes ------------------------ */
Vector2d FindMatches(Image im1, Keypoint keys1, Image im2, Keypoint keys2, int imageCount);
Keypoint CheckForMatch(Keypoint key, Keypoint klist);
int DistSquared(Keypoint k1, Keypoint k2);
Image CombineImagesHorizontally(Image im1, Image im2);
Image CombineImagesVertically(Image im1, Image im2);
IplImage *rotateImage(const IplImage *src, float angleDegrees);
/*----------------------------- Routines ----------------------------------*/
int main (int argc, char **argv)
{
  Image im1 = NULL, im2 = NULL;
  Keypoint k1 = NULL, k2 = NULL;
  int imageCount=1;
  char imageName[100], keypointName[100], command[256];
  double alpha;
  Vector2d move;

  /* ROBOT DECLARATIONS */
  ArArgumentParser parser(&argc, argv); // set up our parser
  ArSimpleConnector simpleConnector(&parser); // set up our simple connector
  ArRobot robot; // declare robot object
  //  ArSonarDevice sonar;

  /* INITIALIZATION OF CONNECTION TO ROBOT */
  Aria::init(); // mandatory init
  parser.loadDefaultArguments(); // load the default arguments
  if (!simpleConnector.parseArgs() // check command line args
      || !parser.checkHelpAndWarnUnparsed(1))
    {
      simpleConnector.logOptions();
      Aria::shutdown();
      return 1;
    }
  if (!simpleConnector.connectRobot(&robot)) // ask for connection to robot
    {
      printf("Could not connect to robot... exiting\n");
      Aria::shutdown();
      return 1;
    }

  /* INITIALIZATION OF ROBOT*/
  robot.runAsync(true); // commands processed in separate thread
  robot.enableMotors(); // turn the power to the motors on
  //  robot.addRangeDevice(&sonar); // add sonar (THIS IS UNNECCESARY FOR VH)
  ArUtil::sleep(1000); // sleep time allows robot to initialise sonar, motors, etc

  robot.setRotVelMax(30);
  robot.setTransVelMax(80);

  /*** CAPTURE HOME IMAGE ***/
  cout<<endl<<"Place robot at goal location, to capture home image.."<<endl
      <<"Then press ENTER.";
  cin.get();
  ArUtil::sleep(1000);
  IplImage *homeImage, *homeGray;
  system("mplayer tv:// -tv width=1024:height=768:device=/dev/video1:outfmt=rgb24 -frames 1 -vo jpeg:outdir=data");
  sleep(1);
  system("mv data/00000001.jpg data/home.jpg");
  sleep(1);
  system("./omnicamtools_test calib_results.txt data/home.jpg");
  sleep(1);
  system("mv unwarped_image.jpg data/temphome.jpg");

  cout<<"Home image captured and unwarped."<<endl;

  homeImage=cvLoadImage("data/temphome.jpg",1);
  homeGray=cvCreateImage(cvGetSize(homeImage),IPL_DEPTH_8U,1);
  cvCvtColor(homeImage,homeGray,CV_RGB2GRAY);
  cvReleaseImage(&homeImage);
  cout<<"Resizing home image.."<<endl;
  homeImage=cvCreateImage( cvSize((int)(homeGray->width*PERCENT/100),(int)(homeGray->height*PERCENT/100)), homeGray->depth, homeGray->nChannels );
  cvResize(homeGray,homeImage);
  cvReleaseImage(&homeGray);
  homeImage=rotateImage(homeImage,180); // rotate by 180 deg
  cvSaveImage("data/home.pgm",homeImage);
  remove("data/temphome.jpg");
  //  remove("data/home.jpg");

  sleep(1);

  /*** OBTAIN SIFT FEATURES ***/
  cout<<endl<<"Obtaining SIFT features from home image and storing as home.key.."<<endl;
  system("./sift <data/home.pgm > data/home.key");
  /*** -------------------- ***/
  sleep(2);
  /******************************/

  cout<<endl<<"Now position robot away from goal location, "
      <<"and press ENTER to home";
  cin.get();

  IplImage *image,*gray;
  char buf[100];
  vector<double> angleList; angleList.clear();
  vector<int> signList; signList.clear();

  do
    {
      robot.stop();
      ArUtil::sleep(1000);

      system("mplayer tv:// -tv width=1024:height=768:device=/dev/video1:outfmt=rgb24 -frames 1 -vo jpeg:outdir=data");
      sleep(1);
      system("mv data/00000001.jpg data/image.jpg");
      sleep(1);
      system("./omnicamtools_test calib_results.txt data/image.jpg");
      sleep(1);
      sprintf(buf,"mv data/image.jpg data/img%d.jpg",imageCount);
      system(buf);
      system("mv unwarped_image.jpg data/image.jpg");

      cout<<endl<<"Current image captured and unwarped."<<endl;

      /* load temp.jpg into img */
      image=cvLoadImage("data/image.jpg",1);
      ArUtil::sleep(1000);
      gray=cvCreateImage(cvGetSize(image),IPL_DEPTH_8U,1);
      cvCvtColor(image,gray,CV_RGB2GRAY);
      cvReleaseImage(&image);
      image=cvCreateImage( cvSize((int)(gray->width*PERCENT/100),(int)(gray->height*PERCENT/100)), gray->depth, gray->nChannels );
      cvResize(gray,image);
      sprintf(imageName,"data/image%d.pgm",imageCount);
      image=rotateImage(image,180); // rotate by 180 deg
      cvSaveImage(imageName,image);
      sprintf(keypointName,"data/image%d.key",imageCount);
      remove("data/image.jpg");
      cvReleaseImage(&gray);
      cvReleaseImage(&image);

      /*** OBTAIN SIFT FEATURES ***/
      snprintf(command,256,"./sift <%s >%s",imageName,keypointName);
      system(command);
      /*** -------------------- ***/

      im1=ReadPGMFile(HOMEIMAGE);
      k1=ReadKeyFile(HOMEKEY);
      im2=ReadPGMFile(imageName);
      k2=ReadKeyFile(keypointName);

      /* determine movement vector using SIFT features */
      move=FindMatches(im1, k1, im2, k2,imageCount);
      alpha=move(0)*180/M_PI;

      angleList.push_back(alpha);
      signList.push_back((int)move(1));

      if(alpha!=alpha)
	{
	  cout<<"Alpha = "<<alpha<<endl;
	  cout<<"Unable to complete homing. Exiting.."<<endl;
	  return -1;
	}
      else// if(fabs(alpha)>=EPS && alpha==alpha)
	{
	  cout<<"Robot orienting by "<<alpha<<"degrees. ";
	  if(move(1)>0)
	    cout<<"and moving forwards by "<<DISTANCE<<"mm"<<endl;
	  else
	    cout<<"and moving backwards by "<<DISTANCE<<"mm"<<endl;

	  ArUtil::sleep(1000);
	  robot.setDeltaHeading(alpha); // orient robot towards goal
	  ArUtil::sleep(2000);
	  robot.move(DISTANCE*move(1)); // move forwards or backwards depending on move(1)
	  ArUtil::sleep(4000+(DISTANCE*10));
	}
      imageCount++;
    }
  while(imageCount<10);

  cout<<"Log of all angles and direction:"<<endl;
  vector<double>::iterator dit;
  vector<int>::iterator iit;
  for(dit=angleList.begin(),iit=signList.begin();dit!=angleList.end();dit++,iit++)
    cout<<"\talpha="<<*dit<<", sign="<<*iit<<endl;

  angleList.clear();
  signList.clear();

  return 0;
}
/* Given a pair of images and their keypoints, pick the first keypoint
   from one image and find its closest match in the second set of
   keypoints.
*/
Vector2d FindMatches(Image im1, Keypoint keys1, Image im2, Keypoint keys2, int imageCount)
{
  Keypoint k,match;
  int count=0;
  double alpha=0,delta=0;
  //  vector< Vector2d,aligned_allocator<Vector2d> > unitVec;
  Image result;
  result=CombineImagesVertically(im1,im2);

  vector<Keypoint> mPOS1, mPOS2, mNEG1, mNEG2;
  vector<double> thetaPOS, thetaNEG;
  Vector2d move;

  /* Match the keys in list keys1 to their best matches in keys2 */
  for(k=keys1;k!=NULL;k=k->next) // home image
    {
      match=CheckForMatch(k,keys2); // k = home img, keys2 = current image
      if(match!=NULL)
	{
	  delta = k->scale - match->scale;
	  DrawLine(result, (int) k->row, (int) k->col,
		   (int) (match->row + im1->rows), (int) match->col);

	  // STORE mPOS and mNEG
	  if(delta>=0)
	    {
	      mPOS1.push_back(k);
	      mPOS2.push_back(match);
	      // 360 degrees field of view
	      thetaPOS.push_back((match->col*(2*M_PI/IMG_WIDTH)) - M_PI);
	    }
	  if(delta<0)
	    {
	      mNEG1.push_back(k);
	      mNEG2.push_back(match);
	      // 360 degrees field of view
	      thetaNEG.push_back((match->col*(2*M_PI/IMG_WIDTH)) - M_PI);
	    }
	  count++;
	}//end if(match..
    }//end for(k=keys1..

  FILE *matched;
  char matchfileName[100];
  sprintf(matchfileName,"data/matched%d.pgm",imageCount);
  matched=fopen(matchfileName,"w");
  WritePGM(matched, result);
  free_img(result);
  fclose(matched);
  cout<<"Wrote '"<<matchfileName<<"'.\n";

  if(count==0)
    {
      cout<<"No matches found. Exiting..\n";
      abort();
    }

  fprintf(stderr,"Found a total of %d matches.\n", count);

  double POSthetaAverage, NEGthetaAverage;
  vector<double>::iterator pit,nit;
  double sintheta=0, costheta=0;
  for(pit=thetaPOS.begin();pit!=thetaPOS.end();pit++)
    {
      sintheta+=sin(*pit);
      costheta+=cos(*pit);
    }
  if(sintheta!=0 && costheta!=0)
    POSthetaAverage=atan(sintheta/costheta);
  else
    POSthetaAverage=0;

  sintheta=0; costheta=0;

  for(nit=thetaNEG.begin();nit!=thetaNEG.end();nit++)
    {
      sintheta+=sin(*nit);
      costheta+=cos(*nit);
    }
  if(sintheta!=0 && costheta!=0)
    NEGthetaAverage=atan(sintheta/costheta);
  else
    NEGthetaAverage=0;

  sintheta=0; costheta=0;

  double s,c;
  cout<<"sin(POSthetaAverage) = "<<sin(POSthetaAverage)<<endl;
  cout<<"cos(POSthetaAverage) = "<<cos(POSthetaAverage)<<endl;
  cout<<"sin(NEGthetaAverage) = "<<sin(NEGthetaAverage)<<endl;
  cout<<"cos(NEGthetaAverage) = "<<cos(NEGthetaAverage)<<endl;

  s=thetaPOS.size()*sin(POSthetaAverage) + thetaNEG.size()*(sin(NEGthetaAverage)+M_PI);
  c=thetaPOS.size()*cos(POSthetaAverage) + thetaNEG.size()*(cos(NEGthetaAverage)+M_PI);

  cout<<"thetaPOS.size(): "<<thetaPOS.size()<<" thetaNEG.size(): "<<thetaNEG.size()<<endl;

  double sign=1;
  if (fabs(atan2(s,c))>=(M_PI/2))
    sign=-1;
  else sign=1;
  cout<<"s = "<<s<<", c = "<<c<<endl;
  cout<<"atan2(s,c) = "<<atan2(s,c)<<endl;
  alpha=atan2(s,c);

  /** CLEAR MEMORY **/
  mPOS1.clear(); mPOS2.clear();
  mNEG1.clear(); mNEG2.clear();
  thetaPOS.clear(); thetaNEG.clear();

  move(0)=alpha;
  move(1)=sign;

  return move;

}//end void FindMatches..

/* This searches through the keypoints in klist for the two closest
   matches to key.  If the closest is less than 0.6 times distance to
   second closest, then return the closest match.  Otherwise, return
   NULL.
*/
Keypoint CheckForMatch(Keypoint key, Keypoint klist)
{
  int dsq, distsq1 = 100000000, distsq2 = 100000000;
  Keypoint k, minkey = NULL;

  /* Find the two closest matches, and put their squared distances in
     distsq1 and distsq2.
  */
  for (k = klist; k != NULL; k = k->next) {
    dsq = DistSquared(key, k);

    if (dsq < distsq1) {
      distsq2 = distsq1;
      distsq1 = dsq;
      minkey = k;
    } else if (dsq < distsq2) {
      distsq2 = dsq;
    }
  }

  /* Check whether closest distance is less than 0.6 of second. */
  if (10 * 10 * distsq1 < 6 * 6 * distsq2)
    return minkey;
  else return NULL;
}

/* Return squared distance between two keypoint descriptors.
 */
int DistSquared(Keypoint k1, Keypoint k2)
{
  int i, dif, distsq = 0;
  unsigned char *pk1, *pk2;

  pk1 = k1->descrip;
  pk2 = k2->descrip;

  for (i = 0; i < 128; i++) {
    dif = (int) *pk1++ - (int) *pk2++;
    distsq += dif * dif;
  }
  return distsq;
}

Image CombineImagesHorizontally(Image im1, Image im2)
{
  int rows, cols, r, c;
  Image result;
  rows = MAX(im1->rows,im2->rows);
  cols = im1->cols+im2->cols;
  result = CreateImage(rows,cols);
  /* Set all pixels to 0,5, so that blank regions are grey. */
  for (r = 0; r < rows; r++)
    for (c = 0; c < cols; c++)
      result->pixels[r][c] = 0.5;
  /* Copy images into result. */
  for (r = 0; r < im1->rows; r++)
    for (c = 0; c < im1->cols; c++)
      result->pixels[r][c] = im1->pixels[r][c];
  for (r = 0; r < im2->rows; r++)
    for (c = 0; c < im2->cols; c++)
      result->pixels[r][c+im1->cols] = im2->pixels[r][c];

  return result;
}

Image CombineImagesVertically(Image im1, Image im2)
{
  int rows, cols, r, c;
  Image result;

  rows = im1->rows + im2->rows;
  cols = MAX(im1->cols, im2->cols);
  result = CreateImage(rows, cols);

  /* Set all pixels to 0,5, so that blank regions are grey. */
  for (r = 0; r < rows; r++)
    for (c = 0; c < cols; c++)
      result->pixels[r][c] = 0.5;

  /* Copy images into result. */
  for (r = 0; r < im1->rows; r++)
    for (c = 0; c < im1->cols; c++)
      result->pixels[r][c] = im1->pixels[r][c];
  for (r = 0; r < im2->rows; r++)
    for (c = 0; c < im2->cols; c++)
      result->pixels[r + im1->rows][c] = im2->pixels[r][c];

  return result;
}

// Rotate the image clockwise (or counter-clockwise if negative).
// Remember to free the returned image.
IplImage *rotateImage(const IplImage *src, float angleDegrees)
{
  // Create a map_matrix, where the left 2x2 matrix
  // is the transform and the right 2x1 is the dimensions.
  float m[6];
  CvMat M = cvMat(2, 3, CV_32F, m);
  int w = src->width;
  int h = src->height;
  float angleRadians = angleDegrees * ((float)CV_PI / 180.0f);
  m[0] = (float)( cos(angleRadians) );
  m[1] = (float)( sin(angleRadians) );
  m[3] = -m[1];
  m[4] = m[0];
  m[2] = w*0.5f;
  m[5] = h*0.5f;

  // Make a spare image for the result
  CvSize sizeRotated;
  sizeRotated.width = cvRound(w);
  sizeRotated.height = cvRound(h);

  // Rotate
  IplImage *imageRotated = cvCreateImage( sizeRotated,
					  src->depth, src->nChannels );

  // Transform the image
  cvGetQuadrangleSubPix( src, imageRotated, &M);

  return imageRotated;
}


#include "visualization/feature-matches-visualization.h"

#include <vector>

#include <Eigen/Dense>
#include <aslam/cameras/camera.h>
#include <aslam/matcher/match-helpers.h>
#include <aslam/matcher/match-visualization.h>
#include <glog/logging.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

namespace visualization {

void saveOpenCvMatchesAndFeaturesAsImage(
    const aslam::VisualFrame& frame_A, const cv::Mat& image_A,
    const aslam::VisualFrame& frame_B, const cv::Mat& image_B,
    const aslam::OpenCvMatches& matches_A_B, const std::string& filename,
    aslam::FeatureVisualizationType visualization_type, vi_map::VIMap* map) {
  CHECK_NOTNULL(map);

  if (matches_A_B.empty()) {
    VLOG(1) << "No matches found.";
  }

  if (frame_A.getNumKeypointMeasurements() == 0 ||
      frame_B.getNumKeypointMeasurements() == 0) {
    VLOG(1) << "No features found.";
    return;
  }

  // Scale the keypoint score to sth we can visualize.
  const Eigen::Matrix2Xd& key_point_matrix_A(frame_A.getKeypointMeasurements());
  const Eigen::Matrix2Xd& key_point_matrix_B(frame_B.getKeypointMeasurements());
  const Eigen::VectorXd& key_point_scores_A = frame_A.getKeypointScores();
  const Eigen::VectorXd& key_point_scores_B = frame_B.getKeypointScores();
  const double max_score =
      std::max(key_point_scores_A.maxCoeff(), key_point_scores_B.maxCoeff());
  const double min_score =
      std::min(key_point_scores_A.minCoeff(), key_point_scores_B.minCoeff());
  const double score_range = std::max(max_score - min_score, 1.0);
  const double max_key_point_size = 30.0;
  const double min_key_point_size = 5.0;

  const Eigen::VectorXd& key_point_orientation_A =
      frame_A.getKeypointOrientations();
  const Eigen::VectorXd& key_point_orientation_B =
      frame_B.getKeypointOrientations();

  // Convert the keypoints to cv::KeyPoint.
  std::vector<cv::KeyPoint> key_points_A, key_points_B;
  for (uint i = 0; i < frame_A.getNumKeypointMeasurements(); ++i) {
    double key_point_size = std::max(
        (key_point_scores_A(i) - min_score) / score_range * max_key_point_size,
        min_key_point_size);
    key_points_A.emplace_back(
        cv::KeyPoint(
            key_point_matrix_A(0, i), key_point_matrix_A(1, i), key_point_size,
            key_point_orientation_A(i)));
  }
  for (uint i = 0; i < frame_B.getNumKeypointMeasurements(); ++i) {
    double key_point_size = std::max(
        (key_point_scores_B(i) - min_score) / score_range * max_key_point_size,
        min_key_point_size);
    key_points_B.emplace_back(
        cv::KeyPoint(
            key_point_matrix_B(0, i), key_point_matrix_B(1, i), key_point_size,
            key_point_orientation_B(i)));
  }

  // Convert color images to grayscale if necessary.
  cv::Mat grayscale_image_A, grayscale_image_B;
  bool is_color_image = image_A.channels() == 3;
  if (is_color_image) {
    cv::cvtColor(image_A, grayscale_image_A, CV_BGR2GRAY);
    cv::cvtColor(image_B, grayscale_image_B, CV_BGR2GRAY);
  } else {
    grayscale_image_A = image_A;
    grayscale_image_B = image_B;
  }

  // Write features and matches to image file.
  cv::Mat images_w_matches;
  aslam::drawKeyPointsAndMatches(
      grayscale_image_A, key_points_A, grayscale_image_B, key_points_B,
      matches_A_B, visualization_type, &images_w_matches);
  cv::imwrite(filename, images_w_matches);
}

void saveOpenCvMatchesAndFeaturesAsImage(
    const aslam::VisualFrame& frame_A, const aslam::VisualFrame& frame_B,
    const aslam::OpenCvMatches& matches_A_B, const std::string& filename,
    aslam::FeatureVisualizationType visualization_type, vi_map::VIMap* map) {
  CHECK_NOTNULL(map);

  if (!frame_A.hasRawImage() || !frame_B.hasRawImage()) {
    LOG(ERROR) << "One of the frames has no image!";
    return;
  }

  cv::Mat image_A = frame_A.getRawImage();
  cv::Mat image_B = frame_B.getRawImage();

  if (image_A.empty() || image_B.empty()) {
    LOG(ERROR) << "One of the images is empty!";
    return;
  }

  saveOpenCvMatchesAndFeaturesAsImage(
      frame_A, image_A, frame_B, image_B, matches_A_B, filename,
      visualization_type, map);
}

void saveOpenCvMatchesAndFeaturesAsImage(
    const aslam::VisualFrame& frame_A, const vi_map::Vertex& vertex_A,
    const aslam::VisualFrame& frame_B, const vi_map::Vertex& vertex_B,
    const aslam::OpenCvMatches& matches_A_B, const std::string& filename,
    aslam::FeatureVisualizationType visualization_type,
    const backend::ResourceType image_resource_type, vi_map::VIMap* map) {
  CHECK_NOTNULL(map);

  const unsigned int current_frame_idx =
      vertex_A.getVisualFrameIndex(frame_A.getId());
  const unsigned int next_frame_idx =
      vertex_B.getVisualFrameIndex(frame_B.getId());

  // Load image resources.
  cv::Mat image_A, image_B;
  if (!map->getFrameResource<cv::Mat>(
          vertex_A, current_frame_idx, image_resource_type, &image_A)) {
    LOG(ERROR) << "Frame " << current_frame_idx << " of vertex "
               << vertex_A.id() << " has no resource of type "
               << static_cast<int>(image_resource_type);
    return;
  }
  if (!map->getFrameResource<cv::Mat>(
          vertex_B, next_frame_idx, image_resource_type, &image_B)) {
    LOG(ERROR) << "Frame " << next_frame_idx << " of vertex " << vertex_B.id()
               << " has no resource of type "
               << static_cast<int>(image_resource_type);
    return;
  }

  saveOpenCvMatchesAndFeaturesAsImage(
      frame_A, image_A, frame_B, image_B, matches_A_B, filename,
      visualization_type, map);
}

void saveLandmarkMatchesAndFeaturesAsImage(
    const aslam::VisualFrame& frame_A, const vi_map::Vertex& vertex_A,
    const aslam::VisualFrame& frame_B, const vi_map::Vertex& vertex_B,
    const std::string& filename,
    aslam::FeatureVisualizationType visualization_type,
    const backend::ResourceType image_resource_type, vi_map::VIMap* map) {
  CHECK_NOTNULL(map);

  aslam::OpenCvMatches matches_A_B;
  int index_A = 0;
  int index_B = 0;
  float distance = 0.0;
  const unsigned int current_frame_idx =
      vertex_A.getVisualFrameIndex(frame_A.getId());
  const unsigned int next_frame_idx =
      vertex_B.getVisualFrameIndex(frame_B.getId());

  vi_map::LandmarkIdList landmark_ids_A;
  vi_map::LandmarkIdList landmark_ids_B;
  vertex_A.getFrameObservedLandmarkIds(current_frame_idx, &landmark_ids_A);
  vertex_B.getFrameObservedLandmarkIds(next_frame_idx, &landmark_ids_B);

  for (const vi_map::LandmarkId& landmark_id_A : landmark_ids_A) {
    for (const vi_map::LandmarkId& landmark_id_B : landmark_ids_B) {
      if (landmark_id_A.isValid() && landmark_id_B.isValid()) {
        if (landmark_id_A == landmark_id_B) {
          vi_map::Landmark landmark = map->getLandmark(landmark_id_A);
          const vi_map::KeypointIdentifierList& observations =
              landmark.getObservations();
          bool found_observation_for_current_vertex = false;
          bool found_observation_for_next_vertex = false;

          for (vi_map::KeypointIdentifier observation : observations) {
            if (observation.frame_id.vertex_id == vertex_A.id() &&
                observation.frame_id.frame_index == current_frame_idx) {
              found_observation_for_current_vertex = true;
              index_A = observation.keypoint_index;
              distance = static_cast<float>(frame_A.getKeypointScore(index_A));
            } else if (
                observation.frame_id.vertex_id == vertex_B.id() &&
                observation.frame_id.frame_index == next_frame_idx) {
              found_observation_for_next_vertex = true;
              index_B = observation.keypoint_index;
              distance = static_cast<float>(frame_B.getKeypointScore(index_B));
            }
          }
          if (found_observation_for_current_vertex &&
              found_observation_for_next_vertex) {
            matches_A_B.push_back(cv::DMatch(index_A, index_B, distance));
          }
        }
      }
    }
  }
  saveOpenCvMatchesAndFeaturesAsImage(
      frame_A, vertex_A, frame_B, vertex_B, matches_A_B, filename,
      visualization_type, image_resource_type, map);
}
}  // namespace visualization


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_HPP_
#define CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_HPP_

#include "clotho/data_spaces/population_space/population_space.hpp"
#include <boost/random/bernoulli_distribution.hpp>
#include "clotho/data_spaces/crossover/position_classifier.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#include "clotho/data_spaces/crossover/block_crossover.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class MatePairType, class BlockType, class WeightType, class AlleleSpaceType >
class batch_crossover_task< RNG, MatePairType, population_space< BlockType, WeightType > , AlleleSpaceType > : public task {
public:
    typedef batch_crossover_task< RNG, MatePairType, population_space< BlockType, WeightType >, AlleleSpaceType > self_type;

    typedef population_space< BlockType, WeightType >   space_type;
    typedef AlleleSpaceType                             allele_type;
    typedef RNG                                         random_engine_type;

    typedef typename space_type::genome_type            genome_type;

    typedef typename space_type::individual_type        individual_type;

    typedef MatePairType                                        mate_pair_type;
    typedef typename mate_pair_type::iterator                   iterator;
    typedef typename mate_pair_type::const_iterator             const_iterator;

    typedef PositionClassifier< typename allele_type::position_vector >          classifier_type;
    typedef typename classifier_type::event_type                                event_type;

    typedef typename position_distribution_helper< typename allele_type::position_type >::type position_distribution_type;
    typedef typename crossover_event_distribution_helper< double >::type event_distribution_type;

    batch_crossover_task( random_engine_type * rng, space_type * parent, space_type * offspring, allele_type * alleles, unsigned int off_idx, const_iterator first, const_iterator last, double recomb_rate, double seq_bias ) :
        m_rng(rng)
        , m_parental( parent )
        , m_offspring( offspring )
        , m_alleles( alleles )
        , m_parents(first, last)
        , m_offspring_index(off_idx)
        , m_recomb_rate(recomb_rate)
        , m_seq_bias( seq_bias )
    { }

    batch_crossover_task( const self_type & other ) :
        m_rng( other.m_rng )
        , m_parental( other.m_parental )
        , m_offspring( other.m_offspring )
        , m_alleles( other.m_alleles )
        , m_parents( other.m_parents )
        , m_offspring_index( other.m_offspring_index )
        , m_recomb_rate( other.m_recomb_rate )
        , m_seq_bias( other.m_seq_bias )
    { }

    void operator()() {
        event_distribution_type     event_dist( m_recomb_rate);
        boost::random::bernoulli_distribution< double > bias_dist( m_seq_bias);

        iterator mate_it = m_parents.begin(), mate_end = m_parents.end();

#ifdef DEBUGGING
        BOOST_LOG_TRIVIAL(debug) << "Starting batch crossover task";
#endif // DEBUGGING
        size_t i = m_offspring_index;
        while( mate_it != mate_end ) {

            assert( mate_it->first < m_parental->individual_count() );

            unsigned int offs = mate_it->first;

            individual_type ind = m_parental->getIndividual( offs );
            genome_type c0 = m_offspring->create_sequence();

            event_type evts;

            fill_events( evts, event_dist( *m_rng ) );
            classifier_type cfier0( &m_alleles->getPositions(), evts );
            bool _swap = bias_dist( *m_rng );

#ifdef DEBUGGING
//            BOOST_LOG_TRIVIAL(debug) << mate_it->first << "; Crossover s0: " << ind.first << " - " << ind.first.size() << "; s1: " << ind.second << " - " << ind.second.size() << "; child: " << c0.first << "; event size: " << evts.size();
#endif  // DEBUGGING
            run_crossover_task( cfier0, ind.first, ind.second, c0, _swap );

            assert( mate_it->second < m_parental->individual_count() );

            genome_type c1 = m_offspring->create_sequence();
            ind = m_parental->getIndividual( mate_it->second );
#ifdef DEBUGGING
//            BOOST_LOG_TRIVIAL(debug) << mate_it->second << "; Crossover s0: " << ind.first << " - " << ind.first.size() << "; s1: " << ind.second << " - " << ind.second.size() << "; child: " << c1.first << "; event size: " << evts.size();
#endif  // DEBUGGING

            event_type evts1;

            fill_events( evts1, event_dist( *m_rng ) );

            classifier_type cfier1( &m_alleles->getPositions(), evts1 );

            _swap = bias_dist( *m_rng );
            run_crossover_task( cfier1, ind.first, ind.second, c1, _swap);

            m_offspring->setIndividual( i++, c0, c1 );

            ++mate_it;
        }

#ifdef DEBUGGING
        BOOST_LOG_TRIVIAL(debug) << "End Batch Crossover Task";
#endif // DEBUGGING
    }

    virtual ~batch_crossover_task() {}

protected:

    inline void fill_events( event_type & evt, unsigned int N ) {
        while( N-- ) {
            evt.push_back( m_pos_dist( *m_rng ) );
        }
    }

    event_type make_events( unsigned int N ) {
        event_type res;

        while( N-- ) {
            res.push_back( m_pos_dist( *m_rng ) );
        }

        return res;
    }

    void run_crossover_task( classifier_type & cls, genome_type & top, genome_type & bottom, genome_type & res, bool should_swap_strands ) {

        if( cls.event_count() == 0 ) {
            // there are no crossover cls
            // therefore, offspring strand will be a copy of the top strand
            res = ((should_swap_strands) ? bottom : top);
        } else if( should_swap_strands ) {
            run_crossover_task( cls, bottom, top, res );
        } else {
            run_crossover_task( cls, top, bottom, res );
        }
    }

    void run_crossover_task( classifier_type & cls, genome_type & top, genome_type & bottom, genome_type & res ) {
        typedef typename space_type::base_genome_type::sequence_type::const_sequence_iterator const_iterator;
        typedef block_crossover< classifier_type, BlockType >       crossover_type;
        typedef BlockType                                           block_type;

        crossover_type xover( cls );

        const_iterator tb, te, bb, be;
        if( top ) {
            tb = top->begin_sequence();
            te = top->end_sequence();
        } else {
            te = tb;
        }

        if( bottom ) {
            bb = bottom->begin_sequence();
            be = bottom->end_sequence();
        } else {
            be = bb;
        }

        unsigned int i = 0;
//        bool top_equal = true;
//        bool bottom_equal = true;
        while( true ) {
            if( tb == te ) {
                while( bb != be ) {
                    const block_type t = crossover_type::bit_helper_type::ALL_UNSET;
                    const block_type b = *bb++;
                    const block_type o = xover.crossover( t, b, i );
//                    top_equal = top_equal && (o == t );
//                    bottom_equal = bottom_equal && (o == b);
                    res->append_sequence(o);
                    i += crossover_type::bit_helper_type::BITS_PER_BLOCK;
                }
                break;
            } else if( bb == be ) {
                while( tb != te ) {
                    const block_type t = *tb++;
                    const block_type b = crossover_type::bit_helper_type::ALL_UNSET;
                    const block_type o = xover.crossover( t, b, i );
//                    bottom_equal = bottom_equal && (o == b );
//                    top_equal = top_equal && (o == t);
                    res->append_sequence(o);
                    i += crossover_type::bit_helper_type::BITS_PER_BLOCK;
                }
                break;
            }

            const block_type t = *tb++;
            const block_type b = *bb++;

            const block_type o = xover.crossover(t, b, i );

//            bottom_equal = bottom_equal && (o == b );
//            top_equal = top_equal && (o == t);

            res->append_sequence( o );
            i += crossover_type::bit_helper_type::BITS_PER_BLOCK;
        }

//        if( top_equal ) {
//            res = top;
//        } else if( bottom_equal ) {
//            res = bottom;
//        }
    }

    random_engine_type  * m_rng;
    space_type          * m_parental, * m_offspring;
    allele_type         * m_alleles;

    mate_pair_type m_parents;

    unsigned int m_offspring_index;

    double m_recomb_rate, m_seq_bias;

    position_distribution_type  m_pos_dist;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_HPP_


// Boost.Geometry

// Copyright (c) 2020-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP

#include <boost/geometry/strategies/distance/spherical.hpp>
#include <boost/geometry/strategies/index/services.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace index
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename RadiusTypeOrSphere, typename CalculationType>
class spherical
    : public strategies::distance::detail::spherical<RadiusTypeOrSphere, CalculationType>
{
    using base_t = strategies::distance::detail::spherical<RadiusTypeOrSphere, CalculationType>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

template <typename CalculationType = void>
class spherical
    : public strategies::index::detail::spherical<void, CalculationType>
{};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_tag>
{
    using type = strategies::index::spherical<>;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    using type = strategies::index::spherical<>;
};

template <typename Geometry>
struct default_strategy<Geometry, spherical_polar_tag>
{
    using type = strategies::index::spherical<>;
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_INDEX_SPHERICAL_HPP


// Copyright <disenone>

#include <unordered_map>
#include <iostream>
#include <vector>
#include <cmath>

#define BOOST_TEST_MODULE test_cross
#define BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/timer/timer.hpp>
#include <boost/range/irange.hpp>

#include <common/nuid.hpp>
#include <common/silence_unused.hpp>
#include <cross/cross.hpp>

// #include <gperftools/profiler.h>

using namespace aoi;
using namespace aoi::cross;

BOOST_AUTO_TEST_SUITE(test_cross)

class Player;

class CrossAoiTest: public CrossAoi {
 public:
  CrossAoiTest(): CrossAoi(0, 0, 0, 0, 0, 0, 0) {}

  CrossAoiTest(float map_bound_xmin, float map_bound_xmax, float map_bound_zmin,
               float map_bound_zmax, size_t beacon_x, size_t beacon_z, float beacon_radius)
    : CrossAoi(map_bound_xmin, map_bound_xmax, map_bound_zmin,
               map_bound_zmax, beacon_x, beacon_z, beacon_radius)
  {}

friend class Player;
};

class Player {
 public:
  Player() : nuid_(GenNuid()), pos_(0, 0, 0) {}
  Player(Nuid nuid, Pos pos) : nuid_(nuid), pos_(pos) {}

 public:
  inline Nuid AddSensor(float radius, bool log = false) {
    auto sensor_id = GenNuid();
    aoi_->AddSensor(nuid_, sensor_id, radius);
    if (log) {
      printf("Player %lu Add Sensor %lu, radius %f\n", nuid_, sensor_id, radius);
    }
    return sensor_id;
  }

  inline void AddToAoi(CrossAoiTest* aoi, bool log = false) {
    aoi_ = aoi;
    aoi_->AddPlayer(nuid_, pos_.x, pos_.y, pos_.z);
    player_aoi_ = aoi_->player_map_.find(nuid_)->second.get();

    if (log) {
      printf("Add Player: %lu, Pos(%f, %f, %f)\n",
            nuid_, pos_.x, pos_.y, pos_.z);
    }
  }

  inline void RemoveFromAoi(bool log = false) {
    if (log) {
      printf("Remove Player: %lu, Pos(%f, %f, %f)\n",
            nuid_, pos_.x, pos_.y, pos_.z);
    }
    aoi_->RemovePlayer(nuid_);
    aoi_ = nullptr;
    player_aoi_ = nullptr;
  }

  inline void MoveDelta(float x, float y, float z) {
    MoveTo(pos_.x + x, pos_.y + y, pos_.z + z);
  }

  inline void MoveTo(float x, float y, float z, bool log = false) {
    pos_.Set(x, y, z);
    if (aoi_) {
      aoi_->UpdatePos(nuid_, x, y, z);
    }

    if (log) {
      printf("Player %lu MoveTo (%f, %f, %f)\n",
            nuid_, x, y, z);
    }
  }

 public:
  Nuid nuid_;
  Pos pos_;

  PlayerAoi *player_aoi_;
  CrossAoiTest *aoi_;
};

void PrintAoiUpdateInfos(const AoiUpdateInfos &info) {
  printf("=========================== update_infos\n");
  for (auto &elem : info) {
    auto& update_info = elem.second;
    printf("Player: %lu; update sensors size: %lu\n",
           update_info.nuid, update_info.sensor_update_list.size());
    for (auto &sensor : update_info.sensor_update_list) {
      printf("  Sensor_id: %lu, \n", sensor.sensor_id);

      printf("    enters: (%lu)[", sensor.enters.size());
      for (auto &nuid : sensor.enters) {
        printf("%lu, ", nuid);
      }
      printf("]\n");

      printf("    leaves: (%lu)[", sensor.leaves.size());
      for (auto &nuid : sensor.leaves) {
        printf("%lu,", nuid);
      }
      printf("]\n");
    }
  }
  printf("===========================\n\n");
}

void CheckUpdateInfos(const AoiUpdateInfos &update_infos, const AoiUpdateInfos &require_infos) {
  // BOOST_TEST_REQUIRE((update_infos == require_infos));
  BOOST_TEST_REQUIRE((update_infos.size() == require_infos.size()));
  for (const auto &pair : update_infos) {
    auto &update_info = pair.second;
    BOOST_TEST_REQUIRE((require_infos.find(update_info.nuid) != require_infos.end()));
    auto &require_info = require_infos.find(update_info.nuid)->second;

    BOOST_TEST_REQUIRE(
      (update_info.sensor_update_list.size() == require_info.sensor_update_list.size()));

    for (size_t i =0; i < update_info.sensor_update_list.size(); ++i) {
      auto &sensor_info = update_info.sensor_update_list[i];
      auto &require_sensor_info = require_info.sensor_update_list[i];
      BOOST_TEST_REQUIRE((sensor_info.sensor_id == require_sensor_info.sensor_id));
      BOOST_TEST_REQUIRE((sensor_info.enters == require_sensor_info.enters));
      BOOST_TEST_REQUIRE((sensor_info.leaves == require_sensor_info.leaves));
    }
  }
}

void TestSimple(bool log = false) {
  CrossAoiTest cross_aoi(-1000, 1000, -1000, 1000, 3, 3, 5);

  Player player1{GenNuid(), {0, 0, 0}};
  player1.AddToAoi(&cross_aoi, log);
  auto sensor_id1 = player1.AddSensor(10, log);

  Player player2{GenNuid(), {0, 0, 0}};
  player2.AddToAoi(&cross_aoi, log);
  auto sensor_id2 = player2.AddSensor(5, log);

  auto update_infos = cross_aoi.Tick();
  if (log) PrintAoiUpdateInfos(update_infos);
  if (log) cross_aoi.PrintAllNodeList();

  AoiUpdateInfos require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {player2.nuid_}, {}}}}},
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {player1.nuid_}, {}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);

  // player2 move to (6, 0, 0)
  player2.MoveTo(6, 0, 0, log);

  update_infos = cross_aoi.Tick();
  if (log) PrintAoiUpdateInfos(update_infos);

  require_infos = {
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {}, {player1.nuid_}}}}},
    };
  CheckUpdateInfos(update_infos, require_infos);

  // player2 move to (600, 0, 100)
  if (log) cross_aoi.PrintAllNodeList();
  player2.MoveTo(600, 0, 100, log);
  update_infos = cross_aoi.Tick();
  if (log) cross_aoi.PrintAllNodeList();
  if (log) PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {}, {player2.nuid_}}}}},
    };
  CheckUpdateInfos(update_infos, require_infos);

  player1.MoveTo(601, 100, 101, log);
  if (log) cross_aoi.PrintAllNodeList();
  update_infos = cross_aoi.Tick();
  if (log) PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {player2.nuid_}, {}}}}},
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {player1.nuid_}, {}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);

  player2.RemoveFromAoi(log);
  update_infos = cross_aoi.Tick();
  if (log) PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {}, {player2.nuid_}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);
  if (log) cross_aoi.PrintAllNodeList();
}

BOOST_AUTO_TEST_CASE(test_simple) {
  bool log = false;
  TestSimple(log);
}

std::vector<Player> GenPlayers(const size_t player_num, const float map_size) {
  std::vector<Player> players(player_num);

  boost::random::mt19937 random_generator(std::time(0));
  boost::random::uniform_real_distribution<float> pos_generator(-map_size, map_size);

  for (int i : boost::irange(player_num)) {
    auto &player = players[i];
    player.pos_.Set(pos_generator(random_generator), 0, pos_generator(random_generator));
  }

  BOOST_TEST_REQUIRE((players.size() == player_num));
  return players;
}

std::vector<Pos> GenMovements(const size_t player_num, const float length) {
  std::vector<Pos> movements;
  movements.reserve(player_num);

  boost::random::mt19937 random_generator(std::time(0));
  boost::random::uniform_real_distribution<float> angle_gen(0, 360);
  for (int UNUSED(i) : boost::irange(player_num)) {
    float angle = angle_gen(random_generator);
    float radian = 2 * M_PI * angle / 360;
    movements.emplace_back(std::cos(radian) * length, 0, std::sin(radian) * length);
  }

  return movements;
}

void TestOneMilestone(std::vector<Player> *players, const size_t player_num,
                      const float map_size) {
  printf("\n===Begin Milestore: player_num = %lu, map_size = (%f, %f)\n",
         player_num, -map_size, map_size);

  boost::timer::cpu_timer run_timer;
  int times = 1;
  std::vector<CrossAoiTest> cross_aois;
  for (auto UNUSED(i) : boost::irange(times)) {
    cross_aois.emplace_back(-map_size, map_size, -map_size, map_size, 3, 3, 100);
  }
  // ProfilerStart("a.prof");
  for (auto &cross_aoi : cross_aois) {
    for (auto &player : *players) {
      player.AddToAoi(&cross_aoi);
      player.AddSensor(100);
    }
    BOOST_TEST_REQUIRE((cross_aoi.GetPlayerMap().size() == player_num + 9));
  }
  // ProfilerStop();
  run_timer.stop();
  printf("Add Player (%i times)", times);
  std::cout << run_timer.format();

  for (auto &cross_aoi : cross_aois) {
    cross_aoi.Tick();
  }

  run_timer.start();
  for (auto &cross_aoi : cross_aois) {
    cross_aoi.Tick();
  }
  run_timer.stop();

  printf("Tick (%i times)", times);
  std::cout << run_timer.format();

  float speed = 6;
  float delta_time = 0.1;
  auto movements = GenMovements(player_num, delta_time * speed);
  times = 1 / delta_time;
  run_timer.start();
  for (int UNUSED(t) : boost::irange(times)) {
    for (int i : boost::irange(player_num)) {
      auto &player = players->at(i);
      auto &move = movements[i];
      player.MoveDelta(move.x, move.y, move.z);
    }
  }
  run_timer.stop();
  printf("Update Pos (%i times)", times);
  std::cout << run_timer.format();

  printf("===End Milestore\n");
}

BOOST_AUTO_TEST_CASE(test_milestone) {
  for (size_t player_num : {100, 1000, 10000}) {
    for (float map_size : {50, 100, 1000, 10000}) {
      auto players = GenPlayers(player_num, map_size);
      TestOneMilestone(&players, player_num, map_size);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


/**************************************************************************\
 *
 *  This file is part of the Coin 3D visualization library.
 *  Copyright (C) by Kongsberg Oil & Gas Technologies.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  ("GPL") version 2 as published by the Free Software Foundation.
 *  See the file LICENSE.GPL at the root directory of this source
 *  distribution for additional information about the GNU GPL.
 *
 *  For using Coin with software that can not be combined with the GNU
 *  GPL, and for taking advantage of the additional benefits of our
 *  support services, please contact Kongsberg Oil & Gas Technologies
 *  about acquiring a Coin Professional Edition License.
 *
 *  See http://www.coin3d.org/ for more information.
 *
 *  Kongsberg Oil & Gas Technologies, Bygdoy Alle 5, 0257 Oslo, NORWAY.
 *  http://www.sim.no/  sales@sim.no  coin-support@coin3d.org
 *
\**************************************************************************/

#include <Inventor/engines/SoHeightMapToNormalMap.h>

#include <boost/scoped_array.hpp>

#include <Inventor/SbVec3f.h>
#include <Inventor/SbImage.h>
#include "engines/SoSubEngineP.h"

/*!
  \class SoHeightMapToNormalMap SoHeightMapToNormalMap.h Inventor/engines/SoHeightMapToNormalMap.h
  \brief Engine for computing a normal map from a height map.

  This engine will create a normal map texture from a height map texture.
  You can use it in an Inventor file like this:

  \code
  Texture2 {
    image = HeightMapToNormalMap {
      sourceImage = Texture2 { filename "HeightMap.jpg" } . image
    } . image
  }
  \endcode

  Be aware that the field connections will remain active, so both
  Texture2 nodes and the HeightMapToNormalMap engine will be kept resident
  in memory (unless you intervene manually and detach the engine) even
  though only the "outer" Texture2 node is needed. This can give quite
  a big memory use overhead.

  \ingroup engines
  \COIN_CLASS_EXTENSION
  \since Coin 3.0
*/

/*!
  \enum SoHeightMapToNormalMap::NormalMapFormat
  Enumeration of available normal map formats.
*/

/*!
  \var SoHeightMapToNormalMap::NormalMapFormat SoHeightMapToNormalMap::INT8
  Encode the normals as a 3 component byte texture.
  This is the only option for now, as long as float textures are not conveniently
  supported in Coin.
*/

/*!
  \var SoMFEnum SoHeightMapToNormalMap::format
  This setting decides what kind of normal map is generated.  For now, only the
  INT8 format is available, and it is the default value.
*/

SO_ENGINE_SOURCE(SoHeightMapToNormalMap);

/*!
  Class initializer.
*/
void
SoHeightMapToNormalMap::initClass(void)
{
  SO_ENGINE_INTERNAL_INIT_CLASS(SoHeightMapToNormalMap);
}

/*!
  Constructor.
*/
SoHeightMapToNormalMap::SoHeightMapToNormalMap(void)
{
  SO_ENGINE_INTERNAL_CONSTRUCTOR(SoHeightMapToNormalMap);

  SO_ENGINE_ADD_INPUT(format, (INT8));

  SO_ENGINE_DEFINE_ENUM_VALUE(NormalMapFormat, INT8);
  SO_ENGINE_SET_SF_ENUM_TYPE(format, NormalMapFormat);
}

/*!
  Static function for computing a normal map from a height map.
  This function can be used directly without any engine instantiation.
*/
void
SoHeightMapToNormalMap::convert(const unsigned char * srcptr, SbVec2s size, int nc, SbImage & dst_out)
{
  float dx, dy;
  int width = size[0];
  int height = size[1];
  boost::scoped_array<unsigned char> dstarray(new unsigned char[width*height*3]);
  unsigned char * dstptr = dstarray.get();
  unsigned char red;
  SbVec3f n;

#define GET_PIXEL_RED(x_, y_) \
  srcptr[(y_)*width*nc + (x_)*nc]

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      // do Y Sobel filter
      red = GET_PIXEL_RED((x-1+width) % width, (y+1) % height);
      dy  = static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED(x % width, (y+1) % height);
      dy += static_cast<float>(red) / 255.0f * -2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y+1) % height);
      dy += static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x-1+width) % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  1.0f;

      red = GET_PIXEL_RED(x % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  1.0f;

      // Do X Sobel filter
      red = GET_PIXEL_RED((x-1+width) % width, (y-1+height) % height);
      dx  = static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x-1+width) % width, y % height);
      dx += static_cast<float>(red) / 255.0f * -2.0f;

      red = GET_PIXEL_RED((x-1+width) % width, (y+1) % height);
      dx += static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x+1) % width, (y-1+height) % height);
      dx += static_cast<float>(red) / 255.0f *  1.0f;

      red = GET_PIXEL_RED((x+1) % width, y % height);
      dx += static_cast<float>(red) / 255.0f *  2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y+1) % height);
      dx += static_cast<float>(red) / 255.0f *  1.0f;

      n[0] = -dx;
      n[1] = -dy;
      n[2] = 1.0f;
      (void) n.normalize();

      *dstptr++ = static_cast<unsigned char>(SbMin((n[0]+1.0f) * 128.0f, 255.0f));
      *dstptr++ = static_cast<unsigned char>(SbMin((n[1]+1.0f) * 128.0f, 255.0f));
      *dstptr++ = static_cast<unsigned char>(SbMin((n[2]+1.0f) * 128.0f, 255.0f));
    }
  }
#undef GET_PIXEL_RED
  dst_out.setValue(size, 3, dstarray.get());
}

void
SoHeightMapToNormalMap::inputChanged(SoField * which)
{
  // in case we need to override later
  inherited::inputChanged(which);
}

void
SoHeightMapToNormalMap::evaluate(void)
{
  SbVec2s size;
  int nc;
  const unsigned char * ptr =
    static_cast<const unsigned char *>(sourceImage.getValue(size, nc));

  SbImage targetimg;
  SoHeightMapToNormalMap::convert(ptr, size, nc, targetimg);

  ptr = static_cast<const unsigned char *>(targetimg.getValue(size, nc));
  SO_ENGINE_OUTPUT(image, SoSFImage, setValue(size, nc, ptr));
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2015-2015. Distributed under the Boost
// Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/container for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#define BOOST_CONTAINER_SOURCE
#include <boost/container/detail/config_begin.hpp>
#include <boost/container/detail/min_max.hpp>
#include <boost/container/detail/workaround.hpp>
#include <boost/container/pmr/global_resource.hpp>
#include <boost/container/pmr/monotonic_buffer_resource.hpp>
#include <boost/container/throw_exception.hpp>
#include <boost/intrusive/detail/math.hpp>
#include <cstddef>

namespace
{

#ifdef BOOST_HAS_INTPTR_T
typedef boost::uintptr_t uintptr_type;
#else
typedef std::size_t uintptr_type;
#endif

static const std::size_t minimum_buffer_size = 2 * sizeof(void*);

} // namespace

namespace boost
{
namespace container
{
namespace pmr
{

void monotonic_buffer_resource::increase_next_buffer()
{
    m_next_buffer_size = (std::size_t(-1) / 2 < m_next_buffer_size)
                             ? std::size_t(-1)
                             : m_next_buffer_size * 2;
}

void monotonic_buffer_resource::increase_next_buffer_at_least_to(
    std::size_t minimum_size)
{
    if (m_next_buffer_size < minimum_size)
    {
        if (bi::detail::is_pow2(minimum_size))
        {
            m_next_buffer_size = minimum_size;
        }
        else if (std::size_t(-1) / 2 < minimum_size)
        {
            m_next_buffer_size = minimum_size;
        }
        else
        {
            m_next_buffer_size = bi::detail::ceil_pow2(minimum_size);
        }
    }
}

monotonic_buffer_resource::monotonic_buffer_resource(memory_resource* upstream)
    BOOST_NOEXCEPT
    : m_memory_blocks(upstream ? *upstream : *get_default_resource()),
      m_current_buffer(0),
      m_current_buffer_size(0u),
      m_next_buffer_size(initial_next_buffer_size),
      m_initial_buffer(0),
      m_initial_buffer_size(0u)
{
}

monotonic_buffer_resource::monotonic_buffer_resource(
    std::size_t initial_size, memory_resource* upstream) BOOST_NOEXCEPT
    : m_memory_blocks(upstream ? *upstream : *get_default_resource()),
      m_current_buffer(0),
      m_current_buffer_size(0u),
      m_next_buffer_size(minimum_buffer_size),
      m_initial_buffer(0),
      m_initial_buffer_size(0u)
{ // In case initial_size is zero
    this->increase_next_buffer_at_least_to(initial_size + !initial_size);
}

monotonic_buffer_resource::monotonic_buffer_resource(
    void* buffer, std::size_t buffer_size,
    memory_resource* upstream) BOOST_NOEXCEPT
    : m_memory_blocks(upstream ? *upstream : *get_default_resource()),
      m_current_buffer(buffer),
      m_current_buffer_size(buffer_size),
      m_next_buffer_size(
          bi::detail::previous_or_equal_pow2(boost::container::dtl::max_value(
              buffer_size, std::size_t(initial_next_buffer_size)))),
      m_initial_buffer(buffer),
      m_initial_buffer_size(buffer_size)
{
    this->increase_next_buffer();
}

monotonic_buffer_resource::~monotonic_buffer_resource()
{
    this->release();
}

void monotonic_buffer_resource::release() BOOST_NOEXCEPT
{
    m_memory_blocks.release();
    m_current_buffer = m_initial_buffer;
    m_current_buffer_size = m_initial_buffer_size;
    m_next_buffer_size = initial_next_buffer_size;
}

memory_resource*
    monotonic_buffer_resource::upstream_resource() const BOOST_NOEXCEPT
{
    return &m_memory_blocks.upstream_resource();
}

std::size_t monotonic_buffer_resource::remaining_storage(
    std::size_t alignment,
    std::size_t& wasted_due_to_alignment) const BOOST_NOEXCEPT
{
    const uintptr_type up_alignment_minus1 = alignment - 1u;
    const uintptr_type up_alignment_mask = ~up_alignment_minus1;
    const uintptr_type up_addr = uintptr_type(m_current_buffer);
    const uintptr_type up_aligned_addr =
        (up_addr + up_alignment_minus1) & up_alignment_mask;
    wasted_due_to_alignment = std::size_t(up_aligned_addr - up_addr);
    return m_current_buffer_size <= wasted_due_to_alignment
               ? 0u
               : m_current_buffer_size - wasted_due_to_alignment;
}

std::size_t monotonic_buffer_resource::remaining_storage(
    std::size_t alignment) const BOOST_NOEXCEPT
{
    std::size_t ignore_this;
    return this->remaining_storage(alignment, ignore_this);
}

const void* monotonic_buffer_resource::current_buffer() const BOOST_NOEXCEPT
{
    return m_current_buffer;
}

std::size_t monotonic_buffer_resource::next_buffer_size() const BOOST_NOEXCEPT
{
    return m_next_buffer_size;
}

void* monotonic_buffer_resource::allocate_from_current(std::size_t aligner,
                                                       std::size_t bytes)
{
    char* p = (char*)m_current_buffer + aligner;
    m_current_buffer = p + bytes;
    m_current_buffer_size -= aligner + bytes;
    return p;
}

void* monotonic_buffer_resource::do_allocate(std::size_t bytes,
                                             std::size_t alignment)
{
    if (alignment > memory_resource::max_align)
        throw_bad_alloc();

    // See if there is room in current buffer
    std::size_t aligner = 0u;
    if (this->remaining_storage(alignment, aligner) < bytes)
    {
        // Update next_buffer_size to at least bytes
        this->increase_next_buffer_at_least_to(bytes);
        // Now allocate and update internal data
        m_current_buffer = (char*)m_memory_blocks.allocate(m_next_buffer_size);
        m_current_buffer_size = m_next_buffer_size;
        this->increase_next_buffer();
    }
    // Enough internal storage, extract from it
    return this->allocate_from_current(aligner, bytes);
}

void monotonic_buffer_resource::do_deallocate(
    void* p, std::size_t bytes, std::size_t alignment) BOOST_NOEXCEPT
{
    (void)p;
    (void)bytes;
    (void)alignment;
}

bool monotonic_buffer_resource::do_is_equal(const memory_resource& other) const
    BOOST_NOEXCEPT
{
    return this == dynamic_cast<const monotonic_buffer_resource*>(&other);
}

} // namespace pmr
} // namespace container
} // namespace boost

#include <boost/container/detail/config_end.hpp>


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  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.
 *
 * 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 <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/restrict.hpp>

#include "dbglog/dbglog.hpp"

#include "utility/expect.hpp"
#include "utility/binaryio.hpp"

#include "math/math.hpp"
#include "math/geometry.hpp"
#include "math/transform.hpp"

#include "imgproc/scanconversion.hpp"

#include "half/half.hpp"

#include "../storage/error.hpp"

#include "mesh.hpp"
#include "meshio.hpp"
#include "multifile.hpp"
#include "math.hpp"
#include "tileindex.hpp"

namespace fs = boost::filesystem;
namespace bio = boost::iostreams;
namespace bin = utility::binaryio;

namespace half = half_float::detail;

namespace vtslibs { namespace vts {

namespace {

/** Geo coordinates to coverage mask mapping.
 * NB: result is in pixel system: pixel centers have integral indices
 */
math::Matrix4 geo2mask(const math::Extents2 &extents
                              , const math::Size2 &gridSize)
{
    math::Matrix4 trafo(boost::numeric::ublas::identity_matrix<double>(4));

    auto es(size(extents));

    // scales
    math::Size2f scale(gridSize.width / es.width
                       , gridSize.height / es.height);

    // scale to grid
    trafo(0, 0) = scale.width;
    trafo(1, 1) = -scale.height;

    // move to origin
    trafo(0, 3) = -extents.ll(0) * scale.width;
    trafo(1, 3) = extents.ur(1) * scale.height;

    return trafo;
}

void updateCoverage(Mesh::CoverageMask &cm, const SubMesh &sm
                    , const math::Extents2 &sdsExtents
                    , std::uint8_t smIndex)
{
    const auto rasterSize(cm.size());
    auto trafo(geo2mask(sdsExtents, rasterSize));

    std::vector<imgproc::Scanline> scanlines;
    cv::Point3f tri[3];
    for (const auto &face : sm.faces) {
        for (int i : { 0, 1, 2 }) {
            auto p(transform(trafo, sm.vertices[face[i]]));
            tri[i].x = p(0); tri[i].y = p(1); tri[i].z = p(2);
        }

        scanlines.clear();
        imgproc::scanConvertTriangle(tri, 0, rasterSize.height, scanlines);

        for (const auto &sl : scanlines) {
            imgproc::processScanline
                (sl, 0, rasterSize.width, [&](int x, int y, float)
            {
                cm.set(x, y, smIndex + 1);
            });
        }
    }
}

} // namespace

void updateCoverage(Mesh &mesh, const SubMesh &sm
                    , const math::Extents2 &sdsExtents
                    , std::uint8_t smIndex)
{
    updateCoverage(mesh.coverageMask, sm, sdsExtents, smIndex);
}

void generateCoverage(Mesh &mesh, const math::Extents2 &sdsExtents)
{
    mesh.createCoverage(false);

    std::uint8_t smIndex(0);
    for (const auto &sm : mesh) {
        updateCoverage(mesh, sm, sdsExtents, smIndex++);
    }
}

void generateMeshMask(MeshMask &mask, const Mesh &mesh
                      , const math::Extents2 &sdsExtents)
{
    mask.createCoverage(false);
    mask.surfaceReferences.clear();

    std::uint8_t smIndex(0);
    for (const auto &sm : mesh) {
        updateCoverage(mask.coverageMask, sm, sdsExtents, smIndex++);
        mask.surfaceReferences.push_back(sm.surfaceReference);
    }
}

} } // namespace vtslibs::vts


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_COMPARE_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_COMPARE_EQUAL_HPP_INCLUDED

#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/reduction/functions/compare_equal.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                              ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return result_type(_mm_movemask_pd(eq(a0,a1)) == 0X03); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                              ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return  result_type(_mm_movemask_ps(eq(a0,a1)) == 0X0F); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<integer_<A0>,boost::simd::tag::sse_>))
                              ((simd_<integer_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return  result_type(_mm_movemask_epi8(eq(a0,a1)) == 0X0FFFF);
    }
  };
} } }

#endif
#endif


#define BOOST_TEST_MODULE "test_read_afm_fitting_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/forcefield/AFMFit/AFMFitInteraction.hpp>
#include <mjolnir/input/read_external_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_afm_fitting_interaction)
{
    mjolnir::LoggerManager::set_default_logger("test_read_afm_fitting_interaction.log");
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using namespace toml::literals;
    const toml::value v = u8R"(
        interaction = "AFMFlexibleFitting"
        k           = 100.0
        gamma       =   1.0
        pixel_x     =  10.0
        pixel_y     =  10.0
        sigma_x     =   2.0
        sigma_y     =   2.0
        length_x    =   5
        length_y    =   5
        z0          = 0.0
        cutoff      = 5.0
        margin      = 0.5
        image       = [
            0.0, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.5, 1.0, 0.5,
            0.0, 0.0, 1.0, 2.0, 1.0,
            0.0, 0.0, 0.5, 1.0, 0.5,
        ]
        parameters  = [
        {index = 0, radius = 1.0},
        {index = 1, radius = 2.0},
        {index = 4, radius = 3.0},
        {index = 5, radius = 4.0},
        ]
    )"_toml;

    const auto base = mjolnir::read_external_interaction<traits_type>(v);
    BOOST_TEST(static_cast<bool>(base));

    const auto derv = dynamic_cast<
        const mjolnir::AFMFitInteraction<traits_type>*>(base.get());
    BOOST_TEST(static_cast<bool>(derv));

    BOOST_TEST(derv->k() == 100.0);
    BOOST_TEST(derv->gamma   () ==  1.0);
    BOOST_TEST(derv->pixel_x () == 10.0);
    BOOST_TEST(derv->pixel_y () == 10.0);
    BOOST_TEST(derv->sigma_x () ==  2.0);
    BOOST_TEST(derv->sigma_y () ==  2.0);
    BOOST_TEST(derv->length_x() ==  5u );
    BOOST_TEST(derv->length_y() ==  5u );
    BOOST_TEST(derv->z0      () ==  0.0);
    BOOST_TEST(derv->cutoff  () ==  5.0);
    BOOST_TEST(derv->margin  () ==  0.5);

    BOOST_TEST_REQUIRE(derv->participants().size() == 4u);
    BOOST_TEST_REQUIRE(derv->participants().at(0)  == 0u);
    BOOST_TEST_REQUIRE(derv->participants().at(1)  == 1u);
    BOOST_TEST_REQUIRE(derv->participants().at(2)  == 4u);
    BOOST_TEST_REQUIRE(derv->participants().at(3)  == 5u);

    BOOST_TEST(derv->parameters().at(0) == 1.0);
    BOOST_TEST(derv->parameters().at(1) == 2.0);
    BOOST_TEST(derv->parameters().at(4) == 3.0);
    BOOST_TEST(derv->parameters().at(5) == 4.0);

    BOOST_TEST(derv->image().at( 0) == 0.0);
    BOOST_TEST(derv->image().at( 1) == 0.0);
    BOOST_TEST(derv->image().at( 2) == 0.0);
    BOOST_TEST(derv->image().at( 3) == 0.0);
    BOOST_TEST(derv->image().at( 4) == 0.0);
    BOOST_TEST(derv->image().at( 5) == 0.0);
    BOOST_TEST(derv->image().at( 6) == 0.0);
    BOOST_TEST(derv->image().at( 7) == 0.0);
    BOOST_TEST(derv->image().at( 8) == 0.0);
    BOOST_TEST(derv->image().at( 9) == 0.0);
    BOOST_TEST(derv->image().at(10) == 0.0);
    BOOST_TEST(derv->image().at(11) == 0.0);
    BOOST_TEST(derv->image().at(12) == 0.5);
    BOOST_TEST(derv->image().at(13) == 1.0);
    BOOST_TEST(derv->image().at(14) == 0.5);
    BOOST_TEST(derv->image().at(15) == 0.0);
    BOOST_TEST(derv->image().at(16) == 0.0);
    BOOST_TEST(derv->image().at(17) == 1.0);
    BOOST_TEST(derv->image().at(18) == 2.0);
    BOOST_TEST(derv->image().at(19) == 1.0);
    BOOST_TEST(derv->image().at(20) == 0.0);
    BOOST_TEST(derv->image().at(21) == 0.0);
    BOOST_TEST(derv->image().at(22) == 0.5);
    BOOST_TEST(derv->image().at(23) == 1.0);
    BOOST_TEST(derv->image().at(24) == 0.5);
}


/*
 * Copyright 2020 Robert Bosch GmbH
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
/**
 * \file noisy_object_sensor.cpp
 */

#include <Eigen/Geometry>  // for Isometry3d, Vector3d
#include <memory>          // for shared_ptr<>
#include <random>          // for random_device
#include <string>          // for string

#include <cloe/component.hpp>                // for Component, Json
#include <cloe/component/frustum.hpp>        // for Frustum
#include <cloe/component/object.hpp>         // for Object
#include <cloe/component/object_sensor.hpp>  // for ObjectSensor
#include <cloe/conf/action.hpp>              // for actions::ConfigureFactory
#include <cloe/plugin.hpp>                   // for EXPORT_CLOE_PLUGIN
#include <cloe/registrar.hpp>                // for Registrar
#include <cloe/sync.hpp>                     // for Sync
#include <cloe/trigger/set_action.hpp>       // for actions::SetVariableActionFactory
#include "noise_data.hpp"                    // for NoiseData, NoiseConf

namespace cloe {

enum class ObjectField { Translation, Velocity, Acceleration };

// clang-format off
ENUM_SERIALIZATION(ObjectField, ({
    {ObjectField::Translation, "translation"},
    {ObjectField::Velocity, "velocity"},
    {ObjectField::Acceleration, "acceleration"},
}))
// clang-format on

namespace component {

void apply_noise_xy(Eigen::Vector3d* vec, const NoiseConf& noise) {
  vec->x() = vec->x() + noise.get();
  vec->y() = vec->y() + noise.get();
}

void add_noise_translation(Object* obj, const NoiseConf* noise) {
  Eigen::Vector3d transl = obj->pose.translation();
  apply_noise_xy(&transl, *noise);
  obj->pose.translation() = transl;
}

void add_noise_velocity(Object* obj, const NoiseConf* noise) {
  Eigen::Vector3d vel = obj->velocity;
  apply_noise_xy(&vel, *noise);
  obj->velocity = vel;
}

void add_noise_acceleration(Object* obj, const NoiseConf* noise) {
  Eigen::Vector3d accel = obj->acceleration;
  apply_noise_xy(&accel, *noise);
  obj->acceleration = accel;
}

class ObjectNoiseConf : public NoiseConf {
 public:
  ObjectNoiseConf() = default;

  virtual ~ObjectNoiseConf() noexcept = default;

  /**
   * Add noise to target parameter.
   */
  std::function<void(Object*)> apply;

  /**
   * Set the appropriate target function.
   */
  void set_target() {
    using namespace std::placeholders;  // for _1
    switch (target_) {
      case ObjectField::Translation:
        apply = std::bind(add_noise_translation, _1, this);
        break;
      case ObjectField::Velocity:
        apply = std::bind(add_noise_velocity, _1, this);
        break;
      case ObjectField::Acceleration:
        apply = std::bind(add_noise_acceleration, _1, this);
        break;
    }
  }

  CONFABLE_SCHEMA(ObjectNoiseConf) {
    return Schema{
        NoiseConf::schema_impl(),
        fable::schema::PropertyList<fable::schema::Box>{
            // clang-format off
            {"target", Schema(&target_, "data field of the object the noise should be applied to")},
            // clang-format on
        },
    };
  }

  void to_json(Json& j) const override {
    NoiseConf::to_json(j);
    j = Json{
        {"target", target_},
    };
  }

 private:
  ObjectField target_{ObjectField::Translation};
};

struct NoisyObjectSensorConf : public NoisySensorConf {
  /// List of noisy object parameters.
  std::vector<ObjectNoiseConf> noisy_params;

  CONFABLE_SCHEMA(NoisyObjectSensorConf) {
    return Schema{
        NoisySensorConf::schema_impl(),
        fable::schema::PropertyList<fable::schema::Box>{
            // clang-format off
              {"noise", Schema(&noisy_params, "configure noisy parameters")},
            // clang-format on
        },
    };
  }

  void to_json(Json& j) const override {
    NoisySensorConf::to_json(j);
    j = Json{
        {"noise", noisy_params},
    };
  }
};

class NoisyObjectSensor : public ObjectSensor {
 public:
  NoisyObjectSensor(const std::string& name, const NoisyObjectSensorConf& conf,
                    std::shared_ptr<ObjectSensor> obs)
      : ObjectSensor(name), config_(conf), sensor_(obs) {
    reset_random();
  }

  virtual ~NoisyObjectSensor() noexcept = default;

  const Objects& sensed_objects() const override {
    if (cached_) {
      return objects_;
    }
    for (const auto& o : sensor_->sensed_objects()) {
      auto obj = apply_noise(o);
      if (obj) {
        objects_.push_back(obj);
      }
    }
    cached_ = true;
    return objects_;
  }

  const Frustum& frustum() const override { return sensor_->frustum(); }

  const Eigen::Isometry3d& mount_pose() const override { return sensor_->mount_pose(); }

  /**
   * Process the underlying sensor and clear the cache.
   *
   * We could process and create the filtered list of objects now, but we can
   * also delay it (lazy computation) and only do it when absolutely necessary.
   * This comes at the minor cost of checking whether cached_ is true every
   * time sensed_objects() is called.
   */
  Duration process(const Sync& sync) override {
    // This currently shouldn't do anything, but this class acts as a prototype
    // for How It Should Be Done.
    Duration t = ObjectSensor::process(sync);
    if (t < sync.time()) {
      return t;
    }

    // Process the underlying sensor and clear the cache.
    t = sensor_->process(sync);
    clear_cache();
    return t;
  }

  void reset() override {
    ObjectSensor::reset();
    sensor_->reset();
    clear_cache();
    reset_random();
  }

  void abort() override {
    ObjectSensor::abort();
    sensor_->abort();
  }

  void enroll(Registrar& r) override {
    r.register_action(std::make_unique<actions::ConfigureFactory>(
        &config_, "config", "configure noisy object component"));
    r.register_action<actions::SetVariableActionFactory<bool>>(
        "noise_activation", "switch sensor noise on/off", "enable", &config_.enabled);
  }

 protected:
  std::shared_ptr<Object> apply_noise(const std::shared_ptr<Object>& o) const {
    if (!config_.enabled) {
      return o;
    }
    auto obj = std::make_shared<Object>(*o);

    for (auto& np : config_.noisy_params) {
      np.apply(obj.get());
    }
    return obj;
  }

  void reset_random() {
    // Reset the sensor's "master" seed, if applicable.
    unsigned long seed = config_.seed;
    if (seed == 0) {
      std::random_device r;
      do {
        seed = r();
      } while (seed == 0);

      if (config_.reuse_seed) {
        config_.seed = seed;
      }
    }
    for (auto& np : config_.noisy_params) {
      np.set_target();
      np.reset(seed);
      ++seed;
    }
  }

  void clear_cache() {
    objects_.clear();
    cached_ = false;
  }

 private:
  NoisyObjectSensorConf config_;
  std::shared_ptr<ObjectSensor> sensor_;
  mutable bool cached_;
  mutable Objects objects_;
};

DEFINE_COMPONENT_FACTORY(NoisyObjectSensorFactory, NoisyObjectSensorConf, "noisy_object_sensor",
                         "add gaussian noise to object sensor output")

DEFINE_COMPONENT_FACTORY_MAKE(NoisyObjectSensorFactory, NoisyObjectSensor, ObjectSensor)

}  // namespace component
}  // namespace cloe

EXPORT_CLOE_PLUGIN(cloe::component::NoisyObjectSensorFactory)