/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "mitkHandeyeCalibrateFromDirectory.h"
#include "mitkCameraCalibrationFacade.h"
#include "mitkHandeyeCalibrate.h"
#include <mitkOpenCVFileIOUtils.h>
#include <mitkOpenCVMaths.h>
#include <ios>
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <ctime>
#include <opencv2/opencv.hpp>
#include <highgui.h>
#include <niftkFileHelper.h>

#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

namespace mitk {

//-----------------------------------------------------------------------------
HandeyeCalibrateFromDirectory::HandeyeCalibrateFromDirectory()
: m_FramesToUse(30)
, m_FramesToUseFactor(2)
, m_StickToFramesToUse(false)
, m_SaveProcessedVideoData(true)
, m_VideoInitialised(false)
, m_TrackingDataInitialised(false)
, m_TrackerIndex(0)
, m_AbsTrackerTimingError(20e6) // 20 milliseconds
, m_WriteOutChessboards(false)
, m_WriteOutCalibrationImages(true)
, m_NoVideoSupport(false)
, m_SwapVideoChannels(false)
, m_IntrinsicMatrixLeft(cvCreateMat(3,3,CV_64FC1))
, m_IntrinsicMatrixRight(cvCreateMat(3,3,CV_64FC1))
, m_DistortionCoefficientsLeft(cvCreateMat(1,4,CV_64FC1))
, m_DistortionCoefficientsRight(cvCreateMat(1,4,CV_64FC1))
, m_RotationMatrixRightToLeft(cvCreateMat(3,3,CV_64FC1))
, m_RotationVectorRightToLeft(cvCreateMat(3,1,CV_64FC1))
, m_TranslationVectorRightToLeft(cvCreateMat(3,1,CV_64FC1))
, m_OptimiseIntrinsics(true)
, m_OptimiseRightToLeft(true)
, m_Randomise(false)
, m_ChessBoardToTracker(NULL)
{
  m_PixelScaleFactor.Fill(1);
  cvSetIdentity(m_IntrinsicMatrixLeft);
  cvSetIdentity(m_IntrinsicMatrixRight);
  cvSetZero(m_DistortionCoefficientsLeft);
  cvSetZero(m_DistortionCoefficientsRight);
  cvSetIdentity(m_RotationMatrixRightToLeft);
  cvSetZero(m_TranslationVectorRightToLeft);
}

//-----------------------------------------------------------------------------
HandeyeCalibrateFromDirectory::~HandeyeCalibrateFromDirectory()
{
  cvReleaseMat(&m_IntrinsicMatrixLeft);
  cvReleaseMat(&m_IntrinsicMatrixRight);
  cvReleaseMat(&m_DistortionCoefficientsLeft);
  cvReleaseMat(&m_DistortionCoefficientsRight);
  cvReleaseMat(&m_RotationMatrixRightToLeft);
  cvReleaseMat(&m_TranslationVectorRightToLeft);
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::SetChessBoardToTracker(vtkMatrix4x4* matrix)
{
  m_ChessBoardToTracker = matrix;
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::SetOutputDirectory(const std::string& outputDir)
{
  if (outputDir.size() == 0)
  {
    m_OutputDirectory = m_InputDirectory;
  }
  else
  {
    m_OutputDirectory = outputDir;
  }
  this->Modified();
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::SetInputDirectory(const std::string& inputDir)
{
  if (inputDir.size() == 0)
  {
    m_InputDirectory = m_OutputDirectory;
  }
  else
  {
    m_InputDirectory = inputDir;
  }
  this->Modified();
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::InitialiseOutputDirectory()
{
  if (!niftk::DirectoryExists(m_OutputDirectory))
  {
    if (!niftk::CreateDirAndParents(m_OutputDirectory))
    {
      throw std::runtime_error("Failed to create output directory");
    }
  }
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::InitialiseVideo()
{
  std::vector<std::string> filenames = niftk::FindVideoData(m_InputDirectory);
  if ( filenames.size() == 0 )
  {
    MITK_ERROR << "Failed to find any video files";
    m_VideoInitialised = false;
    return;
  }
  if ( filenames.size() > 1 )
  {
    MITK_ERROR << "Found too many video files. ";
    for ( unsigned int  i = 0 ; i < filenames.size() ; i++ )
    {
      MITK_ERROR << filenames[i];
    }
    m_VideoInitialised = false;
    return;
  }

  MITK_INFO << "Loading video frames from " << filenames[0];
  LoadVideoData (filenames[0]);
  return;

}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::InitialiseTracking()
{
  if ( m_Matcher.IsNull() )
  {
    MITK_INFO << "Initialising Video Tracker Matcher";
    m_Matcher = mitk::VideoTrackerMatching::New();
  }
  if ( ! m_Matcher->IsReady() )
  {
    m_Matcher->Initialise (m_InputDirectory);
  }

  if ( m_Matcher->IsReady() )
  {
    m_TrackingDataInitialised = true;
  }
}
//-----------------------------------------------------------------------------
bool HandeyeCalibrateFromDirectory::LoadExistingIntrinsicCalibrations(std::string directory)
{

  cv::Mat litemp = cv::Mat(m_IntrinsicMatrixLeft);
  cv::Mat ldtemp = cv::Mat(m_DistortionCoefficientsLeft);
  cv::Mat ritemp = cv::Mat(m_IntrinsicMatrixRight);
  cv::Mat rdtemp = cv::Mat(m_DistortionCoefficientsRight);

  std::string leftIntrinsicName("calib.left.intrinsic.txt");
  boost::filesystem::path leftIntrinsicNameFull (directory);
  leftIntrinsicNameFull /= leftIntrinsicName;

  std::string rightIntrinsicName("calib.right.intrinsic.txt");
  boost::filesystem::path rightIntrinsicNameFull (directory);
  rightIntrinsicNameFull /= rightIntrinsicName;

  mitk::LoadCameraIntrinsicsFromPlainText(leftIntrinsicNameFull.string(), &litemp, &ldtemp);
  mitk::LoadCameraIntrinsicsFromPlainText(rightIntrinsicNameFull.string(), &ritemp, &rdtemp);
  *m_IntrinsicMatrixLeft = CvMat(litemp);
  *m_DistortionCoefficientsLeft = CvMat(ldtemp);
  *m_IntrinsicMatrixRight = CvMat(ritemp);
  *m_DistortionCoefficientsRight = CvMat(rdtemp);
  m_OptimiseIntrinsics=false;
  return true;
}

//-----------------------------------------------------------------------------
bool HandeyeCalibrateFromDirectory::LoadExistingRightToLeft(const std::string& directoryName)
{
  cv::Mat r2lr = cv::Mat(m_RotationMatrixRightToLeft);
  cv::Mat r2lt = cv::Mat(m_TranslationVectorRightToLeft);

  mitk::LoadStereoTransformsFromPlainText(niftk::ConcatenatePath(directoryName, "calib.r2l.txt"), &r2lr, &r2lt);

  *m_RotationMatrixRightToLeft = CvMat(r2lr);
  *m_TranslationVectorRightToLeft = CvMat(r2lt);

  m_OptimiseRightToLeft = false;
  return true;
}

//-----------------------------------------------------------------------------
void HandeyeCalibrateFromDirectory::LoadVideoData(std::string filename)
{
  if ( m_NoVideoSupport )
  {
    MITK_WARN << "Ran load video without video support, returning.";
    return;
  }

  cv::VideoCapture *capture;
  try
  {
    bool ignoreVideoReadFailure = false;
    capture = mitk::InitialiseVideoCapture(filename, ignoreVideoReadFailure) ;
  }
  catch (std::exception& e)
  {
    MITK_ERROR << "Caught std::exception:" << e.what();
    exit(1);
  }

  if ( ! capture->isOpened() )
  {
    MITK_ERROR << "Failed to open " << filename;
    return;
  }

  if ( ! m_TrackingDataInitialised )
  {
    InitialiseTracking();
  }

  //get frame count doesn't work for 264 files, which are just
  //raw data get the frame count from the framemap log
  int numberOfFrames = m_Matcher->GetNumberOfFrames();

  double framewidth = capture->get(CV_CAP_PROP_FRAME_WIDTH);
  double frameheight = capture->get(CV_CAP_PROP_FRAME_HEIGHT);

  double filesize = numberOfFrames * framewidth * frameheight * 4 / 1e9;
  MITK_INFO << numberOfFrames << "frames in video : " << framewidth << "x" << frameheight;;
  MITK_INFO << filesize << "gigabytes required to store";

  //go through data set, randomly selecting frames from a uniform distribution until
  //sufficient frames are found.
  //First check tracking data is ok (based on timing error threshold)
  //Then try and extract corners in both left and right frames.
  //std::default_random_engine generator;
  //std::uniform_int_distribution<int> distribution (0, numberOfFrames/2);

  if (m_Randomise)
  {
    std::srand(std::time(NULL));
  }
  else
  {
    std::srand(0);
  }

  std::vector <int> leftFramesToCheck;
  std::vector <int> rightFramesToCheck;
  while ( leftFramesToCheck.size() < m_FramesToUse*m_FramesToUseFactor )
  {
    int frameToUse =  std::rand()%(numberOfFrames/2);

    //first check it's not already in array
    if ( (std::find(leftFramesToCheck.begin(), leftFramesToCheck.end(), frameToUse * 2 ) == leftFramesToCheck.end()) )
    {
      MITK_INFO << "Checking tracking for frame pair " << frameToUse * 2 << "," << frameToUse*2 +1;

      long long int  leftTimingError;
      long long int  rightTimingError;
      cv::Mat leftTrackingMatrix = m_Matcher->GetTrackerMatrix(frameToUse * 2 ,
        &leftTimingError, m_TrackerIndex );
      cv::Mat rightTrackingMatrix = m_Matcher->GetTrackerMatrix(frameToUse * 2 + 1 ,
        &rightTimingError, m_TrackerIndex );
      if ( std::abs(leftTimingError) > m_AbsTrackerTimingError ||
        std::abs(rightTimingError) > m_AbsTrackerTimingError )
      {
        MITK_INFO << "Rejecting frame " << frameToUse << "Due to high timing error: " <<
          std::abs(leftTimingError) << " > " <<  m_AbsTrackerTimingError;
      }
      else
      {
        //timing error OK, now check if we can extract corners
        leftFramesToCheck.push_back (frameToUse *2);
        rightFramesToCheck.push_back (frameToUse * 2 + 1);
      }
    }
  }

  MITK_INFO << "Got " << leftFramesToCheck.size() << " with accurate enough tracking.";

  //sort the vector in ascending order
  std::sort (leftFramesToCheck.begin(), leftFramesToCheck.end());
  std::sort (rightFramesToCheck.begin(), rightFramesToCheck.end());

  //now go through video once and check if a chessboard can be found.
  int frameNumber = 0 ;
  cv::Size imageSize;
  int numberOfGoodFrames = 0;

  std::vector<cv::Mat>  allLeftImagePoints;
  std::vector<cv::Mat>  allLeftObjectPoints;
  std::vector<cv::Mat>  allRightImagePoints;
  std::vector<cv::Mat>  allRightObjectPoints;
  std::vector<cv::Mat>  allLeftFrames;
  std::vector<cv::Mat>  allRightFrames;
  std::vector<cv::Mat>  allLeftChessBoards;
  std::vector<cv::Mat>  allRightChessBoards;
  std::vector <int>     allLeftFrameNumbers;
  std::vector <int>     allRightFrameNumbers;

  while ( frameNumber < numberOfFrames)
  {
    cv::Mat tempFrame;
    cv::Mat leftFrame;
    cv::Mat leftFrameOrig;
    cv::Mat rightFrame;
    cv::Mat rightFrameOrig;

    *capture >> tempFrame;
    if (!m_SwapVideoChannels)
      leftFrame = tempFrame.clone();
    else
      rightFrame = tempFrame.clone();
    *capture >> tempFrame;
    if (!m_SwapVideoChannels)
      rightFrame = tempFrame.clone();
    else
      leftFrame = tempFrame.clone();

    leftFrameOrig = leftFrame.clone();
    rightFrameOrig = rightFrame.clone();

    imageSize = rightFrame.size();

    if ( (std::find(leftFramesToCheck.begin(), leftFramesToCheck.end(), frameNumber) != leftFramesToCheck.end()) )
    {
      if ((std::find(rightFramesToCheck.begin(), rightFramesToCheck.end(), frameNumber + 1) != rightFramesToCheck.end()) )
      {
        std::vector <cv::Point2d>* leftImageCorners = new std::vector<cv::Point2d>;
        std::vector <cv::Point3d>* leftObjectCorners = new std::vector<cv::Point3d>;
        bool leftOK = mitk::ExtractChessBoardPoints (
          leftFrame, m_NumberCornersWidth,
          m_NumberCornersHeight,
          true, m_SquareSizeInMillimetres, m_PixelScaleFactor,
          *leftImageCorners, *leftObjectCorners);

        std::vector <cv::Point2d>* rightImageCorners = new std::vector<cv::Point2d>;
        std::vector <cv::Point3d>* rightObjectCorners = new std::vector<cv::Point3d>;
        bool rightOK = mitk::ExtractChessBoardPoints (
          rightFrame, m_NumberCornersWidth,
          m_NumberCornersHeight,
          true, m_SquareSizeInMillimetres, m_PixelScaleFactor,
          *rightImageCorners, *rightObjectCorners);

        if ( leftOK && rightOK )
        {

          MITK_INFO << "Frame " << capture->get(CV_CAP_PROP_POS_FRAMES)-2 << " got " << leftImageCorners->size() << " corners for both images";

          allLeftImagePoints.push_back(cv::Mat(*leftImageCorners,true));
          allLeftObjectPoints.push_back(cv::Mat(*leftObjectCorners,true));
          allRightImagePoints.push_back(cv::Mat(*rightImageCorners,true));
          allRightObjectPoints.push_back(cv::Mat(*rightObjectCorners,true));
          allLeftFrames.push_back(leftFrameOrig);
          allRightFrames.push_back(rightFrameOrig);
          allLeftChessBoards.push_back(leftFrame);
          allRightChessBoards.push_back(rightFrame);
          allLeftFrameNumbers.push_back(frameNumber);
          allRightFrameNumbers.push_back(frameNumber + 1);
          numberOfGoodFrames++;
        }
        else
        {
          MITK_INFO << "Frame " <<  capture->get(CV_CAP_PROP_POS_FRAMES)-2 << " failed corner extraction. Removing from good frame buffer [" << leftImageCorners->size() << "," << rightImageCorners->size() << "].";

          std::vector<int>::iterator newEnd = std::remove(leftFramesToCheck.begin(), leftFramesToCheck.end(), frameNumber);
          leftFramesToCheck.erase(newEnd, leftFramesToCheck.end());
          newEnd = std::remove(rightFramesToCheck.begin(), rightFramesToCheck.end(), frameNumber+1);
          rightFramesToCheck.erase(newEnd, rightFramesToCheck.end());
        }

        // buffer contents are copied and stuffed into allLeftImagePoints, etc.
        delete leftImageCorners;
        delete leftObjectCorners;
        delete rightImageCorners;
        delete rightObjectCorners;
      }
      else
      {
        MITK_ERROR << "Left right frame mismatch" ;
        return;
      }
    }
    frameNumber++;
    frameNumber++;
  }

  MITK_INFO << "Got " << allLeftFrameNumbers.size() << " pairs of valid chessboards.";

  int indexToUse = 0;
  int desiredNumberOfFrames = allLeftFrameNumbers.size();
  if (m_StickToFramesToUse)
  {
    desiredNumberOfFrames = m_FramesToUse;
  }

  // If there are not enough valid chessboards, we warn or throw.
  if (allLeftFrameNumbers.size() < desiredNumberOfFrames)
  {
    std::ostringstream oss;
    oss << "The number of chessboards (" << allLeftFrameNumbers.size() << ") is less than the number required (" << m_FramesToUse << ")";

    if (m_StickToFramesToUse)
    {
      mitkThrow() << oss.str();
    }
    else
    {
      MITK_WARN << oss.str();
    }
  }

  // Now randomly select from the list of available chessboards
  int numberOfChosenFrames = 0;
  std::set<int> setOfChosenIndexes;
  std::vector <int> leftFramesToUse;
  std::vector <int> rightFramesToUse;
  std::vector<cv::Mat> chosenLeftImagePoints;
  std::vector<cv::Mat> chosenLeftObjectPoints;
  std::vector<cv::Mat> chosenRightImagePoints;
  std::vector<cv::Mat> chosenRightObjectPoints;

  // Iterate round accumulating frames.
  while (numberOfChosenFrames < desiredNumberOfFrames)
  {
    // If we have more frames than we need, and we MUST stick to the number specified, pick randomly.
    if ((desiredNumberOfFrames < allLeftFrameNumbers.size())
        && m_StickToFramesToUse)
    {
      indexToUse = std::rand() % allLeftFrameNumbers.size();
    }
    // Otherwise, use them all.
    else
    {
      indexToUse = numberOfChosenFrames;
    }

    if (setOfChosenIndexes.find(indexToUse) == setOfChosenIndexes.end())
    {
      setOfChosenIndexes.insert(indexToUse);
      numberOfChosenFrames++;

      MITK_INFO << "Selecting image " << numberOfChosenFrames << " as frame " << allLeftFrameNumbers[indexToUse] << ", " << allRightFrameNumbers[indexToUse];

      // We have randomly selected from the list of available chessboards.
      // So, copy to the buffers that will be required for calibration.
      leftFramesToUse.push_back(allLeftFrameNumbers[indexToUse]);
      rightFramesToUse.push_back(allRightFrameNumbers[indexToUse]);
      chosenLeftImagePoints.push_back(allLeftImagePoints[indexToUse]);
      chosenLeftObjectPoints.push_back(allLeftObjectPoints[indexToUse]);
      chosenRightImagePoints.push_back(allRightImagePoints[indexToUse]);
      chosenRightObjectPoints.push_back(allRightObjectPoints[indexToUse]);

      if ( m_WriteOutCalibrationImages )
      {
        std::string leftFilename = m_OutputDirectory + "/LeftFrame" + boost::lexical_cast<std::string>(numberOfChosenFrames) + ".png";
        MITK_INFO << "Writing image to " << leftFilename;
        cv::imwrite( leftFilename, allLeftFrames[indexToUse] );

        std::string rightFilename = m_OutputDirectory + "/RightFrame" + boost::lexical_cast<std::string>(numberOfChosenFrames) + ".png";
        MITK_INFO << "Writing image to " << rightFilename;
        cv::imwrite( rightFilename, allRightFrames[indexToUse] );
      }
      if ( m_WriteOutChessboards )
      {
        std::string leftFilename = m_OutputDirectory + "/LeftChessboard" + boost::lexical_cast<std::string>(allLeftFrameNumbers[indexToUse]) + ".png";
        MITK_INFO << "Writing image to " << leftFilename;
        cv::imwrite( leftFilename, allLeftChessBoards[indexToUse] );

        std::string rightFilename = m_OutputDirectory + "/RightChessboard" + boost::lexical_cast<std::string>(allRightFrameNumbers[indexToUse]) + ".png";
        MITK_INFO << "Writing image to " << rightFilename;
        cv::imwrite( rightFilename, allRightChessBoards[indexToUse] );
      }
    }
  }

  cv::Mat leftImagePoints (m_NumberCornersWidth * m_NumberCornersHeight * leftFramesToUse.size(),2,CV_64FC1);
  cv::Mat leftObjectPoints (m_NumberCornersWidth * m_NumberCornersHeight * leftFramesToUse.size(),3,CV_64FC1);
  cv::Mat rightImagePoints (m_NumberCornersWidth * m_NumberCornersHeight * leftFramesToUse.size(),2,CV_64FC1);
  cv::Mat rightObjectPoints (m_NumberCornersWidth * m_NumberCornersHeight * leftFramesToUse.size(),3,CV_64FC1);

  cv::Mat leftPointCounts (leftFramesToUse.size(),1,CV_32SC1);
  cv::Mat rightPointCounts (leftFramesToUse.size(),1,CV_32SC1);

  if  (chosenLeftImagePoints.size()   !=  leftFramesToUse.size() ||
       chosenLeftObjectPoints.size()  !=  leftFramesToUse.size() ||
       chosenRightImagePoints.size()  !=  leftFramesToUse.size() ||
       chosenRightObjectPoints.size() !=  leftFramesToUse.size() )
  {
    mitkThrow() << "Detected unequal matrix sizes";
  }

  for ( unsigned int i = 0 ; i < leftFramesToUse.size() ; i ++  )
  {
    unsigned int size1 = chosenLeftImagePoints[i].size().height;
    //FIX ME
    unsigned int size2 = chosenLeftObjectPoints[i].size().height;
    unsigned int size3 = chosenRightImagePoints[i].size().height;
    unsigned int size4 = chosenRightObjectPoints[i].size().height;

    if (size1 != m_NumberCornersWidth * m_NumberCornersHeight ||
        size2 != m_NumberCornersWidth * m_NumberCornersHeight ||
        size3 != m_NumberCornersWidth * m_NumberCornersHeight ||
        size4 != m_NumberCornersWidth * m_NumberCornersHeight)
    {
      mitkThrow() << "Detected unequal matrix sizes";
    }
  }

  for ( unsigned int i = 0 ; i < leftFramesToUse.size() ; i++ )
  {
    MITK_INFO << "Filling "  << i;
    for ( unsigned int j = 0 ; j < m_NumberCornersWidth * m_NumberCornersHeight ; j ++ )
    {
      leftImagePoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,0) =
        chosenLeftImagePoints[i].at<double>(j,0);
      leftImagePoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,1) =
        chosenLeftImagePoints[i].at<double>(j,1);

      leftObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,0) =
        chosenLeftObjectPoints[i].at<double>(j,0);
      leftObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,1) =
        chosenLeftObjectPoints[i].at<double>(j,1);
      leftObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,2) =
        chosenLeftObjectPoints[i].at<double>(j,2);

      rightImagePoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,0) =
        chosenRightImagePoints[i].at<double>(j,0);
      rightImagePoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,1) =
        chosenRightImagePoints[i].at<double>(j,1);

      rightObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,0) =
        chosenRightObjectPoints[i].at<double>(j,0);
      rightObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,1) =
        chosenRightObjectPoints[i].at<double>(j,1);
      rightObjectPoints.at<double>(i* m_NumberCornersWidth * m_NumberCornersHeight + j,2) =
        chosenRightObjectPoints[i].at<double>(j,2);

    }
    leftPointCounts.at<int>(i,0) = m_NumberCornersWidth * m_NumberCornersHeight;
    rightPointCounts.at<int>(i,0) = m_NumberCornersWidth * m_NumberCornersHeight;
  }
  if ( true )
  {
    std::string leftimagePointsfilename = m_OutputDirectory + "/LeftImagePoints.xml";
    std::string leftObjectPointsfilename = m_OutputDirectory + "/LeftObjectPoints.xml";
    std::string rightimagePointsfilename = m_OutputDirectory + "/RightImagePoints.xml";
    std::string rightObjectPointsfilename = m_OutputDirectory + "/RightObjectPoints.xml";
    std::string leftPointCountfilename = m_OutputDirectory + "/LeftPointCount.xml";
    std::string rightPointCountfilename = m_OutputDirectory + "/RightPointCount.xml";
    cv::FileStorage fs1;
    fs1.open(leftimagePointsfilename, cv::FileStorage::WRITE);
    cv::FileStorage fs2(leftObjectPointsfilename, cv::FileStorage::WRITE);
    cv::FileStorage fs3(rightimagePointsfilename, cv::FileStorage::WRITE);
    cv::FileStorage fs4(rightObjectPointsfilename, cv::FileStorage::WRITE);
    cv::FileStorage fs5(leftPointCountfilename, cv::FileStorage::WRITE);
    cv::FileStorage fs6(rightPointCountfilename, cv::FileStorage::WRITE);
    fs1 <<  "leftimagepoints" << leftImagePoints;
    fs2 <<  "leftobjectpoints" << leftObjectPoints;
    fs3 <<  "rightimagepoints" << rightImagePoints;
    fs4 <<  "rightobjectpoints" << rightObjectPoints;
    fs5 <<  "leftpointcounts" << leftPointCounts;
    fs6 <<  "rightpointcounts" << rightPointCounts;
    fs1.release();
    fs2.release();
    fs3.release();
    fs4.release();
    fs5.release();
    fs6.release();
  }

  MITK_INFO << "Starting intrinisic calibration";
  CvMat* outputRotationVectorsLeft = cvCreateMat(leftFramesToUse.size(),3,CV_64FC1);
  CvMat* outputTranslationVectorsLeft= cvCreateMat(leftFramesToUse.size(),3,CV_64FC1);
  CvMat* outputRotationVectorsRight= cvCreateMat(leftFramesToUse.size(),3,CV_64FC1);
  CvMat* outputTranslationVectorsRight= cvCreateMat(leftFramesToUse.size(),3,CV_64FC1);
  CvMat* outputEssentialMatrix = cvCreateMat(3,3,CV_64FC1);
  CvMat* outputFundamentalMatrix= cvCreateMat(3,3,CV_64FC1);

  double reprojectionError = mitk::CalibrateStereoCameraParameters(
      leftObjectPoints,
      leftImagePoints,
      leftPointCounts,
      imageSize,
      rightObjectPoints,
      rightImagePoints,
      rightPointCounts,
      *m_IntrinsicMatrixLeft,
      *m_DistortionCoefficientsLeft,
      *outputRotationVectorsLeft,
      *outputTranslationVectorsLeft,
      *m_IntrinsicMatrixRight,
      *m_DistortionCoefficientsRight,
      *outputRotationVectorsRight,
      *outputTranslationVectorsRight,
      *m_RotationMatrixRightToLeft,
      *m_TranslationVectorRightToLeft,
      *outputEssentialMatrix,
      *outputFundamentalMatrix,
      ! m_OptimiseIntrinsics,
      ! m_OptimiseRightToLeft
      );

  //write it out
  std::string leftIntrinsic = m_OutputDirectory + "/calib.left.intrinsic.txt";
  std::string rightIntrinsic = m_OutputDirectory + "/calib.right.intrinsic.txt";
  std::string rightToLeft = m_OutputDirectory + "/calib.r2l.txt";
  std::string extrinsic = m_OutputDirectory + "/leftextrinsics.txt";

  int outputPrecision = 10;
  int outputWidth = 10;

  std::ofstream fs_leftIntrinsic;
  fs_leftIntrinsic.precision(outputPrecision);
  fs_leftIntrinsic.width(outputWidth);

  std::ofstream fs_rightIntrinsic;
  fs_rightIntrinsic.precision(outputPrecision);
  fs_rightIntrinsic.width(outputWidth);

  std::ofstream fs_r2l;
  fs_r2l.precision(outputPrecision);
  fs_r2l.width(outputWidth);

  std::ofstream fs_ext;
  fs_ext.precision(outputPrecision);
  fs_ext.width(outputWidth);

  fs_leftIntrinsic.open(leftIntrinsic.c_str(), std::ios::out);
  fs_rightIntrinsic.open(rightIntrinsic.c_str(), std::ios::out);
  fs_r2l.open(rightToLeft.c_str(), std::ios::out);
  fs_ext.open(extrinsic.c_str(), std::ios::out);

  for ( int row = 0 ; row < 3 ; row ++ )
  {
    for ( int col = 0 ; col < 3 ; col ++ )
    {
      fs_leftIntrinsic << CV_MAT_ELEM (*m_IntrinsicMatrixLeft, double, row,col) << " ";
      fs_rightIntrinsic << CV_MAT_ELEM (*m_IntrinsicMatrixRight, double, row,col) << " ";
      fs_r2l << CV_MAT_ELEM (*m_RotationMatrixRightToLeft, double , row,col) << " ";
    }
    fs_leftIntrinsic << std::endl;
    fs_rightIntrinsic << std::endl;
    fs_r2l << std::endl;
  }
  for ( int i = 0 ; i < 4 ; i ++ )
  {
    fs_leftIntrinsic << CV_MAT_ELEM (*m_DistortionCoefficientsLeft, double , 0, i ) << " ";
    fs_rightIntrinsic << CV_MAT_ELEM (*m_DistortionCoefficientsRight, double , 0, i ) << " ";
  }
  fs_leftIntrinsic << std::endl;
  fs_rightIntrinsic << std::endl;

  for ( int i = 0 ; i < 3 ; i ++ )
  {
    fs_r2l << CV_MAT_ELEM (*m_TranslationVectorRightToLeft, double , i, 0 ) << " ";
  }
  fs_r2l << std::endl;

  fs_leftIntrinsic.close();
  fs_rightIntrinsic.close();
  fs_r2l.close();

  std::string trackerDirectory = m_OutputDirectory + "/TrackerMatrices" + boost::lexical_cast<std::string>(m_TrackerIndex);
  niftk::CreateDirAndParents(trackerDirectory);

  for ( unsigned int view = 0 ; view < leftFramesToUse.size() ; view ++ )
  {
    for ( int i = 0 ; i < 3 ; i ++ )
    {
      fs_ext << CV_MAT_ELEM ( *outputRotationVectorsLeft , double  , view, i) << " ";
    }
    for ( int i = 0 ; i < 3 ; i ++ )
    {
      fs_ext << CV_MAT_ELEM ( *outputTranslationVectorsLeft , double  , view, i) << " ";
    }
    fs_ext << std::endl;

    cv::Mat LeftTrackingMatrix = m_Matcher->GetTrackerMatrix(leftFramesToUse[view], NULL, m_TrackerIndex );

    std::string trackerFilename = trackerDirectory + "/" + boost::lexical_cast<std::string>(view) + ".txt";
    MITK_INFO << "Saving matrix for frame " << leftFramesToUse[view] << "to " << trackerFilename;
    std::ofstream fs_tracker;
    fs_tracker.open(trackerFilename.c_str(), std::ios::out);
    for ( int row = 0 ; row < 4 ; row ++ )
    {
      for ( int col = 0 ; col < 4 ; col ++ )
      {
        fs_tracker << LeftTrackingMatrix.at<double>(row,col) << " " ;
      }
      fs_tracker << std::endl;
    }
    fs_tracker.close();
  }
  fs_ext.close();

  m_VideoInitialised = true;

  // If user did not specify a chessboard to tracker registration, we do TSAIs method.
  if (m_ChessBoardToTracker == NULL)
  {
    Calibrate ( m_OutputDirectory + "/TrackerMatrices" + boost::lexical_cast<std::string>(m_TrackerIndex) , extrinsic);
  }
  else
  {
    // Otherwise, just do it using registration.
    cv::Matx44d chessBoardToTracker;
    mitk::CopyToOpenCVMatrix(*m_ChessBoardToTracker, chessBoardToTracker);
    std::vector<cv::Mat> handEyeMatrices;

    for ( unsigned int view = 0 ; view < leftFramesToUse.size() ; view ++ )
    {
      cv::Matx44d leftTrackingMatrix = m_Matcher->GetTrackerMatrix(leftFramesToUse[view], NULL, m_TrackerIndex );
      cv::Matx44d leftTrackingMatrixInverted = leftTrackingMatrix.inv(cv::DECOMP_SVD);

      cv::Matx13d rotationVector;
      cv::Matx33d rotationMatrix;
      rotationVector(0,0) = CV_MAT_ELEM ( *outputRotationVectorsLeft , double  , view, 0);
      rotationVector(0,1) = CV_MAT_ELEM ( *outputRotationVectorsLeft , double  , view, 1);
      rotationVector(0,2) = CV_MAT_ELEM ( *outputRotationVectorsLeft , double  , view, 2);
      cv::Rodrigues(rotationVector, rotationMatrix);

      cv::Matx44d chessboardToCamera;
      mitk::MakeIdentity(chessboardToCamera);
      for (int r = 0; r < 3; r++)
      {
        for (int c = 0; c < 3; c++)
        {
          chessboardToCamera(r,c) = rotationMatrix(r,c);
        }
        chessboardToCamera(r, 3) = CV_MAT_ELEM ( *outputTranslationVectorsLeft , double  , view, r);
      }
      cv::Matx44d chessboardToCameraInverted = chessboardToCamera.inv(cv::DECOMP_SVD);

      cv::Matx44d handEye = leftTrackingMatrixInverted * (chessBoardToTracker * chessboardToCameraInverted);
      cv::Mat he(handEye);
      handEyeMatrices.push_back(he);
    }

    cv::Mat averageHandEye = mitk::AverageMatrices(handEyeMatrices);
    m_CameraToMarker = averageHandEye;
    this->WriteHandEye();
  }

  cv::Mat handEyeRotationMatrix(m_CameraToMarker, cv::Range(0, 2), cv::Range(0,2));
  cv::Mat handEyeRotationVector(cvCreateMat(3,1,CV_64FC1));
  cv::Rodrigues(handEyeRotationMatrix, handEyeRotationVector);

  // for spreadsheet/analysis purposes. A big ugly, but amenable to grepping through log files.
  MITK_INFO << "Calibration Summary:"
            << m_FramesToUse << ", "
            << reprojectionError << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixLeft, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixLeft, double, 1,1) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixLeft, double, 0,2) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixLeft, double, 1,2) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsLeft, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsLeft, double, 0,1) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsLeft, double, 0,2) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsLeft, double, 0,3) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixRight, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixRight, double, 1,1) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixRight, double, 0,2) << ", "
            << CV_MAT_ELEM (*m_IntrinsicMatrixRight, double, 1,2) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsRight, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsRight, double, 0,1) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsRight, double, 0,2) << ", "
            << CV_MAT_ELEM (*m_DistortionCoefficientsRight, double, 0,3) << ", "
            << CV_MAT_ELEM (*m_RotationVectorRightToLeft, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_RotationVectorRightToLeft, double, 1,0) << ", "
            << CV_MAT_ELEM (*m_RotationVectorRightToLeft, double, 2,0) << ", "
            << CV_MAT_ELEM (*m_TranslationVectorRightToLeft, double, 0,0) << ", "
            << CV_MAT_ELEM (*m_TranslationVectorRightToLeft, double, 1,0) << ", "
            << CV_MAT_ELEM (*m_TranslationVectorRightToLeft, double, 2,0) << ", "
            << handEyeRotationVector.at<double>(0, 0) << ", "
            << handEyeRotationVector.at<double>(1, 0) << ", "
            << handEyeRotationVector.at<double>(2, 0) << ", "
            << m_CameraToMarker.at<double>(0,3) << ", "
            << m_CameraToMarker.at<double>(1,3) << ", "
            << m_CameraToMarker.at<double>(2,3);
}

} // end namespace


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 5, 4> test_double_5_4(const Eigen::Matrix<double, 5, 4>& M) { return M; }
void export_double_5_4() { boost::python::def("test_double_5_4", test_double_5_4); }


