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

  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 <math.h>
#include <float.h>
#include <iomanip>

#include <niftkConversionUtils.h>
#include <niftkCommandLineParser.h>
#include <itkCommandLineHelper.h>

#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>
#include <itkBasicImageFeaturesImageFilter.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkNifTKImageIOFactory.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkImage.h>
#include <itkUnaryFunctorImageFilter.h>
#include <itkScalarToRGBBIFPixelFunctor.h>
#include <itkScalarToRGBOBIFPixelFunctor.h>
#include <itkMaskImageFilter.h>
#include <itkResampleImageFilter.h>
#include <itkLinearInterpolateImageFunction.h>
#include <itkIdentityTransform.h>

#include <boost/filesystem/path.hpp>

struct niftk::CommandLineArgumentDescription clArgList[] = {
  {OPT_SWITCH, "dbg", 0, "Output debugging information."},
  {OPT_SWITCH, "v", 0,   "Verbose output during execution."},

  {OPT_SWITCH, "st", NULL, "Perform single threaded execution [multi-threaded]."},
  {OPT_SWITCH, "resample", NULL, "Speed up the execution by resampling the image."},

  {OPT_SWITCH, "orientate", NULL, "Calculate orientated BIFs [no]."},
  {OPT_SWITCH, "n72", NULL, "Calculate orientations in one degree increments [45degs]."},

  {OPT_SWITCH, "vflip", NULL, "Flip the orientation vertically (e.g. for PA vs AP views)."},
  {OPT_SWITCH, "hflip", NULL, "Flip the orientation horizontally (e.g. for ML vs LM views)."},

  {OPT_SWITCH, "noSlope", NULL, "Ignore slopes, i.e. only classify as 2nd order."},

  {OPT_DOUBLEx2, "origin", "ox,oy", "Orientate relative to this origin in mm (0,0 = corner of the image)."},

  {OPT_FLOAT, "sigma", "value", "The Guassian std. dev. in mm at which to compute the BIFs [1.0]."},
  {OPT_INT,   "nscales", "n",   "The number of scales to process [1]."},
  {OPT_FLOAT, "fscales", "value", "The multiplicative factor between scales [2.0]."},

  {OPT_FLOAT, "e", "epsilon", "The noise suppression parameter [1e-05]."},

  {OPT_STRING, "u2D", "filename", "Local reference orientation in 'x'."},
  {OPT_STRING, "v2D", "filename", "Local reference orientation in 'y'."},

  {OPT_STRING, "mask", "filename", "Only compute BIFs where the mask image is non-zero."},

  {OPT_FLOAT, "t",    "threshold", "Only compute BIFs where the input image is greater than <threshold>."},
  {OPT_STRING, "om", "filename", "Write the computed mask image to a file."},

  {OPT_STRING, "oS00", "filename", "Save the zero order smoothed image to a file."},
  {OPT_STRING, "oS10", "filename", "Save the first derivative in 'x' to a file."},
  {OPT_STRING, "oS01", "filename", "Save the first derivative in 'y' to a file."},
  {OPT_STRING, "oS11", "filename", "Save the second derivative in 'xy' to a file."},
  {OPT_STRING, "oS20", "filename", "Save the second derivative in 'xx' to a file."},
  {OPT_STRING, "oS02", "filename", "Save the second derivative in 'yy' to a file."},

  {OPT_STRING, "oFlat", "filename", "Save the flatness response to a file."},
  {OPT_STRING, "oSlope", "filename", "Save the slope-like response to a file."},
  {OPT_STRING, "oDarkBlob", "filename", "Save the dark blob response to a file."},
  {OPT_STRING, "oLightBlob", "filename", "Save the light blob response to a file."},
  {OPT_STRING, "oDarkLine", "filename", "Save the dark line response to a file."},
  {OPT_STRING, "oLightLine", "filename", "Save the light line response to a file."},
  {OPT_STRING, "oSaddle", "filename", "Save the saddlelike response to a file."},

  {OPT_STRING, "oOrient", "filename", "Save the continuous orientation image to a file."},
  {OPT_STRING, "oVar", "filename", "Save the BIF response variance image to a file."},

  {OPT_STRING, "oh",   "filename", "Write the histogram of BIFs to a file.."},
  {OPT_STRING, "opng", "filename", "Write the label image as a colour PNG file for display purposes."},
  {OPT_STRING, "o",    "filename", "The output label image."},

  {OPT_STRING|OPT_LONELY|OPT_REQ, NULL, "filename", "The input image."},

  {OPT_DONE, NULL, NULL,
   "Program to compute basic image features for a 2D image.\n"
  }
};

enum {
  O_DEBUG = 0,
  O_VERBOSE,

  O_SINGLE_THREADED,
  O_RESAMPLE_IMAGES,

  O_ORIENTATE,
  O_72_ORIENTATIONS,

  O_FLIP_VERTICALLY,
  O_FLIP_HORIZONTALLY,

  O_SECOND_ORDER_ONLY,

  O_ORIGIN,

  O_SIGMA_IN_MM,
  O_NUMBER_OF_SCALES,
  O_SCALE_FACTOR,

  O_EPSILON,

  O_ORIENTATION_INX,
  O_ORIENTATION_INY,

  O_MASK,

  O_THRESHOLD,
  O_OUTPUT_MASK,

  O_OUTPUT_S00,
  O_OUTPUT_S10,
  O_OUTPUT_S01,
  O_OUTPUT_S11,
  O_OUTPUT_S20,
  O_OUTPUT_S02,

  O_OUTPUT_FLAT,
  O_OUTPUT_SLOPE,
  O_OUTPUT_DARK_BLOB,
  O_OUTPUT_LIGHT_BLOB,
  O_OUTPUT_DARK_LINE,
  O_OUTPUT_LIGHT_LINE,
  O_OUTPUT_SADDLE,

  O_OUTPUT_ORIENTATION,
  O_OUTPUT_VARIANCE,

  O_OUTPUT_HISTOGRAM,
  O_OUTPUT_COLOUR_IMAGE,
  O_OUTPUT_IMAGE,

  O_INPUT_IMAGE
};

std::string AddScaleSuffix( std::string filename, float scale, int nScales )
{
  if ( nScales > 1 ) {

    char strScale[128];

    boost::filesystem::path pathname( filename );
    boost::filesystem::path ofilename;

    std::string extension = pathname.extension().string();
    std::string stem = pathname.stem().string();

    if ( extension == std::string( ".gz" ) ) {

      extension = pathname.stem().extension().string() + extension;
      stem = pathname.stem().stem().string();
    }

    sprintf(strScale, "_%03gmm", scale);

    ofilename = pathname.parent_path() /
      boost::filesystem::path( stem + std::string( strScale ) + extension );

    return ofilename.string();
  }
  else
    return filename;
}

std::string AddSuffix( std::string filename, std::string suffix )
{
  boost::filesystem::path pathname( filename );
  boost::filesystem::path ofilename;

  std::string extension = pathname.extension().string();
  std::string stem = pathname.stem().string();

  if ( extension == std::string( ".gz" ) ) {

    extension = pathname.stem().extension().string() + extension;
    stem = pathname.stem().stem().string();
  }

  ofilename = pathname.parent_path() /
    boost::filesystem::path( stem + suffix + extension );

  return ofilename.string();
}

int main( int argc, char *argv[] )
{
  itk::NifTKImageIOFactory::Initialize();

  bool flgVerbose;
  bool flgDebug;

  bool flgSingleThreaded;
  bool flgOrientate;
  bool flgResampleImages;

  bool flgFlipVertically;
  bool flgFlipHorizontally;

  bool flgN72;

  bool flgSecondOrderOnly;

  unsigned int iDim;

  int iScale;
  int nScales = 1;

  float sigmaInMM = 1;
  float scaleFactor = 2.;
  float scaleFactorRelativeToInput = 1.;

  float epsilon = 1.0e-05;

  float threshold = 0.;

  double *origin = 0;

  std::string fileOrientationInX;
  std::string fileOrientationInY;

  std::string fileMask;
  std::string fileOutputMask;

  std::string fileOutputS00;
  std::string fileOutputS10;
  std::string fileOutputS01;
  std::string fileOutputS11;
  std::string fileOutputS20;
  std::string fileOutputS02;

  std::string fileOutputFlat;
  std::string fileOutputSlope;
  std::string fileOutputDarkBlob;
  std::string fileOutputLightBlob;
  std::string fileOutputDarkLine;
  std::string fileOutputLightLine;
  std::string fileOutputSaddle;

  std::string fileOutputOrientation;
  std::string fileOutputVariance;

  std::string fileOutputHistogram;
  std::string fileOutputImage;
  std::string fileOutputColourImage;

  std::string fileInputImage;

  // Create the command line parser, passing the
  // 'CommandLineArgumentDescription' structure. The final boolean
  // parameter indicates whether the command line options should be
  // printed out as they are parsed.

  niftk::CommandLineParser CommandLineOptions(argc, argv, clArgList, false);

  CommandLineOptions.GetArgument( O_DEBUG, flgDebug );
  CommandLineOptions.GetArgument( O_VERBOSE, flgVerbose );

  CommandLineOptions.GetArgument( O_SINGLE_THREADED, flgSingleThreaded );
  CommandLineOptions.GetArgument( O_RESAMPLE_IMAGES, flgResampleImages );

  CommandLineOptions.GetArgument( O_ORIENTATE, flgOrientate );

  CommandLineOptions.GetArgument( O_72_ORIENTATIONS, flgN72 );

  CommandLineOptions.GetArgument( O_FLIP_VERTICALLY,   flgFlipVertically );
  CommandLineOptions.GetArgument( O_FLIP_HORIZONTALLY, flgFlipHorizontally );

  CommandLineOptions.GetArgument( O_SECOND_ORDER_ONLY, flgSecondOrderOnly);

  CommandLineOptions.GetArgument( O_ORIGIN, origin );

  CommandLineOptions.GetArgument( O_SIGMA_IN_MM, sigmaInMM );
  CommandLineOptions.GetArgument( O_NUMBER_OF_SCALES, nScales );
  CommandLineOptions.GetArgument( O_SCALE_FACTOR, scaleFactor );

  CommandLineOptions.GetArgument( O_EPSILON, epsilon );

  CommandLineOptions.GetArgument( O_ORIENTATION_INX, fileOrientationInX );
  CommandLineOptions.GetArgument( O_ORIENTATION_INY, fileOrientationInY );

  if ( (fileOrientationInX.length() || fileOrientationInY.length()) && origin) {

    std::cerr <<"Command line options: -u2D and -v2D cannot be used with -origin";
    return EXIT_FAILURE;
  }

  if ( (fileOrientationInX.length() || fileOrientationInY.length())
       && ! (fileOrientationInX.length() && fileOrientationInY.length()) ) {

    std::cerr <<"Both command line options: -u2D and -v2D are required";
    return EXIT_FAILURE;
  }

  CommandLineOptions.GetArgument( O_MASK, fileMask );

  CommandLineOptions.GetArgument( O_THRESHOLD, threshold );
  CommandLineOptions.GetArgument( O_OUTPUT_MASK, fileOutputMask );

  CommandLineOptions.GetArgument( O_OUTPUT_S00, fileOutputS00 );
  CommandLineOptions.GetArgument( O_OUTPUT_S10, fileOutputS10 );
  CommandLineOptions.GetArgument( O_OUTPUT_S01, fileOutputS01 );
  CommandLineOptions.GetArgument( O_OUTPUT_S11, fileOutputS11 );
  CommandLineOptions.GetArgument( O_OUTPUT_S20, fileOutputS20 );
  CommandLineOptions.GetArgument( O_OUTPUT_S02, fileOutputS02 );

  CommandLineOptions.GetArgument( O_OUTPUT_FLAT,       fileOutputFlat );
  CommandLineOptions.GetArgument( O_OUTPUT_SLOPE,      fileOutputSlope );
  CommandLineOptions.GetArgument( O_OUTPUT_DARK_BLOB,  fileOutputDarkBlob );
  CommandLineOptions.GetArgument( O_OUTPUT_LIGHT_BLOB, fileOutputLightBlob );
  CommandLineOptions.GetArgument( O_OUTPUT_DARK_LINE,  fileOutputDarkLine );
  CommandLineOptions.GetArgument( O_OUTPUT_LIGHT_LINE, fileOutputLightLine );
  CommandLineOptions.GetArgument( O_OUTPUT_SADDLE,     fileOutputSaddle );

  CommandLineOptions.GetArgument( O_OUTPUT_ORIENTATION, fileOutputOrientation );
  CommandLineOptions.GetArgument( O_OUTPUT_VARIANCE, fileOutputVariance );

  CommandLineOptions.GetArgument( O_OUTPUT_HISTOGRAM, fileOutputHistogram );
  CommandLineOptions.GetArgument( O_OUTPUT_COLOUR_IMAGE, fileOutputColourImage );
  CommandLineOptions.GetArgument( O_OUTPUT_IMAGE, fileOutputImage );

  CommandLineOptions.GetArgument( O_INPUT_IMAGE, fileInputImage );

  // Read the input image
  // ~~~~~~~~~~~~~~~~~~~~

  int dims = itk::PeekAtImageDimension(fileInputImage);
  // Define the dimension of the images
  const unsigned int ImageDimension = 2;

  if (dims != ImageDimension)
  {
    std::cerr << "Unsupported image dimension." << std::endl;
    return EXIT_FAILURE;
  }

  typedef float InputPixelType;
  typedef itk::Image<InputPixelType, ImageDimension> InputImageType;

  typedef itk::ImageFileReader< InputImageType > FileReaderType;

  typedef float OutputPixelType;
  typedef itk::Image<OutputPixelType, ImageDimension> OutputImageType;

  typedef itk::BasicImageFeaturesImageFilter< InputImageType, OutputImageType > BasicImageFeaturesFilterType;

  typedef BasicImageFeaturesFilterType::MaskImageType MaskImageType;

  typedef itk::ImageFileReader< MaskImageType > MaskReaderType;

  FileReaderType::Pointer imageReader = FileReaderType::New();

  imageReader->SetFileName(fileInputImage);

  try
  {
    std::cout << "Reading the input image" << std::endl;
    imageReader->Update();
  }
  catch (itk::ExceptionObject &ex)
  {
    std::cout << ex << std::endl;
    return EXIT_FAILURE;
  }

  InputImageType::SizeType    nPixelsInput;
  InputImageType::SpacingType resnInput;
  InputImageType::PointType   originInput;

  nPixelsInput = imageReader->GetOutput()->GetLargestPossibleRegion().GetSize();
  resnInput    = imageReader->GetOutput()->GetSpacing();
  originInput  = imageReader->GetOutput()->GetOrigin();

  // Set up the image resampler
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~

  InputImageType::SizeType    nPixelsResampled;
  InputImageType::SpacingType resnResampled;
  InputImageType::PointType   originResampled;

  InputImageType::Pointer pInputImage;

  typedef itk::ResampleImageFilter< InputImageType, InputImageType > ResampleFilterType;
  ResampleFilterType::Pointer resampleInputFilter = 0;

  typedef itk::IdentityTransform< double, ImageDimension > IdentityTransformType;
  IdentityTransformType::Pointer resampleIdentityTransform = 0;

  typedef itk::LinearInterpolateImageFunction< InputImageType, double > ResampleInterpolatorType;
  ResampleInterpolatorType::Pointer resampleInterpolator = 0;

  if ( flgResampleImages ) {

    for ( iDim=0; iDim<ImageDimension; iDim++) {

      nPixelsResampled[iDim] = nPixelsInput[iDim];
      resnResampled[iDim]    = resnInput[iDim];
      originResampled[iDim]  = originInput[iDim];
    }

    resampleInputFilter = ResampleFilterType::New();

    resampleIdentityTransform = IdentityTransformType::New();
    resampleInterpolator      = ResampleInterpolatorType::New();

    resampleInputFilter->SetInput( imageReader->GetOutput() );
    resampleInputFilter->SetOutputSpacing( resnResampled );
    resampleInputFilter->SetOutputOrigin( originResampled );
    resampleInputFilter->SetSize( nPixelsResampled );

    resampleInputFilter->SetTransform( resampleIdentityTransform );
    resampleInputFilter->SetInterpolator( resampleInterpolator );

    resampleInputFilter->SetDefaultPixelValue( 0 );

    InputImageType::DirectionType direction;
    direction.SetIdentity();
    resampleInputFilter->SetOutputDirection( direction );

    try
      {
	std::cout << "Resampling the input image" << std::endl;
	resampleInputFilter->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    pInputImage = resampleInputFilter->GetOutput();

  }
  else
    pInputImage = imageReader->GetOutput();

  // Read the local orientation images
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  FileReaderType::Pointer xOrientReader, yOrientReader;

  if ( (fileOrientationInX.length() > 0) && (fileOrientationInY.length() > 0) ) {

    flgOrientate = true;

    xOrientReader = FileReaderType::New();

    xOrientReader->SetFileName( fileOrientationInX );

    try
      {
	std::cout << "Reading the local orientation in 'x'" << std::endl;
	xOrientReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    yOrientReader = FileReaderType::New();

    yOrientReader->SetFileName( fileOrientationInY );

    try
      {
	std::cout << "Reading the local orientation in 'y'" << std::endl;
	yOrientReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }
  }

  // Read the mask
  // ~~~~~~~~~~~~~

  MaskImageType::Pointer pMaskImage = 0;
  MaskReaderType::Pointer maskReader = 0;

  if ( fileMask.length() > 0 ) {

    maskReader = MaskReaderType::New();

    maskReader->SetFileName(fileMask);

    try
      {
	std::cout << "Reading the mask image" << std::endl;
	maskReader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
	std::cout << ex << std::endl;
	return EXIT_FAILURE;
      }

    pMaskImage = maskReader->GetOutput();
  }

  // Or create it by thresholding the input image

  if ( threshold ) {

    if ( ! pMaskImage ) {

      pMaskImage = MaskImageType::New();

      pMaskImage->SetRegions( pInputImage->GetLargestPossibleRegion() );
      pMaskImage->SetSpacing( pInputImage->GetSpacing() );
      pMaskImage->SetOrigin( pInputImage->GetOrigin() );

      pMaskImage->Allocate( );
      pMaskImage->FillBuffer( 1 );
    }

    typedef itk::ImageRegionConstIterator< InputImageType > InputIteratorType;

    InputIteratorType itInput( pInputImage, pInputImage->GetLargestPossibleRegion() );

    InputImageType::IndexType index;

    itInput.GoToBegin();

    while (! itInput.IsAtEnd() ) {

      index = itInput.GetIndex();

      if ( itInput.Get() < threshold )

	pMaskImage->SetPixel( index, 0);

      ++itInput;
    }
  }

  if ( pMaskImage && ( fileOutputMask.length() > 0 ) ) {

    typedef itk::ImageFileWriter< MaskImageType > FileWriterType;

    FileWriterType::Pointer writer = FileWriterType::New();

    writer->SetFileName( fileOutputMask.c_str() );
    writer->SetInput( pMaskImage );

    try
    {
      std::cout << "Writing: " << fileOutputMask.c_str() << std::endl;
      writer->Update();
    }
    catch (itk::ExceptionObject &e)
    {
      std::cerr << e << std::endl;
    }
  }

  // Create the basic image features filter
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  BasicImageFeaturesFilterType::Pointer BIFsFilter = BasicImageFeaturesFilterType::New();

  if ( flgDebug )
    BIFsFilter->DebugOn();

  if ( flgVerbose )
    BIFsFilter->VerboseOn();

  if (flgSingleThreaded)
    BIFsFilter->SetSingleThreadedExecution();

  BIFsFilter->SetEpsilon( epsilon );

  if (flgOrientate) {
    BIFsFilter->CalculateOrientatedBIFs();

    if ( flgN72 )
      BIFsFilter->SetNumberOfOrientations( 72 );
  }

  if ( flgFlipVertically )   BIFsFilter->SetFlipVertically();
  if ( flgFlipHorizontally ) BIFsFilter->SetFlipHorizontally();

  if ( flgSecondOrderOnly ) BIFsFilter->SecondOrderOnly();

  if (origin) {
    BasicImageFeaturesFilterType::OriginType bifOrigin;

    bifOrigin[0] = origin[0];
    bifOrigin[1] = origin[1];

    BIFsFilter->SetOrigin( bifOrigin );
  }

  if ( (fileOrientationInX.length() > 0) && (fileOrientationInY.length() > 0) )
    BIFsFilter->SetLocalOrientation( xOrientReader->GetOutput(),
				     yOrientReader->GetOutput() );

  if ( pMaskImage )
    BIFsFilter->SetMask( pMaskImage );

  BIFsFilter->SetInput( pInputImage );

  // Run the filter at each scale
  // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~

  for (iScale=0; iScale<nScales; iScale++) {

    BIFsFilter->SetSigma( sigmaInMM );

    try
      {
	std::cout << "Computing basic image features" << std::endl;
	BIFsFilter->Update();
      }
    catch (itk::ExceptionObject &e)
      {
	std::cerr << e << std::endl;
      }

    // Compute a histogram of the BIFs
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputHistogram.length() > 0 ) {

      unsigned int iBin;
      unsigned int nBins;

      float nPixels = 0;
      float *histogram;

      OutputPixelType pixel;

      OutputImageType::Pointer bifs = BIFsFilter->GetOutput();

      if (flgOrientate) {
	if ( flgN72 )
	  nBins = 183;
	else
	  nBins = 23;
      }
      else
	nBins = 7;

      histogram = new float[nBins];

      for (iBin=0; iBin<nBins; iBin++)
	histogram[ iBin ] = 0.;

      if ( pMaskImage ) {

	typedef itk::ImageRegionConstIterator< MaskImageType > MaskIteratorType;

	MaskImageType::Pointer mask = pMaskImage;

	MaskIteratorType itMask( pMaskImage, pMaskImage->GetLargestPossibleRegion() );

	MaskImageType::IndexType index;

	itMask.GoToBegin();

	while (! itMask.IsAtEnd() ) {

	  index = itMask.GetIndex();

	  if ( itMask.Get() > 0 ) {	// if inside the mask

	    pixel = bifs->GetPixel( index );

	    if ( (pixel < 0) || (pixel >= nBins) )
	      std::cerr << "BIF value ("
			<< niftk::ConvertToString(pixel)
			<< ") exceeds histogram range (0 to "
			<< niftk::ConvertToString(nBins - 1) << ".";

	    else {
	      nPixels++;
	      histogram[ (unsigned int) pixel ]++;
	    }
	  }

	  ++itMask;
	}
      }
      else {
	typedef itk::ImageRegionConstIterator< OutputImageType > IteratorType;

	IteratorType itBIFs( bifs, bifs->GetLargestPossibleRegion() );

	itBIFs.GoToBegin();

	while (! itBIFs.IsAtEnd() ) {

	  pixel = itBIFs.Get();

	  if ( (pixel < 0) || (pixel >= nBins) )
	    std::cerr <<std::string("BIF value (")
		      << niftk::ConvertToString(pixel)
		      << ") exceeds histogram range (0 to "
		      << niftk::ConvertToString(nBins - 1) + ".";

	  else {
	    nPixels++;
	    histogram[ (unsigned int) pixel ]++;
	  }

	  ++itBIFs;
	}

      }

      std::fstream fout;
      fout.open( AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ).c_str(), std::ios::out );

      if ((! fout) || fout.bad()) {
	std::cerr << "Failed to open file: "
		  << AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ) << std::endl;
	exit(1);
      }

      std::cout << "Writing: "
		<< AddScaleSuffix( fileOutputHistogram, sigmaInMM, nScales ) << std::endl;

      for (iBin=0; iBin<nBins; iBin++)

	fout << std::setw(6) << iBin << " "
	     << histogram[ iBin ]/nPixels << std::endl;

      delete histogram;
      fout.close();
    }

    // Write the derivatives?
    // ~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputS00.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 0, AddScaleSuffix( fileOutputS00,
							    sigmaInMM, nScales ) );
    if ( fileOutputS10.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 1, AddScaleSuffix( fileOutputS10,
							    sigmaInMM, nScales ) );
    if ( fileOutputS01.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 2, AddScaleSuffix( fileOutputS01,
							    sigmaInMM, nScales ) );
    if ( fileOutputS11.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 3, AddScaleSuffix( fileOutputS11,
							    sigmaInMM, nScales ) );
    if ( fileOutputS20.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 4, AddScaleSuffix( fileOutputS20,
							    sigmaInMM, nScales ) );
    if ( fileOutputS02.length() != 0 )
      BIFsFilter->WriteDerivativeToFile( 5, AddScaleSuffix( fileOutputS02,
							    sigmaInMM, nScales ) );

    // Write the filter responses?
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputFlat.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 0, AddScaleSuffix( fileOutputFlat,
								sigmaInMM, nScales ) );

    if ( fileOutputSlope.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 1, AddScaleSuffix( fileOutputSlope,
								sigmaInMM, nScales ) );

    if ( fileOutputDarkBlob.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 2, AddScaleSuffix( fileOutputDarkBlob,
								sigmaInMM, nScales ) );

    if ( fileOutputLightBlob.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 3, AddScaleSuffix( fileOutputLightBlob,
								sigmaInMM, nScales ) );

    if ( fileOutputLightLine.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 4, AddScaleSuffix( fileOutputLightLine,
								sigmaInMM, nScales ) );

    if ( fileOutputDarkLine.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 5, AddScaleSuffix( fileOutputDarkLine,
								sigmaInMM, nScales ) );

    if ( fileOutputSaddle.length() != 0 )
      BIFsFilter->WriteFilterResponseToFile( 6, AddScaleSuffix( fileOutputSaddle,
								sigmaInMM, nScales ) );

    // Write the BIF image?
    // ~~~~~~~~~~~~~~~~~~~~

    if (fileOutputImage.length() != 0) {

      typedef itk::ImageFileWriter< OutputImageType > FileWriterType;

      FileWriterType::Pointer writer = FileWriterType::New();

      writer->SetFileName( AddScaleSuffix( fileOutputImage, sigmaInMM, nScales ) );
      writer->SetInput( BIFsFilter->GetOutput() );

      try
	{
	  std::cout << "Writing: "
		    << AddScaleSuffix( fileOutputImage, sigmaInMM, nScales ) << std::endl;
	  writer->Update();
	}
      catch (itk::ExceptionObject &e)
	{
	  std::cerr << e << std::endl;
	}
    }

    if (fileOutputColourImage.length() != 0) {

      typedef itk::RGBPixel<unsigned char> RGBPixelType;
      typedef itk::Image<RGBPixelType, 2> RGBImageType;

      typedef itk::ImageFileWriter< RGBImageType > FileWriterType;

      FileWriterType::Pointer writer = FileWriterType::New();
      writer->SetFileName( AddScaleSuffix( fileOutputColourImage, sigmaInMM, nScales ) );

      if (flgOrientate) {
	if ( flgN72 ) {
	  typedef itk::Functor::ScalarToRGBOBIFPixelFunctor<OutputPixelType, 72> ColorMapFunctorType;
	  typedef itk::UnaryFunctorImageFilter<OutputImageType, RGBImageType, ColorMapFunctorType> ColorMapFilterType;

	  ColorMapFilterType::Pointer colormapper = ColorMapFilterType::New();
	  colormapper->SetInput(BIFsFilter->GetOutput());
	  colormapper->UpdateLargestPossibleRegion();

	  writer->SetInput(colormapper->GetOutput());
	}
	else {
	  typedef itk::Functor::ScalarToRGBOBIFPixelFunctor<OutputPixelType, 8> ColorMapFunctorType;
	  typedef itk::UnaryFunctorImageFilter<OutputImageType, RGBImageType, ColorMapFunctorType> ColorMapFilterType;

	  ColorMapFilterType::Pointer colormapper = ColorMapFilterType::New();
	  colormapper->SetInput(BIFsFilter->GetOutput());
	  colormapper->UpdateLargestPossibleRegion();

	  writer->SetInput(colormapper->GetOutput());
	}
      }
      else {
	typedef itk::Functor::ScalarToRGBBIFPixelFunctor<OutputPixelType> ColorMapFunctorType;
	typedef itk::UnaryFunctorImageFilter<OutputImageType, RGBImageType, ColorMapFunctorType> ColorMapFilterType;

	ColorMapFilterType::Pointer colormapper = ColorMapFilterType::New();
	colormapper->SetInput(BIFsFilter->GetOutput());
	colormapper->UpdateLargestPossibleRegion();

	writer->SetInput(colormapper->GetOutput());
      }

      try
	{
	  std::cout << "Writing: "
		    << AddScaleSuffix( fileOutputColourImage, sigmaInMM, nScales ) << std::endl;
	  writer->Update();
	}
      catch (itk::ExceptionObject &e)
	{
	  std::cerr << e << std::endl;
	}
    }

    // Output the orientation image
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputOrientation.length() > 0 ) {

      typedef itk::ImageFileWriter< OutputImageType > FileWriterType;

      FileWriterType::Pointer writer = FileWriterType::New();
      writer->SetFileName( AddScaleSuffix( fileOutputOrientation, sigmaInMM, nScales ) );

      typedef itk::MaskImageFilter< OutputImageType, MaskImageType, OutputImageType >
	MaskFilterType;

      if ( pMaskImage ) {
	MaskFilterType::Pointer maskFilter = MaskFilterType::New();

	maskFilter->SetInput1( BIFsFilter->GetOrientation() );
	maskFilter->SetInput2( pMaskImage );

	maskFilter->Update();

	writer->SetInput( maskFilter->GetOutput() );
      }
      else
	writer->SetInput( BIFsFilter->GetOrientation() );

      try
	{
	  std::cout << "Writing: "
		    << AddScaleSuffix( fileOutputOrientation, sigmaInMM, nScales ) << std::endl;
	  writer->Update();
	}
      catch (itk::ExceptionObject &e)
	{
	  std::cerr << e << std::endl;
	}
    }

    // Calculate the BIF response variance image
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    if ( fileOutputVariance.length() > 0 ) {

      float mean, variance, S00;
      float rFlat, rSlope, rDarkBlob, rLightBlob, rDarkLine, rLightLine, rSaddle;

      OutputImageType::Pointer imVariance = OutputImageType::New();
      OutputImageType::RegionType region = pInputImage->GetLargestPossibleRegion();

      imVariance->SetRegions( region );
      imVariance->SetSpacing( pInputImage->GetSpacing() );
      imVariance->SetOrigin( pInputImage->GetOrigin() );

      imVariance->Allocate( );
      imVariance->FillBuffer( 0. );

      if ( pMaskImage ) {

	typedef itk::ImageRegionConstIterator< MaskImageType > MaskIteratorType;

	MaskIteratorType itMask( pMaskImage, pMaskImage->GetLargestPossibleRegion() );

	MaskImageType::IndexType index;

	itMask.GoToBegin();

	while (! itMask.IsAtEnd() ) {

	  index = itMask.GetIndex();

	  if ( itMask.Get() > 0 ) {	// if inside the mask

	    S00 = BIFsFilter->GetS00()->GetPixel( index );

	    rFlat      = BIFsFilter->GetResponseFlat(     )->GetPixel( index );
	    rSlope     = BIFsFilter->GetResponseSlope(    )->GetPixel( index );
	    rDarkBlob  = BIFsFilter->GetResponseDarkBlob( )->GetPixel( index );
	    rLightBlob = BIFsFilter->GetResponseLightBlob()->GetPixel( index );
	    rDarkLine  = BIFsFilter->GetResponseDarkLine( )->GetPixel( index );
	    rLightLine = BIFsFilter->GetResponseLightLine()->GetPixel( index );
	    rSaddle    = BIFsFilter->GetResponseSaddle(   )->GetPixel( index );

	    mean = ( rFlat + rSlope + rDarkBlob + rLightBlob + rDarkLine + rLightLine + rSaddle )/7.;

	    variance = ( (rFlat      - mean)*(rFlat      - mean) +
			 (rSlope     - mean)*(rSlope     - mean) +
			 (rDarkBlob  - mean)*(rDarkBlob  - mean) +
			 (rLightBlob - mean)*(rLightBlob - mean) +
			 (rDarkLine  - mean)*(rDarkLine  - mean) +
			 (rLightLine - mean)*(rLightLine - mean) +
			 (rSaddle    - mean)*(rSaddle    - mean) )/7.;

	    if ( S00 )
	      imVariance->SetPixel( index, variance/S00 );
	    else
	      imVariance->SetPixel( index, 0.);
	  }

	  ++itMask;
	}
      }
      else {
	typedef itk::ImageRegionConstIterator< OutputImageType > IteratorType;

	IteratorType itVar( imVariance, imVariance->GetLargestPossibleRegion() );

	OutputImageType::IndexType index;

	itVar.GoToBegin();

	while (! itVar.IsAtEnd() ) {

	  index = itVar.GetIndex();

	  S00 = BIFsFilter->GetS00()->GetPixel( index );

	  rFlat      = BIFsFilter->GetResponseFlat(     )->GetPixel( index );
	  rSlope     = BIFsFilter->GetResponseSlope(    )->GetPixel( index );
	  rDarkBlob  = BIFsFilter->GetResponseDarkBlob( )->GetPixel( index );
	  rLightBlob = BIFsFilter->GetResponseLightBlob()->GetPixel( index );
	  rDarkLine  = BIFsFilter->GetResponseDarkLine( )->GetPixel( index );
	  rLightLine = BIFsFilter->GetResponseLightLine()->GetPixel( index );
	  rSaddle    = BIFsFilter->GetResponseSaddle(   )->GetPixel( index );

	  mean = ( rFlat + rSlope + rDarkBlob + rLightBlob + rDarkLine + rLightLine + rSaddle )/7.;

	  variance = ( (rFlat      - mean)*(rFlat      - mean) +
		       (rSlope     - mean)*(rSlope     - mean) +
		       (rDarkBlob  - mean)*(rDarkBlob  - mean) +
		       (rLightBlob - mean)*(rLightBlob - mean) +
		       (rDarkLine  - mean)*(rDarkLine  - mean) +
		       (rLightLine - mean)*(rLightLine - mean) +
		       (rSaddle    - mean)*(rSaddle    - mean) )/7.;

	  if ( S00 )
	    imVariance->SetPixel( index, variance/S00 );
	  else
	    imVariance->SetPixel( index, 0.);

	  ++itVar;
	}
      }

      typedef itk::ImageFileWriter< OutputImageType > FileWriterType;

      FileWriterType::Pointer writer = FileWriterType::New();

      writer->SetFileName( AddScaleSuffix( fileOutputVariance, sigmaInMM, nScales ) );
      writer->SetInput( imVariance );

      try
	{
	  std::cout << "Writing: "
		    << AddScaleSuffix( fileOutputVariance, sigmaInMM, nScales ) << std::endl;
	  writer->Update();
	}
      catch (itk::ExceptionObject &e)
	{
	  std::cerr << e << std::endl;
	}
    }

    // Increase the scale used
    // ~~~~~~~~~~~~~~~~~~~~~~~

    sigmaInMM *= scaleFactor;
    scaleFactorRelativeToInput *= scaleFactor;

    // Update the resampling?
    // ~~~~~~~~~~~~~~~~~~~~~~

    if ( flgResampleImages ) {
      float actualSamplingFactor;

      for ( iDim=0; iDim<ImageDimension; iDim++) {

	nPixelsResampled[iDim] = static_cast<InputImageType::SizeValueType>( ceil( ((float) nPixelsInput[iDim])
									      / scaleFactorRelativeToInput ) );

	actualSamplingFactor = ((float) nPixelsInput[iDim]) / ((float) nPixelsResampled[iDim] );

	resnResampled[iDim]    = resnInput[iDim] * actualSamplingFactor;

	originResampled[iDim]  = originInput[iDim] + resnResampled[iDim]/2. - resnInput[iDim]/2.;
      }

      resampleInputFilter->SetOutputSpacing( resnResampled );
      resampleInputFilter->SetOutputOrigin( originResampled );
      resampleInputFilter->SetSize( nPixelsResampled );

      resampleInputFilter->SetInput( BIFsFilter->GetS00() );

      try
	{
	  std::cout << "Resampling the input image by: " << actualSamplingFactor << std::endl;
	  resampleInputFilter->UpdateLargestPossibleRegion();
	}
      catch (itk::ExceptionObject &ex)
	{
	  std::cout << ex << std::endl;
	  return EXIT_FAILURE;
	}
    }

  }
}