#include <boost/date_time/date_iterator.hpp>


/* Copyright 2019 The Spin-Scenario Authors. All Rights Reserved.
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.
==============================================================================*/

#include "phantom.h"
#include <boost/random.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <gnuplot-iostream.h>
#include <time.h>
#include <kernel/utilities/ssl_plot.h>

using namespace ssl::utility;
boost::mt19937 gen;
namespace ssl {
namespace sample {

phantom::phantom() {
}
phantom::phantom(std::string filename, std::string supp_filename) {
  load(filename, supp_filename);
  init_ensemble();
}
phantom::~phantom() {
}
void phantom::init_ensemble() {
  int x0 = -1;
  int x1 = -1;
  int y0 = -1;
  int y1 = -1;
  int z0 = -1;
  int z1 = -1;

  if (g_phantom_space.x0 < 0)
    x0 = 0;
  else
    x0 = g_phantom_space.x0;

  if (g_phantom_space.x1 < 0)
    x1 = dim_[cx] - 1;
  else
    x1 = g_phantom_space.x1;

  if (g_phantom_space.y0 < 0)
    y0 = 0;
  else
    y0 = g_phantom_space.y0;

  if (g_phantom_space.y1 < 0)
    y1 = dim_[cy] - 1;
  else
    y1 = g_phantom_space.y1;

  if (g_phantom_space.z0 < 0)
    z0 = 0;
  else
    z0 = g_phantom_space.z0;

  if (g_phantom_space.z1 < 0)
    z1 = dim_[cz] - 1;
  else
    z1 = g_phantom_space.z1;

  omp_set_num_threads(omp_core_num);
  std::vector<isochromat> *omp_isochromats = new std::vector<isochromat>[omp_core_num];
#pragma omp parallel for
      for (int nz = z0; nz <= z1; nz += g_phantom_space.dz) {
        int id = omp_get_thread_num();
        for (int nx = x0; nx <= x1; nx += g_phantom_space.dx)
          for (int ny = y0; ny <= y1; ny += g_phantom_space.dy) {
            {
              int idx = tissue_dist_(ny, nx, nz);
              // do not deal with background and those tissues not assigned.
              if (idx != 0 && tissue_index_(idx) == 1) {
                isochromat iso;
                iso.data[dB0] = dB0_(ny, nx, nz);
                iso.data[r1] = 1.0 / T1_(ny, nx, nz);
                iso.data[r2] = 1.0 / T2_(ny, nx, nz);
                //iso.data[r2s] = 1.0 / tissue_t1t2_(idx, 1);
                iso.data[cx] = (nx - dim_[cx] / 2 + 0.5) * res_[cx] + offset_[cx];
                iso.data[cy] = (ny - dim_[cy] / 2 + 0.5) * res_[cy] + offset_[cy];
                iso.data[cz] = (nz - dim_[cz] / 2 + 0.5) * res_[cz] + offset_[cz];
                omp_isochromats[id].push_back(iso);
              }

            }
          }
      }
  isochromats_.clear();
  for (int id = 0; id < omp_core_num; id++) {
    std::copy(omp_isochromats[id].begin(), omp_isochromats[id].end(),
              std::back_inserter(isochromats_));
    omp_isochromats[id].clear();
  }
  #ifdef SSL_OUTPUT_ENABLE
  std::string model;
  std::string s = str(boost::format("%s %s.\n") % "total isochromats:" % isochromats_.size());
  ssl_color_text("info", s);
#endif

}
void phantom::view(const sol::table &t) const {
  if (!t.valid() || t.empty())
    throw std::runtime_error("invalid phantom 'view' table parameters (nil or empty).");
  std::string axis;
  int slice = -1;
  std::string prop ="T2";
   if (is_retrievable("prop", t))
     prop = retrieve_table_str("prop", t, "phantom view");

  for (auto &kv : t) {
    axis = kv.first.as<std::string>();
    if (axis == "prop") continue;
    slice = kv.second.as<int>();
    view(axis, slice, prop);
  }
}
void phantom::view(const std::string &axis, int slice, std::string prop) const {
  int dim = -1; //z
  int max_dim = 0;
  std::string X, Y;
  if (axis == "x" || axis == "X") {
    dim = 1;
    max_dim = dim_[_cx];
    X = "Z";
    Y = "Y";
  }
  if (axis == "y" || axis == "Y") {
    dim = 0;
    max_dim = dim_[_cy];
    X = "Z";
    Y = "X";
  }
  if (axis == "z" || axis == "Z") {
    dim = 2;
    max_dim = dim_[_cz];
    X = "X";
    Y = "Y";
  }
  if (dim == -1) {
    std::string s = "unkown axis: " + axis + "!\n";
    throw std::runtime_error(s.c_str());
  }
  if (slice > max_dim) {
    std::string s = "slice num overflow: " + std::to_string(slice) + " max slice: " + std::to_string(max_dim)+"\n";
    ssl_color_text("err", s);
    return;
  }

  Eigen::Tensor<double, 2> sub;

  if(prop=="T2")
  sub = T2_.chip(slice - 1, dim);

  if(prop=="T1")
  sub = T1_.chip(slice - 1, dim);

  Eigen::Map<mat> m(sub.data(), sub.dimension(0), sub.dimension(1));
  utility::map transfer_map(m.matrix().cast<double>()*1e3); // ms.
  (*g_lua)["_map"] = transfer_map;
  std::string AXIS = axis;
  boost::to_upper(AXIS);
  std::string title = prop + " map of "+ X + Y + " plane view (" + AXIS + "=" + std::to_string(slice) + "/" + std::to_string(max_dim) + ")";
  g_lua->script("plot('title<" + title + "> xlabel<" + X + "> ylabel<" + Y + "> color<Spectral> gnuplot<set size ratio -1>', _map)");
}
void phantom::load(std::string filename, std::string supp_filename) {
  //model_ = unidentified_phantom;

  H5File file;
  file.openFile(filename.c_str(), H5F_ACC_RDONLY);
  tissue_dist_ = h5read_icube(file, "/phantom/tissue_dist");

  dim_[_cy] = tissue_dist_.dimension(0);
  dim_[_cx] = tissue_dist_.dimension(1);
  dim_[_cz] = tissue_dist_.dimension(2);

  mat res = h5read_mat(file, "/phantom/resolution");
  res_.setZero();
  res_[_cx] = res(0);
  res_[_cy] = res(1);
  if(res.size()==3) // for 3d case.
	  res_[_cz] = res(2);
	else
	  res_[_cz] = 1; // 2d case.

  offset_.setZero();

  dB0_ = cube(dim_[_cy], dim_[_cx], dim_[_cz]);
  dB0_.setZero();

  pd_ = cube(dim_[_cy], dim_[_cx], dim_[_cz]);
  pd_.setConstant(1);

  T1_ = cube(dim_[_cy], dim_[_cx], dim_[_cz]); // rows->y cols->x
  T1_.setZero();

  T2_ = cube(dim_[_cy], dim_[_cx], dim_[_cz]);
  T2_.setZero();

  T2s_ = cube(dim_[_cy], dim_[_cx], dim_[_cz]);
  T2s_.setZero();

      // load T1/T2 parameters.
      std::string path = utility::g_install_dir + "/share/spin-scenario/config/mida_1.5t_relaxation.dat";
      if (!supp_filename.empty()) path = supp_filename;
      // col 1: index No.
	  // col 2: T1 (ms)
      // col 3: T2 (ms)
	  mat par = eigen_read(path);
	  if (par.cols()!=3)
		  throw std::runtime_error("Unrecognized phantom T1/T2 parameter file!");

	  int max_idx = (int)par.col(0).maxCoeff()+ 1;
      tissue_index_ = ivec::Zero(max_idx); // indexing based 1.
      tissue_t1t2_ = mat::Zero(max_idx, 2); // indexing based 1.

      for (int i = 0; i < par.rows(); i++) {
        int idx = par(i, 0);
        tissue_index_(idx) = 1;  // effective tissue.
        tissue_t1t2_(idx, 0) = par(i, 1);
        tissue_t1t2_(idx, 1) = par(i, 2);
      }
      tissue_t1t2_ *= 1e-3;  // ms into unit s.
      omp_set_num_threads(omp_core_num);
      #pragma omp parallel for
      for (int nz = 0; nz < dim_[_cz]; nz++) {
        for (int nx = 0; nx < dim_[_cx]; nx++)
          for (int ny = 0; ny < dim_[_cy]; ny++) {
            {
              int idx = tissue_dist_(ny, nx, nz);
              // do not deal with background and those tissues not assigned.
              if (idx != 0 && tissue_index_(idx) == 1) {
                T1_(ny, nx, nz) = tissue_t1t2_(idx, 0);
                T2_(ny, nx, nz) = tissue_t1t2_(idx, 1);
              }

            }
          }
      }

#ifdef SSL_OUTPUT_ENABLE
      std::string name;
      //name = h5read_string(file, "/phantom/name");
      std::string s = name + " phantom loaded, cube dimension x/y/z(" +
                      std::to_string(dim_[cx]) + " " +std::to_string(dim_[cy]) + " "
                      +std::to_string(dim_[cz])+ ")\n";
      ssl_color_text("info", s);
#endif
    }
}
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <gtest/gtest.h>
#include <test/Helpers.h>
#include <vw/Plate/TileManipulation.h>
#include <boost/foreach.hpp>

using namespace std;
using namespace vw;
using namespace vw::platefile;
using namespace vw::test;

class BlobManagerTest : public ::testing::Test {
  protected:

#define bbox_does_tile_area(bbox, width, height, total_tiles) do {\
  SCOPED_TRACE("");\
  bbox_does_tile_area_(bbox, width, height, total_tiles);\
} while(0)