// Compile with:
// clang++ -o demoPrincipalComponentProjector5D demoPrincipalComponentProjector5D.cpp -L../build/ -I ../include/ -l diamonds -stdlib=libc++ -std=c++11 -Wno-deprecated-register
//

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "File.h"
#include "PrincipalComponentProjector.h"

using namespace std;
using namespace Eigen;

int main()
{
    // Open the input file and read the data (synthetic sampling of a 5D parameter space)

    ifstream inputFile;
    File::openInputFile(inputFile, "kmeans_testsample5D.txt");

    unsigned long Nrows;
    int Ncols;

    File::sniffFile(inputFile, Nrows, Ncols);
    ArrayXXd data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    ArrayXXd sample = data.transpose();
    inputFile.close();

    // Set up the Principal Component Projector and apply this to the input sample

    bool printNdimensions = true;
    PrincipalComponentProjector projector(printNdimensions);

    ArrayXXd optimizedSample;
    optimizedSample = projector.projection(sample);
    ArrayXXd finalSample = optimizedSample.transpose();

    // Print the reduced-dimensionality sample into an output ASCII file

    ofstream outputFile;
    File::openOutputFile(outputFile, "principalComponentProjection5D.txt");
    outputFile << scientific << setprecision(4);
    File::arrayXXdToFile(outputFile, finalSample);
    outputFile.close();

    // That's it!

    return EXIT_SUCCESS;
}


#include "hash_filter_utils.h"
#include "ps-plus/common/initializer/none_initializer.h"
#include <Eigen/Dense>

namespace ps {
namespace server {
namespace udf {

void l2_norm_op(std::shared_ptr<std::vector<HashMapItem>>& indexes, Tensor* input, std::shared_ptr<Tensor>& output) {
  const auto& shape = input->Shape();
  assert(shape.Size() == 2);
  output.reset(new Tensor(input->Type(), TensorShape({shape[0]}), new initializer::NoneInitializer));
  Eigen::MatrixXf m(indexes->size(), shape[1]);
  m.setZero();

  CASES(input->Type(), {
    T* data_ptr = input->Raw<T>();
    T* data;
    for (size_t i = 0; i < indexes->size(); i++) {

      T* data = data_ptr + (*indexes)[i].id * shape[1];
      for (size_t j = 0; j < shape[1]; j++) {
        m(i, j) = *data;
        ++data;
      }
    }
  });

  //auto& res = m.rowwise().squaredNorm();
  auto& res = m.rowwise().norm();

  CASES(output->Type(), {
    T* data_ptr = output->Raw<T>();
    T* data;
    for (size_t i = 0; i < indexes->size(); i++) {
      data = data_ptr + (*indexes)[i].id;
      *data = res(i);
      ++data;
    }
  });

  printf("l2_norm_op indexes size: %ld, l2 norm output range(%f, %f) \n",
         indexes->size(), res.minCoeff(), res.maxCoeff());
};
}
}
}


#pragma once

#include <Eigen/Dense>

#include "quadrature/qmaxwell.hpp"
#include "spectral/lagrange_polynomial.hpp"

namespace boltzmann {
namespace impl_lsq {

class LSQ_BC_Base
{
 public:
  LSQ_BC_Base(int K);

  virtual void apply(Eigen::DenseBase<DERIVED>& dst, const Eigen::DenseBase<DERIVED>& src) const;

 protected:
  Eigen::MatrixXd L_;
};

LSQ_BC_Base::LSQ_BC_Base(int K)
{
  L_.resize(K, K);

  QMaxwell y_quad(1.0, K);
  QHermiteW qherm(1.0, K);

  Eigen::VectorXd y(K);
  for (int i = 0; i < K; ++i) {
    y(i) = -y_quad.pts(i);
  }

  lagrange_poly_simple lagpoly(qherm.pts().data(), qherm.pts().size());

  for (int i2 = 0; i2 < K; ++i2) {
    for (int j = 0; j < K; ++j) {
      double val = 0;
      for (int q = 0; q < K; ++q) {
        val += lagpoly.eval(j, y(q)) * lagpoly.eval(i2, y(q)) * y_quad.wts(q);
      }
      L_(i2, j) = val;
    }
  }
}

void
LSQ_BC_Base::apply(Eigen::DenseBase& dst, const Eigen::DenseBase& src) const
{
  dst = L_ * src;
}

}  // impl_lsq
}  // boltzmann


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/Plane.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathPlaneTestSuite);

BOOST_AUTO_TEST_CASE(PlaneIsConstructibleFromPointAndNormal) {
	const auto p = Plane({ 0.0f, 1.0f, 0.0f }, { 2.0f, 2.0f, 2.0f });

	BOOST_CHECK_EQUAL(p.normal(), Vec3(0.0f, 1.0f, 0.0f).normalised());
	BOOST_CHECK_CLOSE(p.signedDistanceToOrigin(), -2.0f, 0.01f);
}

BOOST_AUTO_TEST_CASE(PlaneIsConstructibleFromNormalAndDistanceFromOrigin) {
	const auto p = Plane({ 2.0f, 1.0f, 3.0f }, 3.0f);

	BOOST_CHECK_EQUAL(p.normal(), Vec3(2.0f, 1.0f, 3.0f).normalised());
	BOOST_CHECK_CLOSE(p.signedDistanceToOrigin(), 3.0f, 0.01f);
}

BOOST_AUTO_TEST_CASE(CanTestPointPositionWithRespectToPlane) {
	const auto p = Plane({ 1.0f, 1.0f, 0.1f }, { -1.0f, -1.0f, 1.0f });

	BOOST_CHECK_CLOSE_FRACTION(p.signedDistanceToPoint({ 1.0f, 0.0f, 1.0f }), 2.116036f, 0.01f);
	BOOST_CHECK_CLOSE(p.signedDistanceToPoint({ -1.0f, -1.0f, 1.0f }), 0.0f, 0.01f);
	BOOST_CHECK_CLOSE(p.signedDistanceToPoint({ -2.0f, -0.5f, 3.0f }), -0.211603f, 0.01f);
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathPlaneTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // anonymous namespace


#include "Align.h"

#include <iostream>

#include <Eigen/SVD>
#include <cmath>

int flag = 0;
using namespace Eigen;
using namespace std;

Eigen::Matrix3d exp(Eigen::Matrix3d x) {
	//return x.exp();
	double theta = sqrt(x(0, 1)*x(0, 1) + x(0, 2)*x(0, 2) + x(1, 2)*x(1, 2));
	if (abs(theta) == 0) return Eigen::Matrix3d::Identity();
	x /= theta;
	return Eigen::Matrix3d::Identity() +
		x * sin(theta) +
		x * x * (1 - cos(theta));
}

Eigen::Matrix3d log(Eigen::Matrix3d x) {
	//return x.log();
	double theta = (x.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) == 0) return Eigen::Matrix3d::Zero();
	return (theta / (2 * sin(theta))) * (x - x.transpose());
}

double polarDec(const Eigen::Matrix3d &a, Eigen::Matrix3d &r, Eigen::Matrix3d &s) {
	JacobiSVD<Eigen::MatrixXd> svd(a, ComputeThinU | ComputeThinV);
	r = svd.matrixU() * svd.matrixV().transpose();
	s = svd.matrixV() * svd.singularValues().asDiagonal() * svd.matrixV().transpose();

	if (r.determinant() < 0) {
		Vector3d sv = svd.singularValues();
		int minsv = 0;
		r = svd.matrixU();
		if (sv(1) < sv(minsv)) minsv = 1;
		if (sv(2) < sv(minsv)) minsv = 2;
		if (sv(minsv) < -Eps) {
			cerr << "polar dec Error, min singular values <= 0 :" << endl;
			cerr << a << endl;
		}
		//cout << "Min SV " << sv(minsv) << " " << minsv << endl;
		r.col(minsv) *= -1;
		sv(minsv) *= -1;
		//cout << "R :\n" << r << endl;
		r = r * svd.matrixV().transpose();
		s = svd.matrixV() * sv.asDiagonal() * svd.matrixV().transpose();
		return sv.sum();
	}
	return svd.singularValues().sum();
}

AffineAlign::AffineAlign(std::vector<Eigen::Vector3d> &v) : p(v), AtA(Matrix3d::Zero()) {
	//if (flag) fout << p.size() << endl;
	for (int i = 0; i < p.size(); i++) {
		//assert(p[i] == p[i]);
		AtA += p[i] * p[i].transpose();
		//if (flag) fout << p[i] << endl << endl;
	}
	if (flag) {
		//fout << "AtA" << endl;
		//fout << AtA << endl;
		//fout << AtA.determinant() << endl;
	}
	AtA = AtA.inverse().eval();
}

Matrix3d AffineAlign::calc(const std::vector<Vector3d> &v) {
	if (v.size() != p.size()) {
		cout << "!!Error v.size() != p.size()" << endl;
		//fout << "!!Error v.size() != p.size()" << endl;
	}
	Vector3d vx(Vector3d::Zero()), vy(Vector3d::Zero()), vz(Vector3d::Zero());
	for (int i = 0; i < p.size(); i++) {
		vx += p[i] * v[i](0);
		vy += p[i] * v[i](1);
		vz += p[i] * v[i](2);
	}
	//assert(vx == vx);
	vx = AtA * vx;
	vy = AtA * vy;
	vz = AtA * vz;
	Matrix3d res;
	res << vx, vy, vz;
	return res.transpose();
}

double AffineAlign::residualwithoutnormal(Eigen::Matrix3d m, std::vector<Eigen::Vector3d> v) {
	double rs = 0;
	for (int i = 0; i < v.size() - 1; i++)
		rs += (v[i] - m*p[i]).squaredNorm();
	return rs;
}

OpenMesh::Vec3d EtoO(const Eigen::Vector3d &v) {
	return OpenMesh::Vec3d(v(0), v(1), v(2));
}

Eigen::Vector3d OtoE(const OpenMesh::Vec3d &v) {
	return Eigen::Vector3d(v[0], v[1], v[2]);
}

void Rot::ToLogR()
{
	double the = circlek * 2 * M_PI + theta;
	//the = theta;
	logr = Eigen::Matrix3d::Zero();
	logr(0, 1) = -axis(2);
	logr(0, 2) = axis(1);
	logr(1, 2) = -axis(0);
	Eigen::Matrix3d logr1 = logr.transpose();
	logr = the*(logr - logr1);
}

double Rot::ToAngle()
{
	double the = circlek * 2 * M_PI + theta;
	return the;
}