  void bbox_does_tile_area_(const BBox2i& orig_bbox, unsigned width, unsigned height, unsigned total_tiles) {
    list<BBox2i> tiles = bbox_tiles(orig_bbox, width, height);
    EXPECT_EQ(total_tiles, tiles.size());

    BBox2i total_bbox;

    // It should completely tile the space. No tile should overlap.
    BOOST_FOREACH( const BBox2i& bbox, tiles ) {
      SCOPED_TRACE(Message() << "Outer box[" << bbox << "]");

      // Tiles are allowed to be smaller than the requested size. Just not larger.
      EXPECT_LE(bbox.width(),  width);
      EXPECT_LE(bbox.height(), height);

      // One of the boxes should be the same.
      bool intersected = false;

      // Make sure it doesn't intersect any other box. (Since we're comparing a
      // list to itself, that means there should be just one intersection)
      BOOST_FOREACH( const BBox2i& new_box, tiles ) {
        SCOPED_TRACE(Message() << "Inner box[" << new_box << "]");
        if (bbox.intersects(new_box)) {
          EXPECT_FALSE(intersected);
          intersected = true;
        }
      }

      total_bbox.grow(bbox);
    }

    // Make sure that the union of all tiles is the original bbox
    EXPECT_EQ(orig_bbox, total_bbox);
  }
};

TEST_F(BlobManagerTest, Tiles) {
  bbox_does_tile_area(BBox2i(0,0,8,12), 4, 4, 6);
}

TEST_F(BlobManagerTest, Weird) {
  bbox_does_tile_area(BBox2i(7,17,31,13), 7, 5, 15);
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_ROTMG_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_ROTMG_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
inline void rotmg( float& d1, float& d2, float& x1, const float y1,
        float* sparam ) {
    cblas_srotmg( &d1, &d2, &x1, y1, sparam );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
inline void rotmg( double& d1, double& d2, double& x1, const double y1,
        double* dparam ) {
    cblas_drotmg( &d1, &d2, &x1, y1, dparam );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
inline void rotmg( float& d1, float& d2, float& x1, const float y1,
        float* sparam ) {
    cublasSrotmg( &d1, &d2, &x1, &y1, sparam );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
inline void rotmg( double& d1, double& d2, double& x1, const double y1,
        double* dparam ) {
    cublasDrotmg( &d1, &d2, &x1, &y1, dparam );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
inline void rotmg( float& d1, float& d2, float& x1, const float y1,
        float* sparam ) {
    BLAS_SROTMG( &d1, &d2, &x1, &y1, sparam );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
inline void rotmg( double& d1, double& d2, double& x1, const double y1,
        double* dparam ) {
    BLAS_DROTMG( &d1, &d2, &x1, &y1, dparam );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to rotmg.
//
template< typename Value >
struct rotmg_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef void result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorDPARAM >
    static result_type invoke( real_type& d1, real_type& d2, real_type& x1,
            const real_type y1, VectorDPARAM& dparam ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorDPARAM >::value) );
        detail::rotmg( d1, d2, x1, y1, bindings::begin_value(dparam) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the rotmg_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for rotmg. Its overload differs for
//
template< typename VectorDPARAM >
inline typename rotmg_impl< typename bindings::value_type<
        VectorDPARAM >::type >::result_type
rotmg( typename remove_imaginary< typename bindings::value_type<
        VectorDPARAM >::type >::type& d1, typename remove_imaginary<
        typename bindings::value_type< VectorDPARAM >::type >::type& d2,
        typename remove_imaginary< typename bindings::value_type<
        VectorDPARAM >::type >::type& x1, const typename remove_imaginary<
        typename bindings::value_type< VectorDPARAM >::type >::type y1,
        VectorDPARAM& dparam ) {
    rotmg_impl< typename bindings::value_type<
            VectorDPARAM >::type >::invoke( d1, d2, x1, y1, dparam );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <boost/units/static_rational.hpp>


/***********************************
[DEPRECATED] this file is deprecated now.

at the very begining, I want to convert the handler path (that provided by this
framework's users in the handler registration function) into regular expression
format of Named Captures, such as bellow:

handler path                 named capture regular expression

/abc/{int:id}/bala/  ---- > ^/abc/(?P<id>\\d+)/bala/?$
/abc/{str:name}/bala/  ---- > ^/abc/(?P<name>\\w+)/bala/?$

then store the named capture regular expression into the the global map.

when client request is comming, use boost::xpressive::regex_search() to match the request url
and the regex expression and capture the named group.

The method stated above relies on the boost's regular expression library, which make things
complicated, and the performance of the method does not reach the limit. As a geek, I
implemented a amazing function to do all the work. Want to know more about the amazing function?
See https://github.com/Andrewpqc/vogro/blob/master/test/url_match.cpp

So, the content of this file is deprecated now.
**********************************/

#include <iostream>
#include <boost/xpressive/xpressive.hpp>

//old version of transformUserReqextoBstRegex

std::string transformUserRegexToBstRegex(std::string userRegex) {
    using namespace std;
    using namespace boost::xpressive;

    sregex pathParam = sregex::compile("\\{(int|str)?:?(\\w+)\\}");
    smatch what;
    while (regex_search(userRegex, what, pathParam)) {

        string pattern;

        if (what[1] == "int" || what[1] == "") {
            pattern = "\\d+";
        }
        else if (what[1] == "str") {
            pattern = "\\w+";
        }
        else {
            cout << "error" << endl;
        }

        stringstream s;
        string format;
        s << "(?P<" << what[2] << ">" << pattern << ")";
        s >> format;
        s.clear();

        userRegex = regex_replace(userRegex, pathParam, format, regex_constants::format_first_only);
    }

    if (userRegex.back() != '/') {
        userRegex += "/?$";
    }
    else {
        userRegex += "?$";
    }

    userRegex = '^' + userRegex;
    return userRegex;
}

void testUrlMatch(std::string regString, std::string path, std::string name) {
    boost::xpressive::sregex e = boost::xpressive::sregex::compile(regString);
    boost::xpressive::smatch what;
    if (boost::xpressive::regex_search(path, what, e)) {
        try {
            std::cout << what[name] << std::endl;
        }
        catch (boost::exception_detail::clone_impl<boost::xpressive::regex_error> e) {
            std::cout << "参数" << name << "不存在" << std::endl;
        }
    }
    else {
        std::cout << "not match" << std::endl;
    }
}

int main(void) {
    std::string testUserRegex1 = "/abc/{id}";
    std::cout << transformUserRegexToBstRegex(testUserRegex1) << std::endl;
    std::string testUserRegex2 = "/abc/{int:id}";
    std::cout << transformUserRegexToBstRegex(testUserRegex2) << std::endl;
    std::string testUserRegex3 = "/abc/{id}/{str:name}/";
    std::cout << transformUserRegexToBstRegex(testUserRegex3) << std::endl;
    std::string testUserRegex4 = "/abc/{int:id}/{str:name}/hhh/";
    std::string regString = transformUserRegexToBstRegex(testUserRegex4);

    boost::xpressive::sregex e = boost::xpressive::sregex::compile(regString);
    testUrlMatch(regString, "/abc/2/andrewpqc/hhh", "id");
    testUrlMatch(regString, "/abc/2/andrewpqc/hhh", "name");
    testUrlMatch(regString, "/abc/2/andrewpqc/hhh/", "name");
    testUrlMatch(regString, "/ab/2/andrewpqc/hhh/", "name");

    return 0;
}


#include <boost/hana/scan_left.hpp>


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// @project        Open Space Toolkit ▸ Physics
/// @file           OpenSpaceToolkit/Physics/Units/Unit.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <OpenSpaceToolkit/Physics/Units/Unit.hpp>

#include <OpenSpaceToolkit/Core/Error.hpp>
#include <OpenSpaceToolkit/Core/Utilities.hpp>

#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

namespace ostk
{
namespace physics
{
namespace units
{

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                Unit::Unit                                  (   const   Unit::Type&                 aType,
                                                                                const   Real&                       aValue                                      )
                                :   type_(aType),
                                    value_(aValue)
{

}

                                Unit::~Unit                                 ( )
{

}

bool                            Unit::operator ==                           (   const   Unit&                       aUnit                                       ) const
{

    if ((!this->isDefined()) || (!aUnit.isDefined()))
    {
        return false ;
    }

    return (type_ == aUnit.type_) && (value_ == aUnit.value_) ;

}

bool                            Unit::operator !=                           (   const   Unit&                       aUnit                                       ) const
{

    if ((!this->isDefined()) || (!aUnit.isDefined()))
    {
        return true ;
    }

    return (type_ != aUnit.type_) || (value_ != aUnit.value_) ;

}

bool                            Unit::isDefined                             ( ) const
{
    return (type_ != Unit::Type::Undefined) && value_.isDefined() ;
}

bool                            Unit::isZero                                ( ) const
{
    return (type_ != Unit::Type::Undefined) && value_.isZero() ;
}

const Real&                     Unit::accessValue                           ( ) const
{
    return value_ ;
}

Unit::Type                      Unit::getType                               ( ) const
{
    return type_ ;
}

Real                            Unit::getValue                              ( ) const
{
    return value_ ;
}

Real&                           Unit::accessValue                           ( )
{
    return value_ ;
}

void                            Unit::setValue                              (   const   Real&                       aValue                                      )
{
    value_ = aValue ;
}

Pair<Real, String>              Unit::ParseString                           (   const   String&                     aString                                     )
{

    if (aString.isEmpty())
    {
        throw ostk::core::error::runtime::Undefined("String") ;
    }

    boost::smatch match ;

    if (boost::regex_match(aString, match, boost::regex("^([-+\\de.]+) \\[([\\w\\/^\\.]+)\\]$")))
    {

        const Real value = boost::lexical_cast<double>(match[1]) ;
        const String unit = String(match[2]) ;

        return { value, unit } ;

    }
    else
    {
        throw ostk::core::error::RuntimeError("Cannot parse unit string [{}].", aString) ;
    }

}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

}
}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file scenario/scenario.hpp
    \brief Scenario class
    \ingroup scenario
*/

#pragma once

#include <map>
#include <vector>

#include <boost/shared_ptr.hpp>
#include <ored/utilities/serializationdate.hpp>
#include <ql/time/date.hpp>
#include <ql/types.hpp>

namespace ore {
namespace analytics {
using QuantLib::Real;
using QuantLib::Size;
using QuantLib::Date;
using std::string;

//! Data types stored in the scenario class
/*! \ingroup scenario
 */
class RiskFactorKey {
public:
    //! Risk Factor types
    enum class KeyType {
        None,
        DiscountCurve,
        YieldCurve,
        IndexCurve,
        SwaptionVolatility,
        YieldVolatility,
        OptionletVolatility,
        FXSpot,
        FXVolatility,
        EquitySpot,
        EquityVolatility,
        DividendYield,
        SurvivalProbability,
        RecoveryRate,
        CDSVolatility,
        BaseCorrelation,
        CPIIndex,
        ZeroInflationCurve,
        ZeroInflationCapFloorVolatility,
        YoYInflationCurve,
        YoYInflationCapFloorVolatility,
        CommoditySpot,
        CommodityCurve,
        CommodityVolatility,
        SecuritySpread,
        Correlation,
        CPR
    };

    //! Constructor
    RiskFactorKey() : keytype(KeyType::None), name(""), index(0) {}
    //! Constructor
    RiskFactorKey(const KeyType& iKeytype, const string& iName, const Size& iIndex = 0)
        : keytype(iKeytype), name(iName), index(iIndex) {}

    //! Key type
    KeyType keytype;
    //! Key name.
    /*! For FX this is a pair ("EURUSD") for Discount or swaption it's just a currency ("EUR")
     *  and for an index it's the index name
     */
    std::string name;
    //! Index
    Size index;

private:
    friend class boost::serialization::access;
    template <class Archive> void serialize(Archive& ar, const unsigned int) {
        ar& keytype;
        ar& name;
        ar& index;
    }
};

inline bool operator<(const RiskFactorKey& lhs, const RiskFactorKey& rhs) {
    return std::tie(lhs.keytype, lhs.name, lhs.index) < std::tie(rhs.keytype, rhs.name, rhs.index);
}

inline bool operator==(const RiskFactorKey& lhs, const RiskFactorKey& rhs) {
    return lhs.keytype == rhs.keytype && lhs.name == rhs.name && lhs.index == rhs.index;
}

inline bool operator>(const RiskFactorKey& lhs, const RiskFactorKey& rhs) { return rhs < lhs; }
inline bool operator<=(const RiskFactorKey& lhs, const RiskFactorKey& rhs) { return !(lhs > rhs); }
inline bool operator>=(const RiskFactorKey& lhs, const RiskFactorKey& rhs) { return !(lhs < rhs); }
inline bool operator!=(const RiskFactorKey& lhs, const RiskFactorKey& rhs) { return !(lhs == rhs); }

std::ostream& operator<<(std::ostream& out, const RiskFactorKey::KeyType& type);
std::ostream& operator<<(std::ostream& out, const RiskFactorKey& key);

RiskFactorKey::KeyType parseRiskFactorKeyType(const string& str);
RiskFactorKey parseRiskFactorKey(const string& str);

//-----------------------------------------------------------------------------------------------
//! Scenario Base Class
/*! A scenario contains a single cross asset model sample in terms of
  yield curves by currency, FX rates, etc.

  This base class provides the interface to add and retrieve data to and from a scenario.
  Concrete simple and memory optimized "compact" scenario classes are derived from this.

  \ingroup scenario
*/
class Scenario {
public:
    //! Destructor
    virtual ~Scenario() {}

    //! Return the scenario asof date
    virtual const Date& asof() const = 0;

    //! Get the scenario label
    virtual const string& label() const = 0;
    //! Set the scenario label
    virtual void label(const string&) = 0;

    //! Get Numeraire ratio n = N(t) / N(0) so that Price(0) = N(0) * E [Price(t) / N(t) ]
    virtual Real getNumeraire() const = 0;
    //! Set the Numeraire ratio n = N(t) / N(0) so that Price(0) = N(0) * E [Price(t) / N(t) ]
    virtual void setNumeraire(Real n) = 0;

    //! Check whether this scenario provides the data for the given key
    virtual bool has(const RiskFactorKey& key) const = 0;
    //! Risk factor keys for which this scenario provides data
    virtual const std::vector<RiskFactorKey>& keys() const = 0;
    //! Add an element to the scenario
    virtual void add(const RiskFactorKey& key, Real value) = 0;
    //! Get an element from the scenario
    virtual Real get(const RiskFactorKey& key) const = 0;

    //! clones a scenario and returns a pointer to the new object
    virtual boost::shared_ptr<Scenario> clone() const = 0;

private:
    friend class boost::serialization::access;
    template <class Archive> void serialize(Archive&, const unsigned int) {}
};
} // namespace analytics
} // namespace ore


#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>
#include <Eigen/Core>

#include <fstream>
#include <iostream>
#include <numeric>

typedef pcl::PointXYZI PointT;

float
getAveragePointsInNeighborhood(pcl::PointCloud<PointT>::Ptr cloud, float searchRadius )
{
    // create a kdtree object
    pcl::KdTreeFLANN<PointT> kdtreeObj;
    // set the input cloud for the kdtree. this should start creating the tree.
    kdtreeObj.setInputCloud (cloud);

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    // determine size of PC before entering for, as the size changes by one during the loop.
    size_t nIters = cloud->points.size();

    std::vector<float> nPtsVec;
    int nPts; // number of points found within searchRadius. This will also be returned by function.

    #pragma omp parallel for // use parallel processing to speed up the process slightly. More cores would be better i guess...
    for (size_t p = 0; p < nIters; p++){

        //clear the vectors before each search.
        pointIdxRadiusSearch.clear();
        pointRadiusSquaredDistance.clear();

        const PointT searchPoint = cloud->points[p];

        // if no neighbours were found add point to result point cloud;.
        nPts = kdtreeObj.radiusSearch (searchPoint, searchRadius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
        if ( nPts > 1 )
        {
/*
                    if (p % 10000 == 0)
                        std::cout << "Performed " << p+1 << " kdtree searches out of "
                              << nIters << ". " << std::setprecision(4) << "Found " << nPts
                              << " points within searchRadius of " << searchRadius << ". "
                              << float((p+1.))/float(nIters)*100.0 << " % completed." << std::endl;
*/
            nPtsVec.push_back((float)nPts);

        }
        else
        {
            nPts = -1;
        }

    }

    return std::accumulate(nPtsVec.begin(), nPtsVec.end(), 0.0f) / (float) nPtsVec.size();
}

int
applyHull (pcl::PointCloud<PointT>::Ptr cloud_filtered, pcl::PointCloud<PointT>::Ptr hull, pcl::PointCloud<PointT>::Ptr convexhull )
{

    pcl::console::TicToc tt;
    tt.tic();

      pcl::PointCloud<PointT>::Ptr cloud_projected (new pcl::PointCloud<PointT>);
      // read in parameter file
      std::vector<float> params;
      readParamsFromFile("../cfg/hull2d.cfg", params, true);

      // Create a Concave Hull representation of the projected inliers
      pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>);
      pcl::ConcaveHull<PointT> chull;
      chull.setInputCloud (cloud_filtered);
      chull.setAlpha (params[0]);

      int nPts = (int)getAveragePointsInNeighborhood(cloud_hull, 0.25 ); // param2 = searchDistance in meters
      std::cout << "number of average points within searchRadius found: " <<  nPts << "  alpha= " << chull.getAlpha() << std::endl;

      if (nPts <= 8)
          std::cout << "average number of neighboring points < 8. Continuing with processing... " << std::endl;
      else
      {
        std::cout << "average number of neighboring points > 8, increasing Alpha by 0.15 => Alpha: " << chull.getAlpha() + 0.15 << " and reapplying hullDetection... " << std::endl;
        chull.setAlpha (chull.getAlpha() + 0.1);
        chull.reconstruct (*cloud_hull);
      }

      pcl::copyPointCloud(*cloud_hull, *hull);

      std::cerr << "Concave hull has: " << cloud_hull->points.size ()
                << " data points." << std::endl;

      pcl::PCDWriter writer;
      writer.write ("ConcavHull.pcd", *cloud_hull, false);

      pcl::ConvexHull<PointT> convhull;
      convhull.setInputCloud (cloud_projected);
      convhull.setComputeAreaVolume( false );
      convhull.reconstruct (*cloud_hull);
      pcl::copyPointCloud(*cloud_hull, *convexhull);

      std::cerr << "Convex hull has: " << cloud_hull->points.size ()
                << " data points." << " Processing took: " <<
                   tt.toc()/1000.0 << " seconds." << std::endl;

      //writer.write ("Convexhull.pcd", *cloud_hull, false);

      return (0);

}

int
applyHull (pcl::PointCloud<PointT>::Ptr cloud_filtered, pcl::PointCloud<PointT>::Ptr hull)
{

    pcl::console::TicToc tt;
    tt.tic();

//  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cloud_projected (new pcl::PointCloud<PointT>);

  // read in parameter file
  std::vector<float> params;
  readParamsFromFile("../cfg/hull2d.cfg", params, true);

  // Create a Concave Hull representation of the projected inliers
  pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>);
  pcl::ConcaveHull<PointT> chull;
  chull.setInputCloud (cloud_filtered);
  chull.setAlpha (params[0]);

  chull.reconstruct (*cloud_hull);

  int nPts = (int)getAveragePointsInNeighborhood(cloud_hull, 0.25 ); // param2 = searchDistance in meters
  std::cout << "number of average points within searchRadius found: " <<  nPts << "  alpha= " << chull.getAlpha() << std::endl;

  if (nPts <= 8)
      std::cout << "average number of neighboring points < 8. Continuing with processing... " << std::endl;
  else
  {
    std::cerr << "average number of neighboring points > 8, increasing Alpha by 0.15 => Alpha: " << chull.getAlpha() + 0.15 << " and reapplying hullDetection... " << std::endl;
    chull.setAlpha (chull.getAlpha() + 0.1);
    chull.reconstruct (*cloud_hull);
  }

  //viewer<PointT>(cloud_hull);
  pcl::copyPointCloud(*cloud_hull, *hull);

  std::cerr << "Concave hull has: " << cloud_hull->points.size ()
            << " data points." << " Processing took: " <<
               tt.toc()/1000.0 << " seconds." << std::endl;

  pcl::PCDWriter writer;
  if (cloud_hull->size() > 0)
    writer.write ("ConcavHull.pcd", *cloud_hull, false);

  return (0);
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file PointCloudPclPointCloudConvertersTest.cpp
 * @date 01/12/2017
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup CommonTests
 *
 * Testing conversion from PointCloud to PclPointCloud and viceversa.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <Converters/PointCloudToPclPointCloudConverter.hpp>
#include <Converters/PclPointCloudToPointCloudConverter.hpp>
#include <Types/CPP/PointCloud.hpp>
#include <Errors/Assert.hpp>
#include <boost/smart_ptr.hpp>

using namespace Converters;
using namespace PointCloudWrapper;

TEST_CASE( "PclPointCloud to PointCloud and Back", "[PclPointCloudToPointCloud]" )
	{
	PclPointCloudToPointCloudConverter firstConverter;
	PointCloudToPclPointCloudConverter secondConverter;

	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		inputCloud->points.push_back( pcl::PointXYZ(pointIndex, (float)pointIndex/3, std::sqrt(pointIndex)) );
		}

	PointCloudSharedConstPtr asnPointCloud = firstConverter.ConvertShared(inputCloud);
	REQUIRE(GetNumberOfPoints(*asnPointCloud) == inputCloud->points.size() );
	for(int pointIndex = 0; pointIndex < static_cast<int>( inputCloud->points.size() ); pointIndex++)
		{
		REQUIRE( GetXCoordinate(*asnPointCloud, pointIndex) == inputCloud->points.at(pointIndex).x );
		REQUIRE( GetYCoordinate(*asnPointCloud, pointIndex) == inputCloud->points.at(pointIndex).y );
		REQUIRE( GetZCoordinate(*asnPointCloud, pointIndex) == inputCloud->points.at(pointIndex).z );
		}

	pcl::PointCloud<pcl::PointXYZ>::ConstPtr outputCloud = secondConverter.ConvertShared(asnPointCloud);
	REQUIRE(outputCloud->points.size() == inputCloud->points.size() );
	for(unsigned pointIndex = 0; pointIndex < outputCloud->points.size(); pointIndex++)
		{
		pcl::PointXYZ inputPoint = inputCloud->points.at(pointIndex);
		pcl::PointXYZ outputPoint = outputCloud->points.at(pointIndex);
		REQUIRE(inputPoint.x == outputPoint.x);
		REQUIRE(inputPoint.y == outputPoint.y);
		REQUIRE(inputPoint.z == outputPoint.z);
		}

	asnPointCloud.reset();
	}

TEST_CASE( "PointCloud3D to PclPointCloud and Back (Pcl)", "[PointCloud3DToPclPointCloud]" )
	{
	PclPointCloudToPointCloudConverter firstConverter;
	PointCloudToPclPointCloudConverter secondConverter;

	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		inputCloud->points.push_back( pcl::PointXYZ(pointIndex, (float)pointIndex/3, std::sqrt(pointIndex)) );
		}

	PointCloudSharedConstPtr asnPointCloud = firstConverter.ConvertShared(inputCloud);
	pcl::PointCloud<pcl::PointXYZ>::ConstPtr intermediateCloud = secondConverter.ConvertShared(asnPointCloud);
	PointCloudSharedConstPtr outputCloud = firstConverter.ConvertShared(intermediateCloud);

	REQUIRE(GetNumberOfPoints(*outputCloud) == GetNumberOfPoints(*asnPointCloud));
	for(int pointIndex = 0; pointIndex < GetNumberOfPoints(*outputCloud); pointIndex++)
		{
		REQUIRE( GetXCoordinate(*outputCloud, pointIndex) == GetXCoordinate(*asnPointCloud, pointIndex) );
		REQUIRE( GetYCoordinate(*outputCloud, pointIndex) == GetYCoordinate(*asnPointCloud, pointIndex) );
		REQUIRE( GetZCoordinate(*outputCloud, pointIndex) == GetZCoordinate(*asnPointCloud, pointIndex) );
		}

	asnPointCloud.reset();
	outputCloud.reset();
	}

TEST_CASE("Empty Point Cloud conversion (Pcl)", "[EmptyPointCloud]")
	{
	PclPointCloudToPointCloudConverter firstConverter;
	PointCloudToPclPointCloudConverter secondConverter;

	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
	PointCloudSharedConstPtr asnPointCloud = firstConverter.ConvertShared(inputCloud);
	pcl::PointCloud<pcl::PointXYZ>::ConstPtr intermediateCloud = secondConverter.ConvertShared(asnPointCloud);
	PointCloudSharedConstPtr outputCloud = firstConverter.ConvertShared(intermediateCloud);

	REQUIRE(GetNumberOfPoints(*asnPointCloud) == 0);
	REQUIRE(intermediateCloud->points.size() == 0);
	REQUIRE(GetNumberOfPoints(*outputCloud) == 0);
	}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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 NETKET_METROPOLISLOCAL_HPP
#define NETKET_METROPOLISLOCAL_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace netket {

// Metropolis sampling generating local moves in hilbert space
template <class WfType>
class MetropolisLocal : public AbstractSampler<WfType> {
  WfType& psi_;

  const AbstractHilbert& hilbert_;

  // number of visible units
  const int nv_;

  netket::default_random_engine rgen_;

  // states of visible units
  Eigen::VectorXd v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // Look-up tables
  typename WfType::LookupType lt_;

  int nstates_;
  std::vector<double> localstates_;

 public:
  explicit MetropolisLocal(WfType& psi)
      : psi_(psi), hilbert_(psi.GetHilbert()), nv_(hilbert_.Size()) {
    Init();
  }

  void Init() {
    v_.resize(nv_);

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (!hilbert_.IsDiscrete()) {
      throw InvalidInputError(
          "Local Metropolis sampler works only for discrete "
          "Hilbert spaces");
    }

    accept_.resize(1);
    moves_.resize(1);

    nstates_ = hilbert_.LocalSize();
    localstates_ = hilbert_.LocalStates();

    Seed();

    Reset(true);

    InfoMessage() << "Local Metropolis sampler is ready " << std::endl;
  }

  void Seed(int baseseed = 0) {
    std::random_device rd;
    std::vector<int> seeds(totalnodes_);

    if (mynode_ == 0) {
      for (int i = 0; i < totalnodes_; i++) {
        seeds[i] = rd() + baseseed;
      }
    }

    SendToAll(seeds);

    rgen_.seed(seeds[mynode_]);
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      hilbert_.RandomVals(v_, rgen_);
    }

    psi_.InitLookup(v_, lt_);

    accept_ = Eigen::VectorXd::Zero(1);
    moves_ = Eigen::VectorXd::Zero(1);
  }

  void Sweep() override {
    std::vector<int> tochange(1);
    std::vector<double> newconf(1);

    std::uniform_real_distribution<double> distu;
    std::uniform_int_distribution<int> distrs(0, nv_ - 1);
    std::uniform_int_distribution<int> diststate(0, nstates_ - 1);

    for (int i = 0; i < nv_; i++) {
      // picking a random site to be changed
      int si = distrs(rgen_);
      assert(si < nv_);
      tochange[0] = si;

      // picking a random state
      int newstate = diststate(rgen_);
      newconf[0] = localstates_[newstate];

      // make sure that the new state is not equal to the current one
      while (std::abs(newconf[0] - v_(si)) <
             std::numeric_limits<double>::epsilon()) {
        newstate = diststate(rgen_);
        newconf[0] = localstates_[newstate];
      }

      const auto lvd = psi_.LogValDiff(v_, tochange, newconf, lt_);
      double ratio = std::norm(std::exp(lvd));

#ifndef NDEBUG
      const auto psival1 = psi_.LogVal(v_);
      if (std::abs(std::exp(psi_.LogVal(v_) - psi_.LogVal(v_, lt_)) - 1.) >
          1.0e-8) {
        std::cerr << psi_.LogVal(v_) << "  and LogVal with Lt is "
                  << psi_.LogVal(v_, lt_) << std::endl;
        std::abort();
      }
#endif

      // Metropolis acceptance test
      if (ratio > distu(rgen_)) {
        accept_[0] += 1;
        psi_.UpdateLookup(v_, tochange, newconf, lt_);
        hilbert_.UpdateConf(v_, tochange, newconf);

#ifndef NDEBUG
        const auto psival2 = psi_.LogVal(v_);
        if (std::abs(std::exp(psival2 - psival1 - lvd) - 1.) > 1.0e-8) {
          std::cerr << psival2 - psival1 << " and logvaldiff is " << lvd
                    << std::endl;
          std::cerr << psival2 << " and LogVal with Lt is "
                    << psi_.LogVal(v_, lt_) << std::endl;
          std::abort();
        }
#endif
      }
      moves_[0] += 1;
    }
  }

  Eigen::VectorXd Visible() override { return v_; }

  void SetVisible(const Eigen::VectorXd& v) override { v_ = v; }

  WfType& GetMachine() noexcept override { return psi_; }

  const AbstractHilbert& GetHilbert() const noexcept override {
    return hilbert_;
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < 1; i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace netket

#endif