Rot logrot(Eigen::Matrix3d jrotation)
{
	Rot jrot;
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <= 1e-6)
	{
		jrot.circlek = 0;
		jrot.theta = 0;
		jrot.axis = Eigen::Vector3d::Zero();
	}
	else
	{
		jrot.theta = theta;
		Eigen::Matrix3d tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());
		Eigen::Vector3d jaxis;
		jaxis(0) = tmp(2, 1);
		jaxis(1) = tmp(0, 2);
		jaxis(2) = tmp(1, 0);
		jrot.axis = jaxis / jaxis.norm();
	}
	jrot.ToLogR();
	return jrot;
}

void logrot(Rot & jrot ,Eigen::Matrix3d & jrotation)
{
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <= 1e-6)
	{
		jrot.circlek = 0;
		jrot.theta = 0;
		jrot.axis = Eigen::Vector3d::Zero();
	}
	else
	{
		jrot.theta = theta;
		Eigen::Matrix3d tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());
		Eigen::Vector3d jaxis;
		jaxis(0) = tmp(2, 1);
		jaxis(1) = tmp(0, 2);
		jaxis(2) = tmp(1, 0);
		jrot.axis = jaxis / jaxis.norm();
	}
	//jrot.ToLogR();
}

Rot logrot(Eigen::Matrix3d jrotation, Rot irot)
{

	Rot jrot;
	double theta = (jrotation.trace() - 1) / 2;
	theta = acos(max(-1.0, min(1.0, theta)));
	if (abs(theta) <=1e-6)
	//if (abs(sin(theta))<0.0001||abs(theta) <= 0.15)// cylinder ר��
	{
		jrot = irot;
		jrot.theta = 0;
	}
	else
	{
		Eigen::Vector3d jaxis;
		Eigen::Matrix3d tmp = Eigen::Matrix3d::Zero();
		if (abs(theta - M_PI) <= 1e-6)
		{
			jaxis = irot.axis;
			//jrot.theta = M_PI;
			tmp(0, 1) = -jaxis(2);
			tmp(0, 2) = jaxis(1);
			tmp(1, 2) = -jaxis(0);
			Eigen::Matrix3d tmp2 = tmp.transpose();
			tmp = (tmp - tmp2);
		}
		else
		{
			tmp = (1 / (2 * sin(theta))) * (jrotation - jrotation.transpose());

			jaxis(0) = tmp(2, 1);
			jaxis(1) = tmp(0, 2);
			jaxis(2) = tmp(1, 0);
		}

		//debug
		Eigen::Matrix3d logr = Eigen::Matrix3d::Zero();
		logr(0, 1) = -jaxis(2);
		logr(0, 2) = jaxis(1);
		logr(1, 2) = -jaxis(0);
		Eigen::Matrix3d tmp1 = logr.transpose();
		logr = (logr - tmp1);
		double _norm1 = (tmp - logr).squaredNorm();
		if (_norm1 >= 0.0001)
		{
			//cout<<"error"<<endl;
		}
		tmp = theta*tmp;
		double _norm2 = (tmp - log(jrotation)).squaredNorm();
		if (_norm2 > 0.00001)
		{
			log(jrotation);
		}
		double _norm = jaxis.norm();
		if (_norm < 1)
		{
			//cout<<_norm<<endl;
		}
		double _sign = jaxis.dot(irot.axis);
		if (_sign < 0)
		{
			jaxis = -jaxis;
			theta = 2 * PI - theta;
		}
		jrot.axis = jaxis / jaxis.norm();//
		//jrot.axis = jaxis;
		jrot.theta = theta;
		jrot.circlek = irot.circlek;
	}

	if (abs(theta - irot.theta) > M_PI)
	{
		if (irot.theta < M_PI)
		{
			//[0,2*PI)
			jrot.circlek = jrot.circlek - 1;
		}
		else
		{
			if (jrot.circlek >= 0)
			{
				// cout<<"test"<<endl;
			}
			jrot.circlek = jrot.circlek + 1;
		}
	}
	jrot.ToLogR();
	Eigen::Matrix3d tmp = log(jrotation);
	double _norm1 = (jrot.logr - tmp).squaredNorm();
	if (_norm1 >= 0.0001)
	{
		//cout<<"error"<<endl;
	}

	return jrot;
}


// Portfolio VaR through MC simulation
// from test_ptf_mc_var.cpp

#include <string>
#include <vector>

#include <Eigen/Dense>

#include "CVaR.h"

#include "compute_returns_eigen.h"
#include "compute_var.h"
#include "path.h"
#include "mc_engine.h"
#include "rng.h"
#include "pca.h"
#include "portfolio.h"
#include "ptf_var.h"

#include <boost/math/special_functions/erf.hpp>
using boost::math::erfc_inv;

namespace RiskJS {

using namespace Eigen;
using namespace std;

double CVaRHistorical(priceData rawPrices, weightData weights, double alphatest) {
    // Remove lines with missing values
    size_t n(rawPrices.size() - 1);
    size_t m(rawPrices[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n));

    for(size_t i = 1;i < n+1;++i){
        for(size_t j = 1;j < rawPrices[i].size();++j){
            string tmp = rawPrices[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1] = 99999.;
            }
            else{
                _prices[j-1][i-1] = stod(tmp);
            }
        }
    }

    vector<string> indexNames(rawPrices[0].size() - 1);

    for(size_t i = 1;i < rawPrices[0].size();++i){
        indexNames[i-1] = rawPrices[0][i];
    }

		// Remove missing values to compute trailling returns
    Mat prices;
    prices.resize(m,Vec(0));

    bool goodrow = true;
    for(size_t j = 0;j < _prices[0].size();++j){
        for(size_t i = 0;i < _prices.size();++i){
            if((_prices[i][j] == 99999) || (_prices[i][j] == 0))
                goodrow = false;
        }
        if (goodrow)
            for(size_t i = 0;i < _prices.size();++i){
                prices[i].push_back(_prices[i][j]);
            }
            goodrow = true;
        }
    unsigned int windowsize = prices[0].size()-2;
    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,windowsize,true));

    // create portfolio
  	Ptf _ptf;
  	for(size_t i = 0;i < m;++i){
        shared_ptr<Instrument> instrument(new DeltaOne());
        auto p = std::make_pair(i,instrument);
		    _ptf.push_back(p);
    }
	  shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, false, 1.e+07));

    HistoricalVaR model;

    // CVaRhistorical
    model.setAlpha(1.0-alphatest);
    double CVaRHistorical = model(0,ptf->getReturns());

    return CVaRHistorical;
}

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/LevenbergMarquardt>

using namespace std;
using namespace Eigen;

template <typename Scalar>
struct sparseGaussianTest : SparseFunctor<Scalar, int>
{
  typedef Matrix<Scalar,Dynamic,1> VectorType;
  typedef SparseFunctor<Scalar,int> Base;
  typedef typename Base::JacobianType JacobianType;
  sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values)
  { }

  VectorType model(const VectorType& uv, VectorType& x)
  {
    VectorType y; //Change this to use expression template
    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(uv.size()%2 == 0);
    eigen_assert(uv.size() == n);
    eigen_assert(x.size() == m);
    y.setZero(m);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    Scalar coeff;
    for (int j = 0; j < m; j++)
    {
      for (int i = 0; i < half; i++)
      {
        coeff = (x(j)-i)/v(i);
        coeff *= coeff;
        if (coeff < 1. && coeff > 0.)
          y(j) += u(i)*std::pow((1-coeff), 2);
      }
    }
    return y;
  }
  void initPoints(VectorType& uv_ref, VectorType& x)
  {
    m_x = x;
    m_y = this->model(uv_ref,x);
  }
  int operator()(const VectorType& uv, VectorType& fvec)
  {
    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(uv.size()%2 == 0);
    eigen_assert(uv.size() == n);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    fvec = m_y;
    Scalar coeff;
    for (int j = 0; j < m; j++)
    {
      for (int i = 0; i < half; i++)
      {
        coeff = (m_x(j)-i)/v(i);
        coeff *= coeff;
        if (coeff < 1. && coeff > 0.)
          fvec(j) -= u(i)*std::pow((1-coeff), 2);
      }
    }
    return 0;
  }

  int df(const VectorType& uv, JacobianType& fjac)
  {
    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(n == uv.size());
    eigen_assert(fjac.rows() == m);
    eigen_assert(fjac.cols() == n);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    Scalar coeff;

    //Derivatives with respect to u
    for (int col = 0; col < half; col++)
    {
      for (int row = 0; row < m; row++)
      {
        coeff = (m_x(row)-col)/v(col);
          coeff = coeff*coeff;
        if(coeff < 1. && coeff > 0.)
        {
          fjac.coeffRef(row,col) = -(1-coeff)*(1-coeff);
        }
      }
    }
    //Derivatives with respect to v
    for (int col = 0; col < half; col++)
    {
      for (int row = 0; row < m; row++)
      {
        coeff = (m_x(row)-col)/v(col);
        coeff = coeff*coeff;
        if(coeff < 1. && coeff > 0.)
        {
          fjac.coeffRef(row,col+half) = -4 * (u(col)/v(col))*coeff*(1-coeff);
        }
      }
    }
    return 0;
  }

  VectorType m_x, m_y; //Data points
};

template<typename T>
void test_sparseLM_T()
{
  typedef Matrix<T,Dynamic,1> VectorType;

  int inputs = 10;
  int values = 2000;
  sparseGaussianTest<T> sparse_gaussian(inputs, values);
  VectorType uv(inputs),uv_ref(inputs);
  VectorType x(values);
  // Generate the reference solution
  uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3;
  //Generate the reference data points
  x.setRandom();
  x = 10*x;
  x.array() += 10;
  sparse_gaussian.initPoints(uv_ref, x);

  // Generate the initial parameters
  VectorBlock<VectorType> u(uv, 0, inputs/2);
  VectorBlock<VectorType> v(uv, inputs/2, inputs/2);
  v.setOnes();
  //Generate u or Solve for u from v
  u.setOnes();

  // Solve the optimization problem
  LevenbergMarquardt<sparseGaussianTest<T> > lm(sparse_gaussian);
  int info;
//   info = lm.minimize(uv);

  VERIFY_IS_EQUAL(info,1);
    // Do a step by step solution and save the residual
  int maxiter = 200;
  int iter = 0;
  MatrixXd Err(values, maxiter);
  MatrixXd Mod(values, maxiter);
  LevenbergMarquardtSpace::Status status;
  status = lm.minimizeInit(uv);
  if (status==LevenbergMarquardtSpace::ImproperInputParameters)
      return ;

}
void test_sparseLM()
{
  CALL_SUBTEST_1(test_sparseLM_T<double>());

  // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
}


/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2021 University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */

/**
 * @file   test-mp2p_error_terms_jacobians.cpp
 * @brief  Unit tests for Jacobians of error terms
 * @author Francisco Jose Mañas Alvarez, Jose Luis Blanco Claraco
 * @date   Apr 10, 2020
 */

#include <mp2p_icp/errorTerms.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/math/num_jacobian.h>  // finite difference method
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/random.h>
#include <Eigen/Dense>
#include <iostream>  // cerr

using namespace mrpt;  // for the "_deg" suffix
using namespace mrpt::math;
using namespace mrpt::poses;

auto& rnd = mrpt::random::getRandomGenerator();

static double normald(const double sigma)
{
    return rnd.drawGaussian1D_normalized() * sigma;
}
static float normalf(const float sigma)
{
    return rnd.drawGaussian1D_normalized() * sigma;
}

// ===========================================================================
//  Test: error_point2point
// ===========================================================================

void test_Jacob_error_point2point()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mrpt::tfest::TMatchingPair pair;

    pair.this_x  = normalf(20);
    pair.this_y  = normalf(20);
    pair.this_z  = normalf(20);
    pair.other_x = normalf(10);
    pair.other_y = normalf(10);
    pair.other_z = normalf(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;
    // const mrpt::math::CVectorFixed<double, 3> error = // (Ignored here)
    mp2p_icp::error_point2point(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2point(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_point2line
// ===========================================================================

void test_Jacob_error_point2line()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::point_line_pair_t pair;

    pair.ln_this.pBase.x = normalf(20);
    pair.ln_this.pBase.y = normalf(20);
    pair.ln_this.pBase.z = normalf(20);
    pair.ln_this.director[0] = normald(20);
    pair.ln_this.director[1] = normald(20);
    pair.ln_this.director[2] = normald(20);

    pair.pt_other.x = normalf(10);
    pair.pt_other.y = normalf(10);
    pair.pt_other.z = normalf(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 1, 12> J1;

    mp2p_icp::error_point2line(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 1, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<1>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<1>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_point2plane
// ===========================================================================

void test_Jacob_error_point2plane()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::point_plane_pair_t pair;

    pair.pl_this.centroid.x = normalf(20);
    pair.pl_this.centroid.y = normalf(20);
    pair.pl_this.centroid.z = normalf(20);
    pair.pl_this.plane.coefs[0] = normald(20);
    pair.pl_this.plane.coefs[1] = normald(20);
    pair.pl_this.plane.coefs[2] = normald(20);

    pair.pt_other.x = normalf(10);
    pair.pt_other.y = normalf(10);
    pair.pt_other.z = normalf(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 1, 12> J1;

    mp2p_icp::error_point2plane(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 1, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<1>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<1>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_point2plane(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_line2line
// ===========================================================================

void test_Jacob_error_line2line()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::matched_line_t pair;

    pair.ln_this.pBase.x = normalf(10);
    pair.ln_this.pBase.y = normalf(10);
    pair.ln_this.pBase.z = normalf(10);
    pair.ln_this.director[0] = normald(10);
    pair.ln_this.director[1] = normald(10);
    pair.ln_this.director[2] = normald(10);

    pair.ln_other.pBase.x = normalf(10);
    pair.ln_other.pBase.y = normalf(10);
    pair.ln_other.pBase.z = normalf(10);
    pair.ln_other.director[0] = normald(10);
    pair.ln_other.director[1] = normald(10);
    pair.ln_other.director[2] = normald(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 4, 12> J1;

    mp2p_icp::error_line2line(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<4>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<4>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_line2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_plane2plane
// ===========================================================================

void test_Jacob_error_plane2plane()
{
    const CPose3D p = CPose3D(
        // x y z
        normald(10), normald(10), normald(10),
        // Yaw pitch roll
        rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
        rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));

    mp2p_icp::matched_plane_t pair;

    pair.p_this.centroid.x = normalf(20);
    pair.p_this.centroid.y = normalf(20);
    pair.p_this.centroid.z = normalf(20);
    pair.p_this.plane.coefs[0] = normald(20);
    pair.p_this.plane.coefs[1] = normald(20);
    pair.p_this.plane.coefs[2] = normald(20);

    pair.p_other.centroid.x = normalf(10);
    pair.p_other.centroid.y = normalf(10);
    pair.p_other.centroid.z = normalf(10);
    pair.p_other.plane.coefs[0] = normald(10);
    pair.p_other.plane.coefs[1] = normald(10);
    pair.p_other.plane.coefs[2] = normald(10);

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 3, 12> J1;

    mp2p_icp::error_plane2plane(pair, p, J1);

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<3>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<3>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_plane2plane(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }

    if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
    {
        std::cerr << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\n";
        THROW_EXCEPTION("Jacobian mismatch, see above.");
    }
}

// ===========================================================================
//  Test: error_line2line
// ===========================================================================

void test_error_line2line()
{
    const CPose3D p = CPose3D(
        // x y z
        1, 0.5, 0.1,
        // Yaw pitch roll
        0, 0, 0);

    mp2p_icp::matched_line_t pair;

    pair.ln_this.pBase.x = 0;
    pair.ln_this.pBase.y = 1;
    pair.ln_this.pBase.z = -4;
    pair.ln_this.director[0] =  0.4364;
    pair.ln_this.director[1] =  0.8729;
    pair.ln_this.director[2] = -0.2182;

    pair.ln_other.pBase.x = 2;
    pair.ln_other.pBase.y = 1;
    pair.ln_other.pBase.z = -0.5;
    pair.ln_other.director[0] = 0.2357;
    pair.ln_other.director[1] = 0.2357;
    pair.ln_other.director[2] = 0.9428;

    // Implemented values:
    mrpt::math::CMatrixFixed<double, 4, 12> J1;

    mrpt::math::CVectorFixedDouble<4> error = mp2p_icp::error_line2line(pair, p, J1);

    mrpt::math::CVectorFixedDouble<4> ref_error;
    ref_error[0] = 0.0517;
    ref_error[1] = 0.2007;
    ref_error[2] = 0.6372;
    ref_error[3] = -1.1610;

    std::cout << "\nResultado: \n"
              << error <<  "\nRecta A:\n"
              <<  pair.ln_this << "\nRecta B:\n"
              << pair.ln_other << "\n";

    // (12x6 Jacobian)
    const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);

    const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);

    // Numerical Jacobian:
    CMatrixDouble numJacob;
    {
        CVectorFixedDouble<6> x_mean;
        x_mean.setZero();

        CVectorFixedDouble<6> x_incrs;
        x_incrs.fill(1e-6);
        mrpt::math::estimateJacobian(
            x_mean,
            /* Error function to evaluate */
            std::function<void(
                const CVectorFixedDouble<6>& eps, const CPose3D& D,
                CVectorFixedDouble<4>& err)>(
                /* Lambda, capturing the pair data */
                [pair](
                    const CVectorFixedDouble<6>& eps, const CPose3D& D,
                    CVectorFixedDouble<4>& err) {
                    // SE(3) pose increment on the manifold:
                    const CPose3D incr         = Lie::SE<3>::exp(eps);
                    const CPose3D D_expEpsilon = D + incr;
                    err = mp2p_icp::error_line2line(pair, D_expEpsilon);
                }),
            x_incrs, p, numJacob);
    }
        std::cout << "numJacob:\n"
                  << numJacob.asEigen() << "\njacob:\n"
                  << jacob.asEigen() << "\nDiff:\n"
                  << (numJacob - jacob) << "\nJ1:\n"
                  << J1.asEigen() << "\ndDexp_de:\n"
                  << dDexpe_de.asEigen() << "\n";

}

int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
{
    try
    {
        rnd.randomize(1234);  // for reproducible tests

        test_Jacob_error_point2point();
        test_Jacob_error_point2line();
        test_Jacob_error_point2plane();
        // test_Jacob_error_line2line();
        test_Jacob_error_plane2plane();
        test_error_line2line();
    }
    catch (std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e) << "\n";
        return 1;
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

// This file was modified by Oracle on 2016.
// Modifications copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/size_t.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
    BOOST_MPL_ASSERT_MSG((false),
                         NOT_IMPLEMENTED_FOR_THIS_DIMENSION,
                         (mpl::size_t<Dimension>));
};

template <>
struct cross_product<2>
{
    template <typename P1, typename P2, typename ResultP>
    static inline void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();
        assert_dimension<ResultP, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        set<0>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

template <>
struct cross_product<3>
{
    template <typename P1, typename P2, typename ResultP>
    static inline void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        set<0>(result, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(result, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

*/
template <typename ResultP, typename P1, typename P2>
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<ResultP>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P2>) );

    ResultP result;
    detail::cross_product<dimension<ResultP>::value>::apply(p1, p2, result);
    return result;
}

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

\qbk{[heading Examples]}
\qbk{[cross_product] [cross_product_output]}
*/
template <typename P>
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    P result;
    detail::cross_product<dimension<P>::value>::apply(p1, p2, result);
    return result;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP


/*
 * simulated_telemetry_server.cpp
 *
 *  Created on: Dec 5, 2018
 *      Author: ttw2xk
 */

#include "f1_datalogger/car_data/car_data.h"
#include <boost/program_options.hpp>
#include <iostream>
#include <boost/asio.hpp>
#include <memory>
#include <thread>
#include <math.h>
#include <boost/math/constants/constants.hpp>
namespace po = boost::program_options;
void exit_with_help(po::options_description& desc)
{
        std::stringstream ss;
        ss << "F1 Simulated Telemetry Server. Command line arguments are as follows:" << std::endl;
        desc.print(ss);
        std::printf("%s", ss.str().c_str());
        exit(0); // @suppress("Invalid arguments")
}
int main(int argc, char** argv) {
	using boost::asio::ip::udp;
	using namespace deepf1;
        unsigned int BUFLEN = 1289;
        unsigned int UDP_BUFLEN = BUFLEN;
        unsigned int sleep_time;
        unsigned int packet_size = sizeof(UDPPacket);

        std::string address, port;
        po::options_description desc("Allowed Options");

        try{
                desc.add_options()
                ("help,h", "Displays options and exits")
                ("address,a", po::value<std::string>(&address)->default_value("127.0.0.1"), "IPv4 Address to send data to")
                ("port_number,p", po::value<std::string>(&port)->default_value("20777"), "Port number to send data to")
                ("sleep_time,s", po::value<unsigned int>(&sleep_time)->default_value(100), "Number of milliseconds to sleep between simulated packets")
                ;
	        po::variables_map vm;
                po::store(po::parse_command_line(argc, argv, desc), vm);
                po::notify(vm);
                if (vm.find("help") != vm.end()) {
                        exit_with_help(desc);
                }
        }catch(boost::exception& e){
                exit_with_help(desc);
        }
        boost::asio::io_service io_service;
        udp::resolver resolver(io_service);
        udp::resolver::query query(udp::v4(), address, port);
        udp::endpoint receiver_endpoint = *resolver.resolve(query);
        udp::socket socket(io_service);
        socket.open(udp::v4());

        std::shared_ptr<UDPPacket> data(new UDPPacket);
        float fake_time = 0;
        float dt = 1E-3*((float)sleep_time);
        float period = 5.0;
        float freq=1/period;
	float pi = boost::math::constants::pi<float>();
        while (true) {
                data->m_time = fake_time;
                data->m_steer = sin(2*pi*freq*fake_time);
                data->m_throttle = sin(2*pi*freq*fake_time + pi /3.0);
                data->m_brake = sin(2*pi*freq*fake_time + 2.0*pi /3.0);
                std::cout<<"Sending fake UDP data"<<std::endl;
                // std::cout<<"fake_time: "<<fake_time<<std::endl;
                // std::cout<<"Steering: "<<data->m_steer<<std::endl;
                // std::cout<<"Throttle: "<<data->m_throttle<<std::endl;
                // std::cout<<"B:rake "<<data->m_brake<<std::endl;
                socket.send_to(boost::asio::buffer(boost::asio::buffer(data.get(), packet_size)), receiver_endpoint);
                fake_time += dt;
                std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
        return 0;
		/* */
}


#include "simulate.h"

#include <algorithm>
#include <exception>
#include <climits>
#include <cmath>
#include <utility>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/students_t.hpp>

using namespace std;

const boost::math::normal STANDARD_NORMAL(0, 1);

double testBalance(const arma::Col<double> &z, const arma::Col<double> &t) {
  if (z.n_rows != t.n_rows) throw length_error("Column vectors must be the same length.");
  int N = t.n_rows;             // population size
  int n = 0;                    // draws
  int K = 0;                    // "success" events
  int k = 0;                    // observed successes
  for (int i = 0; i < N; ++i) {
    if (!(t(i) == 0 || t(i) == 1)) throw logic_error("Treatment must be a vector of 0s and 1s.");
    if (!(z(i) == 0 || z(i) == 1)) throw logic_error("Covariate must be a vector of 0s and 1s.");
    if (t(i) == 1) ++n;         // assigning to treatment is draw
    if (z(i) == 1) ++K;         // number of possible success events
    if (t(i) == 1 && z(i) == 1) ++k; // observed successes
  }
  boost::math::hypergeometric_distribution<> X(K, n, N);
  // find Prob(|X - K/N*n| >= |k - K/N*n|) = Prob(X <= K/N*n - |k - K/N*n|) + Prob(X >= K/N*n + |k - K/N*n|)
  double p = ((double) K)/N;
  double mean = p*n;
  double delta = abs(k - p*n);
  double pValue = 0;
  int L = mean - delta;
  int U = mean + delta - 1; // complement is not inclusive
  return U < L ? 1 : boost::math::cdf(X, L) + boost::math::cdf(boost::math::complement(X, U));
}

double calculateVariance(int N, double alpha, double power) {
  double z = boost::math::quantile(boost::math::complement(STANDARD_NORMAL, alpha/2)); // critical value
  double lower = 1;
  double upper = 100000000;
  double mid = (upper + lower)/2;
  while (upper - lower > 0.00000001) {
    double val = boost::math::cdf(STANDARD_NORMAL, -z + sqrt(N)/sqrt(mid)) + boost::math::cdf(STANDARD_NORMAL, -z - sqrt(N)/sqrt(mid));
    if (val > power) {
      lower = mid;
    } else {
      upper = mid;
    }
    mid = (upper + lower)/2;
  }
  return mid;
}

pair<double, double> calculateBetaPValue(const arma::mat &Z, const arma::Col<double> &Y,
                                         double sigma, bool varianceKnown) {
  arma::mat ZZ = Z.t()*Z;
  arma::mat beta = arma::solve(ZZ, Z.t()*Y);
  arma::Col<double> e(ZZ.n_rows, arma::fill::zeros); e(0) = 1;
  arma::mat inverseFirstColumn = arma::solve(ZZ, e);
  if (varianceKnown) {
    double standardDeviation = sigma*sqrt(inverseFirstColumn(0));
    double t = abs(beta(0))/standardDeviation;
    return make_pair(2*cdf(STANDARD_NORMAL, -t), beta(0));
  } else {
    int df = Y.n_rows - beta.n_rows;            // degrees of freedom
    double s = arma::norm(Y - Z*beta)/sqrt(df); // sample standard deviation
    double standardDeviation = s*sqrt(inverseFirstColumn(0));
    double t = abs(beta(0))/standardDeviation;
    boost::math::students_t tDist(df);
    return make_pair(2*cdf(tDist, -t), beta(0));
  }
}

tuple<double, double, int, int, double, double> simulate(const arma::Col<double> &Y, const vector<int> X,
                                                         double sigma, bool varianceKnown,
                                                         arma::mat &Z, mt19937_64 &rng,
                                                         bool interceptTerm) {
  bernoulli_distribution bernoulli(0.5);
  int N = X.size();
  Z.fill(0);
  // bestColumns[k] keeps track of the k + 1 or k + 2 columns that produce the smallest p-value depending on interceptTerm
  vector<arma::uvec> bestColumns; bestColumns.reserve(N - 1);
  if (interceptTerm) { // make intercept term last column of Z
    fill(Z.begin_col(N - 1), Z.end_col(N - 1), 1);
    copy(X.begin(), X.end(), Z.begin_col(0));
    bestColumns.push_back(arma::uvec{0, (unsigned long long) N - 1ULL});
  } else {
    copy(X.begin(), X.end(), Z.begin_col(0));
    bestColumns.push_back(arma::uvec{0});
  }
  // bestPValues[k] corresponds to p-value if the columns bestColumns[k] are used
  vector<pair<double, double>> bestPValues; bestPValues.reserve(N - 1);
  bestPValues.push_back(calculateBetaPValue(Z.cols(bestColumns.front()), Y, sigma, varianceKnown));
  if (bestPValues.front().first <= 0.05) {
    return make_tuple(bestPValues.front().first, bestPValues.front().second, 0, 0, -1, bestPValues.front().first);
  } else {                    // need more covariates
    bool done = false;
    int smallestSubsetSize = INT_MAX;
    /* add covariates one-by-one, we always include the treatment
     * if we're using the intercept two covariates are included by default
     */
    for (int j = 1; j < N - 2 || (j == N - 2 && !interceptTerm); ++j) {
      for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
      if (!interceptTerm) {
        while (arma::rank(Z) <= j) {
          for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
        }
      } else { // offset rank by 1 for intercept term
        while (arma::rank(Z) <= j + 1) {
          for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
        }
      }
      for (int k = j; k >= 1; --k) { // loop through subset sizes, k is the number of additional covariates
        pair<double, double> newPValue;
        if (k == j) {           // use all available covariates
          bestColumns.emplace_back(bestColumns.back().n_rows + 1); // add one more to biggest subset
          for (int l = 0; l < bestColumns.back().n_rows - 1; ++l) {
            bestColumns.back()(l) = bestColumns[j - 1](l); // copy over from original subset
          }
          bestColumns.back()(bestColumns.back().n_rows - 1) = j; // add new covariate
          newPValue = calculateBetaPValue(Z.cols(bestColumns.back()), Y, sigma, varianceKnown);
          bestPValues.push_back(newPValue);
        } else {                // make a new subset of same size with new covariate
          arma::uvec columnSubset(bestColumns[k].n_rows);
          for (int l = 0; l < columnSubset.n_rows - 1; ++l)
            columnSubset(l) = bestColumns[k - 1](l); // copy over from smaller subset
          columnSubset(columnSubset.n_rows - 1) = j; // add new covariate
          newPValue = calculateBetaPValue(Z.cols(columnSubset), Y, sigma, varianceKnown);
          if (bestPValues[k].first > newPValue.first) { // if better subset replace
            bestPValues[k] = newPValue;
            bestColumns[k] = columnSubset;
          }
        }
        if (newPValue.first <= 0.05) { // stop when we reach significance
          done = true;
          smallestSubsetSize = k;
        }
      }
      if (done) {
        // compute balance p value in special case that only 1 covariate was needed
        double balancePValue = -1;
        if (smallestSubsetSize == 1 && !interceptTerm) {
          balancePValue = testBalance(Z.col(bestColumns[1](1)), Z.col(0));
        } else if (smallestSubsetSize == 1 && interceptTerm) {
          balancePValue = testBalance(Z.col(bestColumns[1](2)), Z.col(0));
        }
        return make_tuple(bestPValues.front().first, bestPValues[smallestSubsetSize].second,
                          j, smallestSubsetSize, balancePValue, bestPValues[smallestSubsetSize].first);
      }
    }
  }
  return make_tuple(bestPValues.front().first, bestPValues.front().second, -1, -1, -1, bestPValues.front().first);
}


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <limits>
#include <vector>

class ScaledInvChiSquareTestRig : public VectorRealRNGTestRig {
 public:
  ScaledInvChiSquareTestRig()
      : VectorRealRNGTestRig(10000, 10, {0.5, 1.3, 2.0, 5.8}, {1, 2, 3, 6},
                             {-2.5, -1.7, -0.1, 0.0}, {-3, -2, -1, 0},
                             {0.1, 1.0, 2.5, 4.0}, {1, 2, 3, 4},
                             {-2.7, -1.5, -0.5, 0.0}, {-3, -2, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& nu, const T2& sigma, const T3&,
                        T_rng& rng) const {
    return stan::math::scaled_inv_chi_square_rng(nu, sigma, rng);
  }

  std::vector<double> generate_quantiles(double nu, double sigma,
                                         double) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::inverse_chi_squared_distribution<> dist(nu);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac) * (nu * sigma * sigma));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsScaledInvChiSquare, errorCheck) {
  check_dist_throws_all_types(ScaledInvChiSquareTestRig());
}

TEST(ProbDistributionsScaledInvChiSquare, distributionTest) {
  check_quantiles_real_real(ScaledInvChiSquareTestRig());
}


#include <boost/test/unit_test.hpp>
#include <vector>
#include <numeric>
#include <limits>
#include <list>
#include <set>
#include <queue>
#include <map>
#include <stack>
#include <math.h>

using namespace std;

__int64 CollatzProblem()
{
    map<__int64,__int64> seq;
    map<__int64,__int64>::iterator I;
    seq[2] = 1;

    for( int i=3; i< (int)1e6; ++i )
    {
        stack<__int64> series;
        __int64 v = i;

        for( ;; )
        {
            I = seq.find( v );
            if( I == seq.end() )
            {
                series.push( v );
                v = ( v%2 == 0 ) ? ( v/2 ) : ( 3*v+1 );
                if ( v < 0 )
                {
                    break;
                }
            }
            else
            {
                __int64 c = I->second + 1;
                while( series.size() > 0 )
                {
                    seq[ series.top() ] = c;
                    series.pop();
                    c++;
                }

                break;
            }
        }
    }

    struct CountComp {
        bool operator()( const pair<__int64,__int64>& l, const pair<__int64,__int64>& r) { return l.second<r.second; }
    };

    I = max_element( seq.begin(), seq.end(), CountComp() );
    return I->first;
}

__int64 CollatzProblem_UsingLinearAddress( int N )
{
    int* seq = new int[ N ];
    memset( seq, 0, sizeof(int)*N );
    seq[2] = 1;

    for( int i=3; i< N; ++i )
    {
        stack<__int64> series;
        __int64 v = i;

        for( ;; )
        {
            if( v>=N || (v<N && seq[ (int)v ] == 0) )
            {
                series.push( v );
                v = ( v%2 == 0 ) ? ( v/2 ) : ( 3*v+1 );
                if ( v < 0 )
                {
                    break;
                }
            }
            else
            {
                int c = seq[ (int)v ] + 1;
                while( series.size() > 0 )
                {
                    if ( series.top() < N ) {
                        seq[ (int)series.top() ] = c;
                    }
                    series.pop();
                    c++;
                }

                break;
            }
        }
    }

    struct CountComp {
        bool operator()( const int& l, const int& r) { return l<r; }
    };

    int ret = max_element( seq, seq+N, CountComp() ) - seq;
    delete seq;

    return ret;
}

BOOST_AUTO_TEST_CASE( TestEulerProject14 )
{
    //BOOST_CHECK_EQUAL( 837799, CollatzProblem() );
    //BOOST_CHECK_EQUAL( 837799, CollatzProblem_UsingLinearAddress( 1000000) );
}

void PathInGrid( int r, int d, __int64 &count )
{
    if ( r == 0 || d == 0 )
    {
        count++;
        return;
    }

    PathInGrid( r-1, d, count );
    PathInGrid( r, d-1, count );
}

__int64 PathInGrid_Fast( int d )
{
    vector<__int64> P, C;

    P.assign( 3, 0 );
    P[2]=6; P[1]=3;

    for( int n = 3; n <= d; ++n )
    {
        C.assign( n+1, 0 );

        C[1] = n + 1;
        C[n] = 1 + P[1];
        for( int i=2; i<n; ++i)
        {
            C[i] = P[i] + C[i-1];
            C[n] += P[i];
        }
        C[n] *= 2;

        P.swap( C );
    }

    return P[ d ];
}

BOOST_AUTO_TEST_CASE( TestEulerProject15 )
{
    {
        __int64 count = 0;
        PathInGrid( 3, 3, count );
        BOOST_CHECK_EQUAL( 20, count );
    }

    {
        BOOST_CHECK_EQUAL( 20, PathInGrid_Fast( 3) );
        //BOOST_CHECK_EQUAL( 137846528820, PathInGrid_Fast( 20 ) );
    }
}

__int64 DigitSumOfTwoSquare( int power )
{
	class Number
	{
	public:
        Number( const Number& r ) : _V( r._V ) {}

        Number( const char *str )
		{
			const char *l = str + strlen( str );
			while ( l != str )
			{
				_V.push_back( *(--l) - '0' );
			}
		}

		void Add( const Number& r )
		{
			char v = 0;
			size_t i=0;
			for( ; i<r._V.size(); ++i )
			{
				if( i >= _V.size() )
				{
					_V.push_back( r._V[i] );
				}

				v += r._V[i] + _V[i];
				_V[i] = v%10;
				v /=  10;
			}

			while( v != 0 )
			{
				if( i >= _V.size() ) {
					_V.push_back( v );
					break;
				}
				else
				{
					v += _V[i];
					_V[i] = v % 10;
					v /=  10;
				}
				++i;
			}
		}

		string AsString() const
		{
			string r;
			for( int i=_V.size()-1; i>=0; --i)
			{
				r.push_back( _V[i] + '0' );
			}

			return r;
		}

	private:
		vector<char> _V;
	};

	if( power == 0 )
		return 1;

	Number v( "2" );
	for( int i = 2; i<=power; ++i )
	{
		Number a( v );
		v.Add( a );
	}

    struct IntCharAdd {
        __int64 operator() ( __int64 s, const char& v ) { return s + ( v - '0' ); }
    };

    string vs( v.AsString() );
    return accumulate( vs.begin(), vs.end(), (__int64)0, IntCharAdd() );
}

BOOST_AUTO_TEST_CASE( TestEulerProject16 )
{
    BOOST_CHECK_EQUAL( 26, DigitSumOfTwoSquare( 15 ) );
//    BOOST_CHECK_EQUAL( 1366, DigitSumOfTwoSquare( 1000 ) );
}

int LetterCountSumFrom0neTo( int n )
{
    class NumericWord
    {
    public:
        static int Count( int v )
        {
            static int letters_0_19[] =
            {
                0,
                strlen("one"),
                strlen("two"),
                strlen("three"),
                strlen("four"),
                strlen("five"),
                strlen("six"),
                strlen("seven"),
                strlen("eight"),
                strlen("nine"),
                strlen("ten"),
                strlen("eleven"),
                strlen("twelve"),
                strlen("thirteen"),
                strlen("fourteen"),
                strlen("fifteen"),
                strlen("sixteen"),
                strlen("seventeen"),
                strlen("eighteen"),
                strlen("nineteen"),
            };

            static int letters_0_90[] =
            {
                0,
                0,
                strlen("twenty"),
                strlen("thirty"),
                strlen("forty"),
                strlen("fifty"),
                strlen("sixty"),
                strlen("seventy"),
                strlen("eighty"),
                strlen("ninety"),
            };

            int c1 = v / 20;
            if ( c1 == 0 )
            {
                return letters_0_19[ v % 20 ];
            }
            else if ( v < 100 )
            {
                return letters_0_90[ v / 10 ] + letters_0_19[ v % 10 ];
            }
            else if ( v < 1000 )
            {
                static int hundred = 7;
                static int and = 3;

                if( v%100 == 0 )
                {
                    return letters_0_19[ v / 100 ] + hundred;
                }

                return letters_0_19[ v / 100 ] + hundred + and + Count( v % 100 );
            }
            else if ( v == 1000 )
            {
                return 11;
            }

            return -1;
        }

        static int SummationOfCount( int s, int e )
        {
            int sum = 0;
            for( int i = s; i<=e; ++i ) { sum += Count(i); }
            return sum;
        }

        static int SummationOfCount( int n ) // FIX ME : This method is not working!
        {
            if( n <= 19 )
            {
                return SummationOfCount( 1, n );
            }
            else if ( n < 100 )
            {
                int sum = SummationOfCount( 1, 19 );
                int s_9 = SummationOfCount( 0, 9 );

                int i = 20;
                for( ; i< (n/10)*10; i+= 10 )
                {
                    sum += Count(i) * 10;
                    sum += s_9;
                }

                sum += Count(i) * ((n%10)+1);
                sum += SummationOfCount( 1, n % 10 );
                return sum;
            }
            else if ( n < 1000 )
            {
                static int hundred = 7;
                static int and = 3;
                int s99 = SummationOfCount( 99 );
                int i = 100, sum = 0;
                for( ; i < (n/100)*100; i+=100 )
                {
                    sum += hundred;
                    sum += ( Count( i / 100 ) + + hundred + and ) * 100;
                    sum += s99;
                }

                sum += hundred;
                sum += ( Count( i / 100 ) + hundred + and ) * ( n % 100 );
                sum += SummationOfCount( 1, n%100 );
                return sum;
            }
            else if ( n == 1000 )
            {
                static int one_thousand = 11;
                int s999 = SummationOfCount( 999 );
                return s999 + one_thousand;
            }

            return -1;
        }
    };

    return NumericWord::SummationOfCount( 1, n );
}

BOOST_AUTO_TEST_CASE( TestEulerProject17 )
{
    BOOST_CHECK_EQUAL( 19, LetterCountSumFrom0neTo( 5 ) );
//    BOOST_CHECK_EQUAL( 21124, LetterCountSumFrom0neTo( 1000 ) );
}

struct Place {
    int Row, Col;
    int Sum, UpperBound;
    Place() {};
    Place( int row, int col, int sum ) : Row(row), Col(col), Sum(sum)
    {
        UpperBound = sum + (s_RowCount - row - 1)*s_MaxValue;
    }

    bool operator() ( const Place& l, const Place& r )
    {
        if( l.UpperBound == r.UpperBound ) {
            if( l.Sum == r.Sum ) {
                if( l.Row == r.Row )
                    return l.Col > r.Col;
                return l.Row > r.Row;
            }
            return l.Sum > r.Sum;
        }
        return l.UpperBound > r.UpperBound;
    }

    static void Dump( const Place & p )
    {
        printf( "r=%d,c=%d,s=%d,u=%d\n", p.Row, p.Col, p.Sum, p.UpperBound );
    }

    static void SetUpperBoundVariable( int rowCount, int maxValue )
    {
        s_RowCount = rowCount;
        s_MaxValue = maxValue;
    }

    static int s_RowCount, s_MaxValue;
};

int Place::s_RowCount = 0;
int Place::s_MaxValue = 0;

int FindMaxTopToBottom( const vector< vector<int> >& triangle, int maxValue )
{
    typedef priority_queue< Place, vector<Place>, Place > PlaceQueue;

    int n = triangle.size();
    Place::SetUpperBoundVariable( n, maxValue );

    PlaceQueue q;
    q.push( Place( 0,0, triangle[0][0] ));
    int max = 0, iter = 0;

    while( q.size() > 0 )
    {
        Place t( q.top() );
        q.pop();
        Place::Dump( t );

        if( t.Row < n-1 )
        {
            if( t.UpperBound < max )
                continue;

            q.push( Place( t.Row+1, t.Col, t.Sum + triangle[t.Row+1][t.Col] ) );
            q.push( Place( t.Row+1, t.Col+1, t.Sum + triangle[t.Row+1][t.Col+1] ) );
        }
        else
        {
            if( t.Sum > max )
            {
                max = t.Sum;
            }
        }

        iter++;
    }

    printf( "max = %d, iter count = %d\n", max, iter );
    return max;
}

vector<int> ParseAsVector( const char *numbers )
{
    vector<int> v;
    const char *s = numbers;
    char *e = const_cast<char *>(numbers);
    while( *e != NULL )
    {
        int i = strtol( s, &e, 10  );
        if( i == 0 )
            break;

        v.push_back( i );
        s = e;
    }
    return v;
}

int FindMaxTopToBottomFromBottomToTop( vector< vector<int> >& t )
{
    for( size_t i=t.size()-1; i>0; --i )
    {
        for( size_t j=0; j<t[i].size()-1; ++j )
        {
            t[i-1][j] += max( t[i][j], t[i][j+1] );
        }
    }

    return t[0][0];
}

BOOST_AUTO_TEST_CASE( TestEulerProject18 )
{
    {
        vector< vector<int> > triangle;
        triangle.assign( 4, vector<int>() );
        triangle[0].swap( ParseAsVector( "3" ) );
        triangle[1].swap( ParseAsVector( "7 4" ) );
        triangle[2].swap( ParseAsVector( "2 4 6" ) );
        triangle[3].swap( ParseAsVector( "8 5 9 3" ) );

        int ret = FindMaxTopToBottom( triangle, 9 );
        BOOST_CHECK_EQUAL( 23, ret );
    }

    {
        vector< vector<int> > triangle;
        triangle.assign( 15, vector<int>() );
        triangle[0].swap( ParseAsVector( "75" ) );
        triangle[1].swap( ParseAsVector( "95 64" ) );
        triangle[2].swap( ParseAsVector( "17 47 82" ) );
        triangle[3].swap( ParseAsVector( "18 35 87 10" ) );
        triangle[4].swap( ParseAsVector( "20 04 82 47 65" ) );
        triangle[5].swap( ParseAsVector( "19 01 23 75 03 34" ) );
        triangle[6].swap( ParseAsVector( "88 02 77 73 07 63 67" ) );
        triangle[7].swap( ParseAsVector( "99 65 04 28 06 16 70 92" ) );
        triangle[8].swap( ParseAsVector( "41 41 26 56 83 40 80 70 33" ) );
        triangle[9].swap( ParseAsVector( "41 48 72 33 47 32 37 16 94 29" ) );
        triangle[10].swap( ParseAsVector( "53 71 44 65 25 43 91 52 97 51 14" ) );
        triangle[11].swap( ParseAsVector( "70 11 33 28 77 73 17 78 39 68 17 57" ) );
        triangle[12].swap( ParseAsVector( "91 71 52 38 17 14 91 43 58 50 27 29 48" ) );
        triangle[13].swap( ParseAsVector( "63 66 04 68 89 53 67 30 73 16 69 87 40 31" ) );
        triangle[14].swap( ParseAsVector( "04 62 98 27 23 09 70 98 73 93 38 53 60 04 23" ) );

//        BOOST_CHECK_EQUAL( 1074, FindMaxTopToBottom( triangle, 99 ) );
    }
}

BOOST_AUTO_TEST_CASE( TestEulerProject67 )
{
#if 0
    {
        vector< vector<int> > triangle;
        triangle.clear();

        {
            FILE *fp = fopen( "triangle.txt", "rt" );

            while( !feof( fp ) ) {
                char buf[1024];
                if( fgets( buf, 1024, fp ) == NULL )
                    break;

                triangle.push_back( ParseAsVector( buf ) );
            }

            fclose( fp );
        }

        int ret = FindMaxTopToBottomFromBottomToTop( triangle);
        BOOST_CHECK_EQUAL( 7273, ret );
    }
#endif
}


#define BOOST_TEST_MODULE variables
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "exprtest.hpp"

double const x = 1;
double const y = 2;

std::map<std::string, double> const symtab = {
    std::make_pair("x",  x),
    std::make_pair("y",  y),
    std::make_pair("e", -1),
};

SYMEXPRTEST(var1, "x+y" , symtab, x+y)
SYMEXPRTEST(var2, "x-y" , symtab, x-y)
SYMEXPRTEST(var3, "x*y" , symtab, x*y)
SYMEXPRTEST(var4, "x/y" , symtab, x/y);
SYMEXPRTEST(var5, "x%y" , symtab, std::fmod(x,y));
SYMEXPRTEST(var6, "x**y", symtab, std::pow(x,y));

// Constants have higher priority than variables of the same name
SYMEXPRTEST(var7, "e", symtab, boost::math::constants::e<double>())


#include <Eigen/Dense>

#include "benchmark.hpp"

#include <iostream>

int main() {
  for (size_t k = 10; k <= 1000; k+=10) {
    size_t m = k;
    double *A = create_random_sq_matrix(m);

    sleep(0.1);
    Eigen::MatrixXd A_;
    A_.resize(m, m);
    size_t index = 0;
    for (size_t i = 0; i < m; i++) {
      for (size_t j = 0; j < m; j++) {
        A_(i, j) = A[index];
      }
    }

    struct timespec t = tic();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A_, Eigen::ComputeFullV | Eigen::ComputeFullU);
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::VectorXd d = svd.singularValues();
    Eigen::MatrixXd S = d.asDiagonal();
    Eigen::MatrixXd V = svd.matrixV();
    printf("matrix_size: %ld\tEigen SVD: %fs\n", A_.rows(), toc(&t));

    bool retval = A_.isApprox(U * S * V.transpose());
    if (retval == false) {
      exit(-1);
    }

    free(A);
  }

  return 0;
}