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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
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 or http://www.mitk.org for details.

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

#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <itkEvaluateDirectionImagesFilter.h>
#include <metaCommand.h>
#include "mitkCommandLineParser.h"
#include <itkTractsToVectorImageFilter.h>
#include <usAny.h>
#include <itkImageFileWriter.h>
#include <mitkIOUtil.h>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>
#include <itksys/SystemTools.hxx>
#include <mitkCoreObjectFactory.h>

#define _USE_MATH_DEFINES
#include <math.h>

/*!
\brief Calculate angular error of a tractogram with respect to the input reference directions.
*/
int main(int argc, char* argv[])
{
    mitkCommandLineParser parser;

    parser.setTitle("Local Directional Fiber Plausibility");
    parser.setCategory("Fiber Tracking and Processing Methods");
    parser.setDescription("Calculate angular error of a tractogram with respect to the input reference directions.");
    parser.setContributor("MBI");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input:", "input tractogram (.fib, vtk ascii file format)", us::Any(), false);
    parser.addArgument("reference", "r", mitkCommandLineParser::StringList, "Reference images:", "reference direction images", us::Any(), false);
    parser.addArgument("out", "o", mitkCommandLineParser::OutputDirectory, "Output:", "output root", us::Any(), false);
    parser.addArgument("mask", "m", mitkCommandLineParser::StringList, "Masks:", "mask images");
    parser.addArgument("athresh", "a", mitkCommandLineParser::Float, "Angular threshold:", "angular threshold in degrees. closer fiber directions are regarded as one direction and clustered together.", 25, true);
    parser.addArgument("sthresh", "s", mitkCommandLineParser::Float, "Size threshold:", "Relative peak size threshold per voxel.", 0.0, true);
    parser.addArgument("maxdirs", "md", mitkCommandLineParser::Int, "Max. Clusters:", "Maximum number of fiber clusters.", 0, true);
    parser.addArgument("verbose", "v", mitkCommandLineParser::Bool, "Verbose:", "output optional and intermediate calculation results");
    parser.addArgument("ignore", "n", mitkCommandLineParser::Bool, "Ignore:", "don't increase error for missing or too many directions");
    parser.addArgument("empty", "e", mitkCommandLineParser::Bool, "Empty Voxels:", "don't increase error for empty voxels");
    parser.addArgument("fileID", "id", mitkCommandLineParser::String, "ID:", "optional ID field");

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    mitkCommandLineParser::StringContainerType referenceImages = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["reference"]);
    mitkCommandLineParser::StringContainerType maskImages;
    if (parsedArgs.count("mask"))
        maskImages = us::any_cast<mitkCommandLineParser::StringContainerType>(parsedArgs["mask"]);

    string fibFile = us::any_cast<string>(parsedArgs["input"]);

    float angularThreshold = 25;
    if (parsedArgs.count("athresh"))
        angularThreshold = us::any_cast<float>(parsedArgs["athresh"]);

    float sizeThreshold = 0;
    if (parsedArgs.count("sthresh"))
        sizeThreshold = us::any_cast<float>(parsedArgs["sthresh"]);

    int maxDirs = 0;
    if (parsedArgs.count("maxdirs"))
        maxDirs = us::any_cast<int>(parsedArgs["maxdirs"]);

    string outRoot = us::any_cast<string>(parsedArgs["out"]);

    bool verbose = false;
    if (parsedArgs.count("verbose"))
        verbose = us::any_cast<bool>(parsedArgs["verbose"]);

    bool ignoreMissing = false;
    if (parsedArgs.count("ignore"))
        ignoreMissing = us::any_cast<bool>(parsedArgs["ignore"]);

    bool ignoreEmpty = false;
    if (parsedArgs.count("empty"))
        ignoreEmpty = us::any_cast<bool>(parsedArgs["empty"]);

    string fileID = "";
    if (parsedArgs.count("fileID"))
        fileID = us::any_cast<string>(parsedArgs["fileID"]);

    try
    {
        typedef itk::Image<unsigned char, 3>                                    ItkUcharImgType;
        typedef itk::Image< itk::Vector< float, 3>, 3 >                         ItkDirectionImage3DType;
        typedef itk::VectorContainer< unsigned int, ItkDirectionImage3DType::Pointer >   ItkDirectionImageContainerType;
        typedef itk::EvaluateDirectionImagesFilter< float >                     EvaluationFilterType;

        // load fiber bundle
        mitk::FiberBundle::Pointer inputTractogram = dynamic_cast<mitk::FiberBundle*>(mitk::IOUtil::LoadDataNode(fibFile)->GetData());

        // load reference directions
        ItkDirectionImageContainerType::Pointer referenceImageContainer = ItkDirectionImageContainerType::New();
        for (unsigned int i=0; i<referenceImages.size(); i++)
        {
            try
            {
                mitk::Image::Pointer img = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(referenceImages.at(i))->GetData());
                typedef mitk::ImageToItk< ItkDirectionImage3DType > CasterType;
                CasterType::Pointer caster = CasterType::New();
                caster->SetInput(img);
                caster->Update();
                ItkDirectionImage3DType::Pointer itkImg = caster->GetOutput();
                referenceImageContainer->InsertElement(referenceImageContainer->Size(),itkImg);
            }
            catch(...){ std::cout << "could not load: " << referenceImages.at(i); }
        }

        ItkUcharImgType::Pointer itkMaskImage = ItkUcharImgType::New();
        ItkDirectionImage3DType::Pointer dirImg = referenceImageContainer->GetElement(0);
        itkMaskImage->SetSpacing( dirImg->GetSpacing() );
        itkMaskImage->SetOrigin( dirImg->GetOrigin() );
        itkMaskImage->SetDirection( dirImg->GetDirection() );
        itkMaskImage->SetLargestPossibleRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->SetBufferedRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->SetRequestedRegion( dirImg->GetLargestPossibleRegion() );
        itkMaskImage->Allocate();
        itkMaskImage->FillBuffer(1);

        // extract directions from fiber bundle
        itk::TractsToVectorImageFilter<float>::Pointer fOdfFilter = itk::TractsToVectorImageFilter<float>::New();
        fOdfFilter->SetFiberBundle(inputTractogram);
        fOdfFilter->SetMaskImage(itkMaskImage);
        fOdfFilter->SetAngularThreshold(cos(angularThreshold*M_PI/180));
        fOdfFilter->SetNormalizeVectors(true);
        fOdfFilter->SetUseWorkingCopy(false);
        fOdfFilter->SetSizeThreshold(sizeThreshold);
        fOdfFilter->SetMaxNumDirections(maxDirs);
        fOdfFilter->Update();
        ItkDirectionImageContainerType::Pointer directionImageContainer = fOdfFilter->GetDirectionImageContainer();

        if (verbose)
        {
            // write vector field
            mitk::FiberBundle::Pointer directions = fOdfFilter->GetOutputFiberBundle();

            string outfilename = outRoot;
            outfilename.append("_VECTOR_FIELD.fib");

            mitk::IOUtil::SaveBaseData(directions.GetPointer(), outfilename );

            // write direction images
            for (unsigned int i=0; i<directionImageContainer->Size(); i++)
            {
                itk::TractsToVectorImageFilter<float>::ItkDirectionImageType::Pointer itkImg = directionImageContainer->GetElement(i);
                typedef itk::ImageFileWriter< itk::TractsToVectorImageFilter<float>::ItkDirectionImageType > WriterType;
                WriterType::Pointer writer = WriterType::New();

                string outfilename = outRoot;
                outfilename.append("_DIRECTION_");
                outfilename.append(boost::lexical_cast<string>(i));
                outfilename.append(".nrrd");

                writer->SetFileName(outfilename.c_str());
                writer->SetInput(itkImg);
                writer->Update();
            }

            // write num direction image
            {
                ItkUcharImgType::Pointer numDirImage = fOdfFilter->GetNumDirectionsImage();
                typedef itk::ImageFileWriter< ItkUcharImgType > WriterType;
                WriterType::Pointer writer = WriterType::New();

                string outfilename = outRoot;
                outfilename.append("_NUM_DIRECTIONS.nrrd");

                writer->SetFileName(outfilename.c_str());
                writer->SetInput(numDirImage);
                writer->Update();
            }
        }

        string logFile = outRoot;
        logFile.append("_ANGULAR_ERROR.csv");
        ofstream file;
        file.open (logFile.c_str());

        if (maskImages.size()>0)
        {
            for (unsigned int i=0; i<maskImages.size(); i++)
            {
                mitk::Image::Pointer mitkMaskImage = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(maskImages.at(i))->GetData());
                mitk::CastToItkImage(mitkMaskImage, itkMaskImage);

                // evaluate directions
                EvaluationFilterType::Pointer evaluationFilter = EvaluationFilterType::New();
                evaluationFilter->SetImageSet(directionImageContainer);
                evaluationFilter->SetReferenceImageSet(referenceImageContainer);
                evaluationFilter->SetMaskImage(itkMaskImage);
                evaluationFilter->SetIgnoreMissingDirections(ignoreMissing);
                evaluationFilter->SetIgnoreEmptyVoxels(ignoreEmpty);
                evaluationFilter->Update();

                if (verbose)
                {
                    EvaluationFilterType::OutputImageType::Pointer angularErrorImage = evaluationFilter->GetOutput(0);
                    typedef itk::ImageFileWriter< EvaluationFilterType::OutputImageType > WriterType;
                    WriterType::Pointer writer = WriterType::New();

                    string outfilename = outRoot;
                    outfilename.append("_ERROR_IMAGE.nrrd");

                    writer->SetFileName(outfilename.c_str());
                    writer->SetInput(angularErrorImage);
                    writer->Update();
                }

                string maskFileName = itksys::SystemTools::GetFilenameWithoutExtension(maskImages.at(i));
                unsigned found = maskFileName.find_last_of("_");

                string sens = itksys::SystemTools::GetFilenameWithoutLastExtension(fibFile);
                if (!fileID.empty())
                    sens = fileID;
                sens.append(",");

                sens.append(maskFileName.substr(found+1));
                sens.append(",");

                sens.append(boost::lexical_cast<string>(evaluationFilter->GetMeanAngularError()));
                sens.append(",");

                sens.append(boost::lexical_cast<string>(evaluationFilter->GetMedianAngularError()));
                sens.append(",");

                sens.append(boost::lexical_cast<string>(evaluationFilter->GetMaxAngularError()));
                sens.append(",");

                sens.append(boost::lexical_cast<string>(evaluationFilter->GetMinAngularError()));
                sens.append(",");

                sens.append(boost::lexical_cast<string>(std::sqrt(evaluationFilter->GetVarAngularError())));
                sens.append(";\n");
                file << sens;
            }
        }
        else
        {
            // evaluate directions
            EvaluationFilterType::Pointer evaluationFilter = EvaluationFilterType::New();
            evaluationFilter->SetImageSet(directionImageContainer);
            evaluationFilter->SetReferenceImageSet(referenceImageContainer);
            evaluationFilter->SetMaskImage(itkMaskImage);
            evaluationFilter->SetIgnoreMissingDirections(ignoreMissing);
            evaluationFilter->SetIgnoreEmptyVoxels(ignoreEmpty);
            evaluationFilter->Update();

            if (verbose)
            {
                EvaluationFilterType::OutputImageType::Pointer angularErrorImage = evaluationFilter->GetOutput(0);
                typedef itk::ImageFileWriter< EvaluationFilterType::OutputImageType > WriterType;
                WriterType::Pointer writer = WriterType::New();

                string outfilename = outRoot;
                outfilename.append("_ERROR_IMAGE.nrrd");

                writer->SetFileName(outfilename.c_str());
                writer->SetInput(angularErrorImage);
                writer->Update();
            }

            string sens = itksys::SystemTools::GetFilenameWithoutLastExtension(fibFile);
            if (!fileID.empty())
                sens = fileID;
            sens.append(",");

            sens.append(boost::lexical_cast<string>(evaluationFilter->GetMeanAngularError()));
            sens.append(",");

            sens.append(boost::lexical_cast<string>(evaluationFilter->GetMedianAngularError()));
            sens.append(",");

            sens.append(boost::lexical_cast<string>(evaluationFilter->GetMaxAngularError()));
            sens.append(",");

            sens.append(boost::lexical_cast<string>(evaluationFilter->GetMinAngularError()));
            sens.append(",");

            sens.append(boost::lexical_cast<string>(std::sqrt(evaluationFilter->GetVarAngularError())));
            sens.append(";\n");
            file << sens;
        }
        file.close();
    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
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 or http://www.mitk.org for details.

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

#include <itkImageFileWriter.h>
#include <itkResampleImageFilter.h>
#include <itkFiniteDiffOdfMaximaExtractionFilter.h>

#include <mitkImage.h>
#include <mitkQBallImage.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <mitkTensorImage.h>

#include <mitkCoreObjectFactory.h>
#include "mitkCommandLineParser.h"
#include <itkShCoefficientImageImporter.h>
#include <itkFlipImageFilter.h>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

#include <mitkIOUtil.h>

using namespace std;

template<int shOrder>
int StartPeakExtraction(int argc, char* argv[])
{
    mitkCommandLineParser parser;
    parser.setArgumentPrefix("--", "-");
    parser.addArgument("image", "i", mitkCommandLineParser::InputFile, "Input image", "sh coefficient image", us::Any(), false);
    parser.addArgument("outroot", "o", mitkCommandLineParser::OutputDirectory, "Output directory", "output root", us::Any(), false);
    parser.addArgument("mask", "m", mitkCommandLineParser::InputFile, "Mask", "mask image");
    parser.addArgument("normalization", "n", mitkCommandLineParser::Int, "Normalization", "0=no norm, 1=max norm, 2=single vec norm", 1, true);
    parser.addArgument("numpeaks", "p", mitkCommandLineParser::Int, "Max. number of peaks", "maximum number of extracted peaks", 2, true);
    parser.addArgument("peakthres", "r", mitkCommandLineParser::Float, "Peak threshold", "peak threshold relative to largest peak", 0.4, true);
    parser.addArgument("abspeakthres", "a", mitkCommandLineParser::Float, "Absolute peak threshold", "absolute peak threshold weighted with local GFA value", 0.06, true);
    parser.addArgument("shConvention", "s", mitkCommandLineParser::String, "Use specified SH-basis", "use specified SH-basis (MITK, FSL, MRtrix)", string("MITK"), true);
    parser.addArgument("noFlip", "f", mitkCommandLineParser::Bool, "No flip", "do not flip input image to match MITK coordinate convention");
    parser.addArgument("clusterThres", "c", mitkCommandLineParser::Float, "Clustering threshold", "directions closer together than the specified angular threshold will be clustered (in rad)", 0.9);
    parser.addArgument("flipX", "fx", mitkCommandLineParser::Bool, "Flip X", "Flip peaks in x direction");
    parser.addArgument("flipY", "fy", mitkCommandLineParser::Bool, "Flip Y", "Flip peaks in y direction");
    parser.addArgument("flipZ", "fz", mitkCommandLineParser::Bool, "Flip Z", "Flip peaks in z direction");

    parser.setCategory("Preprocessing Tools");
    parser.setTitle("Peak Extraction");
    parser.setDescription("");
    parser.setContributor("MBI");

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    // mandatory arguments
    string imageName = us::any_cast<string>(parsedArgs["image"]);
    string outRoot = us::any_cast<string>(parsedArgs["outroot"]);

    // optional arguments
    string maskImageName("");
    if (parsedArgs.count("mask"))
        maskImageName = us::any_cast<string>(parsedArgs["mask"]);

    int normalization = 1;
    if (parsedArgs.count("normalization"))
        normalization = us::any_cast<int>(parsedArgs["normalization"]);

    int numPeaks = 2;
    if (parsedArgs.count("numpeaks"))
        numPeaks = us::any_cast<int>(parsedArgs["numpeaks"]);

    float peakThres = 0.4;
    if (parsedArgs.count("peakthres"))
        peakThres = us::any_cast<float>(parsedArgs["peakthres"]);

    float absPeakThres = 0.06;
    if (parsedArgs.count("abspeakthres"))
        absPeakThres = us::any_cast<float>(parsedArgs["abspeakthres"]);

    float clusterThres = 0.9;
    if (parsedArgs.count("clusterThres"))
        clusterThres = us::any_cast<float>(parsedArgs["clusterThres"]);

    bool noFlip = false;
    if (parsedArgs.count("noFlip"))
        noFlip = us::any_cast<bool>(parsedArgs["noFlip"]);

    bool flipX = false;
    if (parsedArgs.count("flipX"))
        flipX = us::any_cast<bool>(parsedArgs["flipX"]);

    bool flipY = false;
    if (parsedArgs.count("flipY"))
        flipY = us::any_cast<bool>(parsedArgs["flipY"]);

    bool flipZ = false;
    if (parsedArgs.count("flipZ"))
        flipZ = us::any_cast<bool>(parsedArgs["flipZ"]);

    std::cout << "image: " << imageName;
    std::cout << "outroot: " << outRoot;
    if (!maskImageName.empty())
        std::cout << "mask: " << maskImageName;
    else
        std::cout << "no mask image selected";
    std::cout << "numpeaks: " << numPeaks;
    std::cout << "peakthres: " << peakThres;
    std::cout << "abspeakthres: " << absPeakThres;
    std::cout << "shOrder: " << shOrder;

    try
    {
        mitk::Image::Pointer image = mitk::IOUtil::LoadImage(imageName);
        mitk::Image::Pointer mask = mitk::IOUtil::LoadImage(maskImageName);

        typedef itk::Image<unsigned char, 3>  ItkUcharImgType;
        typedef itk::FiniteDiffOdfMaximaExtractionFilter< float, shOrder, 20242 > MaximaExtractionFilterType;
        typename MaximaExtractionFilterType::Pointer filter = MaximaExtractionFilterType::New();

        int toolkitConvention = 0;

        if (parsedArgs.count("shConvention"))
        {
            string convention = us::any_cast<string>(parsedArgs["shConvention"]).c_str();
            if ( boost::algorithm::equals(convention, "FSL") )
            {
                toolkitConvention = 1;
                std::cout << "Using FSL SH-basis";
            }
            else if ( boost::algorithm::equals(convention, "MRtrix") )
            {
                toolkitConvention = 2;
                std::cout << "Using MRtrix SH-basis";
            }
            else
                std::cout << "Using MITK SH-basis";
        }
        else
            std::cout << "Using MITK SH-basis";

        ItkUcharImgType::Pointer itkMaskImage = NULL;
        if (mask.IsNotNull())
        {
            try{
                itkMaskImage = ItkUcharImgType::New();
                mitk::CastToItkImage(mask, itkMaskImage);
                filter->SetMaskImage(itkMaskImage);
            }
            catch(...)
            {

            }
        }

        if (toolkitConvention>0)
        {
            std::cout << "Converting coefficient image to MITK format";
            typedef itk::ShCoefficientImageImporter< float, shOrder > ConverterType;
            typedef mitk::ImageToItk< itk::Image< float, 4 > > CasterType;
            CasterType::Pointer caster = CasterType::New();
            caster->SetInput(image);
            caster->Update();
            itk::Image< float, 4 >::Pointer itkImage = caster->GetOutput();

            typename ConverterType::Pointer converter = ConverterType::New();

            if (noFlip)
            {
                converter->SetInputImage(itkImage);
            }
            else
            {
                std::cout << "Flipping image";
                itk::FixedArray<bool, 4> flipAxes;
                flipAxes[0] = true;
                flipAxes[1] = true;
                flipAxes[2] = false;
                flipAxes[3] = false;
                itk::FlipImageFilter< itk::Image< float, 4 > >::Pointer flipper = itk::FlipImageFilter< itk::Image< float, 4 > >::New();
                flipper->SetInput(itkImage);
                flipper->SetFlipAxes(flipAxes);
                flipper->Update();
                itk::Image< float, 4 >::Pointer flipped = flipper->GetOutput();
                itk::Matrix< double,4,4 > m = itkImage->GetDirection(); m[0][0] *= -1; m[1][1] *= -1;
                flipped->SetDirection(m);

                itk::Point< float, 4 > o = itkImage->GetOrigin();
                o[0] -= (flipped->GetLargestPossibleRegion().GetSize(0)-1);
                o[1] -= (flipped->GetLargestPossibleRegion().GetSize(1)-1);
                flipped->SetOrigin(o);
                converter->SetInputImage(flipped);
            }

            std::cout << "Starting conversion";
            switch (toolkitConvention)
            {
            case 1:
                converter->SetToolkit(ConverterType::FSL);
                filter->SetToolkit(MaximaExtractionFilterType::FSL);
                break;
            case 2:
                converter->SetToolkit(ConverterType::MRTRIX);
                filter->SetToolkit(MaximaExtractionFilterType::MRTRIX);
                break;
            default:
                converter->SetToolkit(ConverterType::FSL);
                filter->SetToolkit(MaximaExtractionFilterType::FSL);
                break;
            }
            converter->GenerateData();
            filter->SetInput(converter->GetCoefficientImage());
        }
        else
        {
            try{
                typedef mitk::ImageToItk< typename MaximaExtractionFilterType::CoefficientImageType > CasterType;
                typename CasterType::Pointer caster = CasterType::New();
                caster->SetInput(image);
                caster->Update();
                filter->SetInput(caster->GetOutput());
            }
            catch(...)
            {
                std::cout << "wrong image type";
                return EXIT_FAILURE;
            }
        }

        filter->SetMaxNumPeaks(numPeaks);
        filter->SetPeakThreshold(peakThres);
        filter->SetAbsolutePeakThreshold(absPeakThres);
        filter->SetAngularThreshold(1);
        filter->SetClusteringThreshold(clusterThres);
        filter->SetFlipX(flipX);
        filter->SetFlipY(flipY);
        filter->SetFlipZ(flipZ);

        switch (normalization)
        {
        case 0:
            filter->SetNormalizationMethod(MaximaExtractionFilterType::NO_NORM);
            break;
        case 1:
            filter->SetNormalizationMethod(MaximaExtractionFilterType::MAX_VEC_NORM);
            break;
        case 2:
            filter->SetNormalizationMethod(MaximaExtractionFilterType::SINGLE_VEC_NORM);
            break;
        }

        std::cout << "Starting extraction";
        filter->Update();

        // write direction images
        {
            typedef typename MaximaExtractionFilterType::ItkDirectionImageContainer ItkDirectionImageContainer;
            typename ItkDirectionImageContainer::Pointer container = filter->GetDirectionImageContainer();
            for (unsigned int i=0; i<container->Size(); i++)
            {
                typename MaximaExtractionFilterType::ItkDirectionImage::Pointer itkImg = container->GetElement(i);

                if (itkMaskImage.IsNotNull())
                {
                    itkImg->SetDirection(itkMaskImage->GetDirection());
                    itkImg->SetOrigin(itkMaskImage->GetOrigin());
                }

                string outfilename = outRoot;
                outfilename.append("_DIRECTION_");
                outfilename.append(boost::lexical_cast<string>(i));
                outfilename.append(".nrrd");

                typedef itk::ImageFileWriter< typename MaximaExtractionFilterType::ItkDirectionImage > WriterType;
                typename WriterType::Pointer writer = WriterType::New();
                writer->SetFileName(outfilename);
                writer->SetInput(itkImg);
                writer->Update();
            }
        }

        // write num directions image
        {
            ItkUcharImgType::Pointer numDirImage = filter->GetNumDirectionsImage();

            if (itkMaskImage.IsNotNull())
            {
                numDirImage->SetDirection(itkMaskImage->GetDirection());
                numDirImage->SetOrigin(itkMaskImage->GetOrigin());
            }

            string outfilename = outRoot.c_str();
            outfilename.append("_NUM_DIRECTIONS.nrrd");
            typedef itk::ImageFileWriter< ItkUcharImgType > WriterType;
            WriterType::Pointer writer = WriterType::New();
            writer->SetFileName(outfilename);
            writer->SetInput(numDirImage);
            writer->Update();
        }

        // write vector field
        {
            mitk::FiberBundle::Pointer directions = filter->GetOutputFiberBundle();

            string outfilename = outRoot.c_str();
            outfilename.append("_VECTOR_FIELD.fib");
            mitk::IOUtil::Save(directions.GetPointer(),outfilename.c_str());
        }
    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

/*!
\brief Extract maxima in the input spherical harmonics image.
*/
int main(int argc, char* argv[])
{
    mitkCommandLineParser parser;
    parser.setArgumentPrefix("--", "-");
    parser.addArgument("image", "i", mitkCommandLineParser::InputFile, "Input image", "sh coefficient image", us::Any(), false);
    parser.addArgument("shOrder", "sh", mitkCommandLineParser::Int, "Spherical harmonics order", "spherical harmonics order");
    parser.addArgument("outroot", "o", mitkCommandLineParser::OutputDirectory, "Output directory", "output root", us::Any(), false);
    parser.addArgument("mask", "m", mitkCommandLineParser::InputFile, "Mask", "mask image");
    parser.addArgument("normalization", "n", mitkCommandLineParser::Int, "Normalization", "0=no norm, 1=max norm, 2=single vec norm", 1, true);
    parser.addArgument("numpeaks", "p", mitkCommandLineParser::Int, "Max. number of peaks", "maximum number of extracted peaks", 2, true);
    parser.addArgument("peakthres", "r", mitkCommandLineParser::Float, "Peak threshold", "peak threshold relative to largest peak", 0.4, true);
    parser.addArgument("abspeakthres", "a", mitkCommandLineParser::Float, "Absolute peak threshold", "absolute peak threshold weighted with local GFA value", 0.06, true);
    parser.addArgument("shConvention", "s", mitkCommandLineParser::String, "Use specified SH-basis", "use specified SH-basis (MITK, FSL, MRtrix)", string("MITK"), true);
    parser.addArgument("noFlip", "f", mitkCommandLineParser::Bool, "No flip", "do not flip input image to match MITK coordinate convention");

    parser.setCategory("Preprocessing Tools");
    parser.setTitle("Peak Extraction");
    parser.setDescription("Extract maxima in the input spherical harmonics image.");
    parser.setContributor("MBI");

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    int shOrder = -1;
    if (parsedArgs.count("shOrder"))
        shOrder = us::any_cast<int>(parsedArgs["shOrder"]);

    switch (shOrder)
    {
    case 4:
        return StartPeakExtraction<4>(argc, argv);
    case 6:
        return StartPeakExtraction<6>(argc, argv);
    case 8:
        return StartPeakExtraction<8>(argc, argv);
    case 10:
        return StartPeakExtraction<10>(argc, argv);
    case 12:
        return StartPeakExtraction<12>(argc, argv);
    }
    return EXIT_FAILURE;
}


/**
 * Demo program for simulation library
 * A virtual camera generates simulated point clouds
 * No visual output, point clouds saved to file
 *
 * three different demo modes:
 * 0 - static camera, 100 poses
 * 1 - circular camera flying around the scene, 16 poses
 * 2 - camera translates between 2 poses using slerp, 20 poses
 * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif
#include <GL/glew.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include "pcl/common/common.h"
#include "pcl/common/transforms.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>

// define the following in order to eliminate the deprecated headers warning
#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>

#include "pcl/simulation/camera.h"
#include "pcl/simulation/model.h"
#include "pcl/simulation/scene.h"
#include "pcl/simulation/range_likelihood.h"

#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

// RangeImage:
#include <pcl/range_image/range_image_planar.h>

// Pop-up viewer
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

// Writing PNG files:
#include <opencv2/opencv.hpp>

using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::io;
using namespace pcl::simulation;

using namespace std;

uint16_t t_gamma[2048];

Scene::Ptr scene_;
Camera::Ptr camera_;
RangeLikelihood::Ptr range_likelihood_;

int window_width_;
int window_height_;
bool paused_;
bool write_file_;

void printHelp(int argc, char **argv) {
    print_error("Syntax is: %s <mode 1,2 or 3> <filename>\n", argv[0]);
    print_info("acceptable filenames include vtk, obj and ply. ply can support "
               "colour\n");
}

void write_score_image(const float *score_buffer) {
    int npixels =
        range_likelihood_->getWidth() * range_likelihood_->getHeight();
    uint8_t *score_img = new uint8_t[npixels * 3];

    float min_score = score_buffer[0];
    float max_score = score_buffer[0];
    for (int i = 1; i < npixels; i++) {
        if (score_buffer[i] < min_score)
            min_score = score_buffer[i];
        if (score_buffer[i] > max_score)
            max_score = score_buffer[i];
    }

    for (int y = 0; y < 480; ++y) {
        for (int x = 0; x < 640; ++x) {
            int i = y * 640 + x;
            int i_in = (480 - 1 - y) * 640 + x; // flip up

            float d =
                (score_buffer[i_in] - min_score) / (max_score - min_score);
            score_img[3 * i + 0] = 0;
            score_img[3 * i + 1] = d * 255;
            score_img[3 * i + 2] = 0;
        }
    }

    // Write to file:
    IplImage *cv_ipl = cvCreateImage(cvSize(640, 480), 8, 3);
    cv::Mat cv_mat(cv_ipl);
    cv_mat.data = score_img;
    cv::imwrite("score_image.png", cv_mat);

    delete[] score_img;
}

void write_depth_image(const float *depth_buffer) {
    int npixels =
        range_likelihood_->getWidth() * range_likelihood_->getHeight();
    uint8_t *depth_img = new uint8_t[npixels * 3];

    float min_depth = depth_buffer[0];
    float max_depth = depth_buffer[0];
    for (int i = 1; i < npixels; i++) {
        if (depth_buffer[i] < min_depth)
            min_depth = depth_buffer[i];
        if (depth_buffer[i] > max_depth)
            max_depth = depth_buffer[i];
    }

    for (int y = 0; y < 480; ++y) {
        for (int x = 0; x < 640; ++x) {
            int i = y * 640 + x;
            int i_in = (480 - 1 - y) * 640 + x; // flip up down

            float zn = 0.7;
            float zf = 20.0;
            float d = depth_buffer[i_in];
            float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
            float b = 0.075;
            float f = 580.0;
            uint16_t kd = static_cast<uint16_t>(1090 - b * f / z * 8);
            if (kd < 0)
                kd = 0;
            else if (kd > 2047)
                kd = 2047;

            int pval = t_gamma[kd];
            int lb = pval & 0xff;
            switch (pval >> 8) {
            case 0:
                depth_img[3 * i + 2] = 255;
                depth_img[3 * i + 1] = 255 - lb;
                depth_img[3 * i + 0] = 255 - lb;
                break;
            case 1:
                depth_img[3 * i + 2] = 255;
                depth_img[3 * i + 1] = lb;
                depth_img[3 * i + 0] = 0;
                break;
            case 2:
                depth_img[3 * i + 2] = 255 - lb;
                depth_img[3 * i + 1] = 255;
                depth_img[3 * i + 0] = 0;
                break;
            case 3:
                depth_img[3 * i + 2] = 0;
                depth_img[3 * i + 1] = 255;
                depth_img[3 * i + 0] = lb;
                break;
            case 4:
                depth_img[3 * i + 2] = 0;
                depth_img[3 * i + 1] = 255 - lb;
                depth_img[3 * i + 0] = 255;
                break;
            case 5:
                depth_img[3 * i + 2] = 0;
                depth_img[3 * i + 1] = 0;
                depth_img[3 * i + 0] = 255 - lb;
                break;
            default:
                depth_img[3 * i + 2] = 0;
                depth_img[3 * i + 1] = 0;
                depth_img[3 * i + 0] = 0;
                break;
            }
        }
    }

    // Write to file:
    IplImage *cv_ipl = cvCreateImage(cvSize(640, 480), 8, 3);
    cv::Mat cv_mat(cv_ipl);
    cv_mat.data = depth_img;
    cv::imwrite("depth_image.png", cv_mat);

    delete[] depth_img;
}

void write_rgb_image(const uint8_t *rgb_buffer) {
    int npixels =
        range_likelihood_->getWidth() * range_likelihood_->getHeight();
    uint8_t *rgb_img = new uint8_t[npixels * 3];

    for (int y = 0; y < 480; ++y) {
        for (int x = 0; x < 640; ++x) {
            int px = y * 640 + x;
            int px_in = (480 - 1 - y) * 640 + x; // flip up down
            rgb_img[3 * (px) + 0] = rgb_buffer[3 * px_in + 0];
            rgb_img[3 * (px) + 1] = rgb_buffer[3 * px_in + 1];
            rgb_img[3 * (px) + 2] = rgb_buffer[3 * px_in + 2];
        }
    }

    // Write to file:
    IplImage *cv_ipl = cvCreateImage(cvSize(640, 480), 8, 3);
    cv::Mat cv_mat(cv_ipl);
    cv_mat.data = rgb_img;
    cv::imwrite("rgb_image.png", cv_mat);

    delete[] rgb_img;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) {
    // Snippet taken from PCLVisualizer tutorial:
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
        new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(
        cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    // viewer->addModelFromPLYFile("/home/mfallon/projects/kmcl/kmcl/models/table_scene/meta_model.ply");

    return (viewer);
}

void capture(Eigen::Isometry3d pose_in, string point_cloud_fname) {
    // No reference image - but this is kept for compatability with
    // range_test_v2:
    float *reference = new float[range_likelihood_->getRowHeight() *
                                 range_likelihood_->getColWidth()];
    const float *depth_buffer = range_likelihood_->getDepthBuffer();
    // Copy one image from our last as a reference.
    for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
        for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
            reference[n++] =
                depth_buffer[i * range_likelihood_->getWidth() + j];
        }
    }

    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
        poses;
    std::vector<float> scores;
    int n = 1;
    poses.push_back(pose_in);
    range_likelihood_->computeLikelihoods(reference, poses, scores);
    std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
              << camera_->getZ() << " " << camera_->getRoll() << " "
              << camera_->getPitch() << " " << camera_->getYaw() << std::endl;

    delete[] reference;

    // Benchmark Values for sim_terminal_demo ( march 27 2012): >>> used
    // PolygonMeshModel<< 27840 triangle faces 13670 vertices

    // 45.00Hz: simuation only
    //  1.28Hz: simuation, addNoise?    , getPointCloud, writeASCII
    // 33.33Hz: simuation, getPointCloud
    // 23.81Hz: simuation, getPointCloud, writeBinary
    // 14.28Hz: simuation, addNoise, getPointCloud, writeBinary
    // MODULE        TIME      FRACTION
    // simulation     0.02222   31%
    // addNoise      0.03	     41%
    // getPointCloud 0.008     11%
    // writeBinary   0.012     16%
    // total	   0.07222

    // Update: march 29: >>> using TriangleMeshModel now <<<
    // 57.00Hz: simulation only
    // 30.61Hz: simulation, getPointCloud
    // 40.00Hz: simulation, getPointCloud, writeBinary (on average
    // 28.50Hz: simulation, addNoise, getPointCloud, writeBinary

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(
        new pcl::PointCloud<pcl::PointXYZRGB>);
    bool write_cloud = true;
    bool demo_other_stuff = false;

    if (write_cloud) {
        // Read Color Buffer from the GPU before creating PointCloud:
        // By default the buffers are not read back from the GPU
        range_likelihood_->getColorBuffer();
        range_likelihood_->getDepthBuffer();

        // Add noise directly to the CPU depth buffer
        range_likelihood_->addNoise();

        // Optional argument to save point cloud in global frame:
        // Save camera relative:
        // range_likelihood_->getPointCloud(pc_out);
        // Save in global frame - applying the camera frame:
        // range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
        // Save in local frame
        range_likelihood_->getPointCloud(pc_out, false, camera_->getPose());
        // TODO: what to do when there are more than one simulated view?
        std::cout << pc_out->points.size() << " points written to file\n";

        pcl::PCDWriter writer;
        // writer.write (point_cloud_fname, *pc_out,	false);  /// ASCII
        writer.writeBinary(point_cloud_fname, *pc_out);
        // cout << "finished writing file\n";
    }
    if (demo_other_stuff && write_cloud) {
        write_score_image(range_likelihood_->getScoreBuffer());
        write_rgb_image(range_likelihood_->getColorBuffer());
        write_depth_image(range_likelihood_->getDepthBuffer());

        // Demo interacton with RangeImage:
        pcl::RangeImagePlanar rangeImage;
        range_likelihood_->getRangeImagePlanar(rangeImage);

        // display viewer: (currently seqfaults on exit of viewer)
        if (1 == 0) {
            boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
            viewer = simpleVis(pc_out);

            while (!viewer->wasStopped()) {
                viewer->spinOnce(100);
                boost::this_thread::sleep(
                    boost::posix_time::microseconds(100000));
            }
        }
    }
}

// Read in a 3D model
void load_PolygonMesh_model(char *polygon_file) {
    pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
    // pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
    pcl::io::loadPolygonFile(polygon_file, mesh);
    pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));

    TriangleMeshModel::Ptr model =
        TriangleMeshModel::Ptr(new TriangleMeshModel(cloud));
    scene_->add(model);

    std::cout << "Just read " << polygon_file << std::endl;
    std::cout << mesh.polygons.size() << " polygons and "
              << mesh.cloud.data.size() << " triangles\n";
}

void initialize(int argc, char **argv) {
    const GLubyte *version = glGetString(GL_VERSION);
    std::cout << "OpenGL Version: " << version << std::endl;

    // works well for MIT CSAIL model 3rd floor:
    // camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);

    // works well for MIT CSAIL model 2nd floor:
    //  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);

    // works for small files:
    // camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
    camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
    camera_->setPitch(0.418879); // not sure why this is here:

    cout << "About to read: " << argv[2] << endl;
    load_PolygonMesh_model(argv[2]);
}

// A 'halo' camera - a circular ring of poses all pointing at a center point
// @param: focus_center: the center points
// @param: halo_r: radius of the ring
// @param: halo_dz: elevation of the camera above/below focus_center's z value
// @param: n_poses: number of generated poses
void generate_halo(
    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
        &poses,
    Eigen::Vector3d focus_center, double halo_r, double halo_dz, int n_poses) {

    //  std::vector<Eigen::Isometry3d,
    //  Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
    for (double t = 0; t < (2 * M_PI); t = t + (2 * M_PI) / ((double)n_poses)) {
        double x = halo_r * cos(t);
        double y = halo_r * sin(t);
        double z = halo_dz;
        double pitch = atan2(halo_dz, halo_r);
        double yaw = atan2(-y, -x);

        Eigen::Isometry3d pose;
        pose.setIdentity();
        Eigen::Matrix3d m;
        m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
            AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
            AngleAxisd(0, Eigen::Vector3d::UnitZ());

        pose *= m;
        Vector3d v(x, y, z);
        v += focus_center;
        pose.translation() = v;
        poses.push_back(pose);
    }
    return; // poses;
}

int main(int argc, char **argv) {
    int width = 640;
    int height = 480;
    window_width_ = width * 2;
    window_height_ = height * 2;

    print_info("Manually generate a simulated RGB-D point cloud using "
               "pcl::simulation. For more information, use: %s -h\n",
               argv[0]);

    if (argc < 3) {
        printHelp(argc, argv);
        return (-1);
    }
    int mode = atoi(argv[1]);

    for (int i = 0; i < 2048; i++) {
        float v = i / 2048.0;
        v = powf(v, 3) * 6;
        t_gamma[i] = v * 6 * 256;
    }

    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB); // was GLUT_RGBA
    glutInitWindowPosition(10, 10);
    glutInitWindowSize(10, 10);
    // glutInitWindowSize (window_width_, window_height_);
    glutCreateWindow("OpenGL range likelihood");

    GLenum err = glewInit();
    if (GLEW_OK != err) {
        std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
        exit(-1);
    }

    std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION)
              << std::endl;

    if (glewIsSupported("GL_VERSION_2_0"))
        std::cout << "OpenGL 2.0 supported" << std::endl;
    else {
        std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
        exit(1);
    }

    std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;

    camera_ = Camera::Ptr(new Camera());
    scene_ = Scene::Ptr(new Scene());

    range_likelihood_ =
        RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height, width, scene_));
    // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96,
    // 96, scene_)); range_likelihood_ = RangeLikelihood::Ptr(new
    // RangeLikelihood(1, 1, 480, 640, scene_));

    // Actually corresponds to default parameters:
    range_likelihood_->setCameraIntrinsicsParameters(
        640, 480, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
    range_likelihood_->setComputeOnCPU(false);
    range_likelihood_->setSumOnCPU(true);
    range_likelihood_->setUseColor(true);
    initialize(argc, argv);

    // simulation mode:
    // 0 100 fixed poses
    // 1 a 'halo' camera
    // 2 slerp between two different poses
    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
        poses;
    if (mode == 0) {
        // Create a pose:
        Eigen::Isometry3d pose;
        pose.setIdentity();
        Matrix3d m;
        // ypr:
        m = AngleAxisd(-9.14989, Vector3d::UnitZ()) *
            AngleAxisd(0.20944, Vector3d::UnitY()) *
            AngleAxisd(0, Vector3d::UnitX());
        pose *= m;
        Vector3d v;
        v << 1.31762, 0.382931, 1.89533;
        pose.translation() = v;
        for (int i = 0; i < 100; i++) { // duplicate the pose 100 times
            poses.push_back(pose);
        }
    } else if (mode == 1) {
        Eigen::Vector3d focus_center(0, 0, 1.3);
        double halo_r = 4;
        double halo_dz = 2;
        int n_poses = 16;
        generate_halo(poses, focus_center, halo_r, halo_dz, n_poses);
    } else if (mode == 2) {
        Eigen::Isometry3d pose1;
        pose1.setIdentity();
        pose1.translation() << 1, 0.75, 2;
        Eigen::Matrix3d rot1;
        rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()) *
               AngleAxisd(M_PI / 10, Eigen::Vector3d::UnitY()) *
               AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
        pose1.rotate(rot1);
        Eigen::Isometry3d pose2;
        pose2.setIdentity();
        pose2.translation() << 1, -1, 3;
        Eigen::Matrix3d rot2;
        rot2 = AngleAxisd(3 * M_PI / 4, Eigen::Vector3d::UnitZ()) *
               AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) *
               AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
        pose2.rotate(rot2);

        int n_poses = 20;
        for (double i = 0; i <= 1; i += 1 / ((double)n_poses - 1)) {
            Eigen::Quaterniond rot3;
            Eigen::Quaterniond r1(pose1.rotation());
            Eigen::Quaterniond r2(pose2.rotation());
            rot3 = r1.slerp(i, r2);
            Eigen::Isometry3d pose;
            pose.setIdentity();
            Eigen::Vector3d trans3 =
                (1 - i) * pose1.translation() + i * pose2.translation();
            pose.translation() << trans3[0], trans3[1], trans3[2];
            pose.rotate(rot3);
            poses.push_back(pose);
        }
    }

    // Do the simulation:
    double tic_main = getTime();
    for (size_t i = 0; i < poses.size(); i++) {
        std::stringstream ss;
        ss.precision(20);
        ss << "simulated_pcl_" << i << ".pcd";
        double tic = getTime();
        capture(poses[i], ss.str());
        cout << (getTime() - tic) << " sec\n";
    }
    cout << poses.size() << " poses simulated in " << (getTime() - tic_main)
         << "seconds\n";
    cout << (poses.size() / (getTime() - tic_main)) << "Hz on average\n";

    return 0;
}


/*************************************************************************\
License
    Copyright (c) 2017 Kavvadias Ioannis.

    This file is part of SPHSimulator.

    Licensed under the MIT License. See LICENSE file in the project root for
    full license information.

\************************************************************************/

#include "Settings.hpp"

#include <fstream>
#include <boost/program_options.hpp>

//----------------SPHSettings-----------------//
SPHSettings::Solver SPHSettings::SPHstep = SPHSettings::Solver::WCSPH;

//field Particles
unsigned SPHSettings::LParticles = 0;
unsigned SPHSettings::NParticles = 0;

double SPHSettings::initDx = 0.;

double     SPHSettings::particleDensity = 0.;
double     SPHSettings::dParticleDensity = 0.;
double     SPHSettings::particleMass = 0.;
double     SPHSettings::stiffness  = 0.;
double     SPHSettings::densityErr = 0.;
double     SPHSettings::viscosity = 0.;
double     SPHSettings::surfTension = 0.;
glm::dvec2 SPHSettings::grav{0.,0.};

//----------------SimulationSettings-----------------//
double SimulationSettings::dt         = 0.;
double SimulationSettings::simTimeEnd = 0.;
double SimulationSettings::simTime    = 0.;
int    SimulationSettings::showProgressEvery = 0;

//----------------InitialConditions-----------------//
InitialConditions::ParticleGeneration
    InitialConditions::particleGeneration = InitialConditions::ALLIN;
double     InitialConditions::particleGenTime = 0.; //for DRIPPING only
glm::dvec2 InitialConditions::particleInitVel{0.,0.0};
glm::dvec2 InitialConditions::particleInitPos{0.,0.0};

//----------------BoundaryConditions-----------------//
BoundingBox<glm::dvec2> BoundaryConditions::bndBox(glm::dvec2(0.0,0.0),glm::dvec2(0.0,0.0));
double                  BoundaryConditions::bndCoeff = 0.;
double                  BoundaryConditions::bndConditionRange = 0.;

//----------------RenderSettings-----------------//
unsigned RenderSettings::width  = 0;
unsigned RenderSettings::height = 0;
int RenderSettings::printEvr= 0; //print to file
RenderSettings::FileRenderType    RenderSettings::fileRender= RenderSettings::RAWDATA;
RenderSettings::DisplayRenderType RenderSettings::displayRender= RenderSettings::SIMPLE;

BoundingBox<glm::vec2> RenderSettings::displayBox;

using namespace boost::program_options;

void SPHSettings::readSettings()
{
    std::string SPHstepMethod;
    double gravityForce = 0.;

    std::ifstream Config_File("SPHConfig.ini");
    options_description SimSet("Settings");
    SimSet.add_options()
        ("SPHSettings.StepSolver",      value<std::string>(&SPHstepMethod))
        ("SPHSettings.LParticles",      value<unsigned>(&LParticles))
        ("SPHSettings.NParticles",      value<unsigned>(&NParticles))
        ("SPHSettings.initDx",          value<double>(&initDx))
        ("SPHSettings.particleDensity", value<double>(&particleDensity))
        ("SPHSettings.stiffness",       value<double>(&stiffness))
        ("SPHSettings.densityErr",      value<double>(&densityErr))
        ("SPHSettings.viscosity",       value<double>(&viscosity))
        ("SPHSettings.surfTension",     value<double>(&surfTension))
        ("SPHSettings.gravity",         value<double>(&gravityForce));

    variables_map vm;
    store(parse_config_file(Config_File, SimSet), vm);
    notify(vm);

    //process...
    if (SPHstepMethod == "WCSPH") {
      SPHstep = Solver::WCSPH;
    }
    else if (SPHstepMethod == "PCISPH") {
      SPHstep = Solver::PCISPH;
    }
    else {
      std::string errorMsg = "Unrecognized StepSolver method - AvailableMethods::\n";
      errorMsg += "  WCSPH\n";
      errorMsg += "  PCISPH\n";
      throw std::runtime_error(errorMsg);
    }

    dParticleDensity = 1.0/particleDensity;
    particleMass = particleDensity*initDx*initDx*0.98;
    grav = glm::dvec2(0.0,gravityForce);
}

void SimulationSettings::readSettings()
{
    std::ifstream Config_File("SimConfig.ini");
    options_description SimSet("Settings");
    SimSet.add_options()
        ("SimulationSettings.stepDt",      value<double>(&dt))
        ("SimulationSettings.StartTime",   value<double>(&simTime))
        ("SimulationSettings.EndTime",     value<double>(&simTimeEnd))
        ("SimulationSettings.ReportEvery", value<int>(&showProgressEvery));

    variables_map vm;
    store(parse_config_file(Config_File, SimSet), vm);
    notify(vm);
}

void InitialConditions::readSettings()
{
    std::string particleGenerator;
    double initPosX;
    double initPosY;
    double initVelX;
    double initVelY;

    std::ifstream Config_File("InitConfig.ini");
    options_description ICSet("Settings");
    ICSet.add_options()
        ("InitialConditions.particleGenerator", value<std::string>(&particleGenerator))
        ("InitialConditions.initPosX",    value<double>(&initPosX))
        ("InitialConditions.initPosY",    value<double>(&initPosY))
        ("InitialConditions.initVelX",    value<double>(&initVelX))
        ("InitialConditions.initVelY",    value<double>(&initVelY))
        ("InitialConditions.genInterval", value<double>(&particleGenTime));

    variables_map vm;
    store(parse_config_file(Config_File, ICSet), vm);
    notify(vm);

    //process...
    if (particleGenerator == "ALLIN") {
      particleGeneration = ParticleGeneration::ALLIN;
    }
    else if (particleGenerator == "FAUCET") {
      particleGeneration = ParticleGeneration::FAUCET;
    }
    else if (particleGenerator == "DRIPPING") {
      particleGeneration = ParticleGeneration::DRIPPING;
    }
    else {
      std::string errorMsg = "Unrecognized ParticleGenerator - Available Generators::\n";
      errorMsg += "  ALLIN\n";
      errorMsg += "  FAUCET\n";
      errorMsg += "  DRIPPING\n";
      throw std::runtime_error(errorMsg);
    }

    particleInitPos = glm::dvec2{initPosX, initPosY};
    particleInitVel = glm::dvec2{initVelX, initVelY};
}

void BoundaryConditions::readSettings()
{
    double bboxMinX;
    double bboxMinY;
    double bboxMaxX;
    double bboxMaxY;

    std::ifstream Config_File("BCConfig.ini");
    options_description BCSet("Settings");
    BCSet.add_options()
        ("BoundaryConditions.bboxMinX",    value<double>(&bboxMinX))
        ("BoundaryConditions.bboxMinY",    value<double>(&bboxMinY))
        ("BoundaryConditions.bboxMaxX",    value<double>(&bboxMaxX))
        ("BoundaryConditions.bboxMaxY",    value<double>(&bboxMaxY))
        ("BoundaryConditions.boundaryCoeff", value<double>(&bndCoeff))
        ("BoundaryConditions.boundaryRange", value<double>(&bndConditionRange));

    variables_map vm;
    store(parse_config_file(Config_File, BCSet), vm);
    notify(vm);

    //process...
    bndBox = BoundingBox<glm::dvec2>(glm::dvec2(bboxMinX, bboxMinY),
                                     glm::dvec2(bboxMaxX, bboxMaxY));
}

void RenderSettings::readSettings()
{
    std::string fileRenderMode;
    std::string displayRenderMode;

    std::ifstream Config_File("RenderConfig.ini");
    options_description RenderSet("Settings");
    RenderSet.add_options()
        ("Rendering.width",      value<unsigned>(&width))
        ("Rendering.height",     value<unsigned>(&height))
        ("Rendering.printEvery", value<int>(&printEvr))
        ("Rendering.renderToFileAs",     value<std::string>(&fileRenderMode))
        ("Rendering.initRenderToScreen", value<std::string>(&displayRenderMode));

    variables_map vm;
    store(parse_config_file(Config_File, RenderSet), vm);
    notify(vm);

    //process...
    if (fileRenderMode == "RAWDATA") {
      fileRender= RAWDATA;
    }
    else if (fileRenderMode == "DISCRETE") {
      fileRender = DISCRETE;
    }
    else if (fileRenderMode == "METABALL") {
      fileRender = METABALL;
    }
    else {
      std::string errorMsg = "Unrecognized fileRender mode - Available modes::\n";
      errorMsg += "  RAWDATA\n";
      errorMsg += "  DISCRETE\n";
      errorMsg += "  METABALL\n";
      throw std::runtime_error(errorMsg);
    }

    if (displayRenderMode == "INDEX") {
      displayRender= INDEX;
    }
    else if (displayRenderMode == "SIMPLE") {
      displayRender= SIMPLE;
    }
    else if (displayRenderMode == "PRESSFORCES") {
      displayRender= PRESSFORCES;
    }
    else if (displayRenderMode == "VISCFORCES") {
      displayRender= VISCFORCES;
    }
    else if (displayRenderMode == "SURFFORCES") {
      displayRender= SURFFORCES;
    }
    else if (displayRenderMode == "OTHERFORCES") {
      displayRender= OTHERFORCES;
    }
    else if (displayRenderMode == "ALLFORCES") {
      displayRender= ALLFORCES;
    }
    else {
      std::string errorMsg = "Unrecognized initial display Render mode - Available modes::\n";
      errorMsg += "  INDEX\n";
      errorMsg += "  SIMPLE\n";
      errorMsg += "  PRESSFORCES\n";
      errorMsg += "  VISCFORCES\n";
      errorMsg += "  SURFFORCES\n";
      errorMsg += "  OTHERFORCES\n";
      errorMsg += "  ALLFORCES\n";
      throw std::runtime_error(errorMsg);
    }

    displayBox = BoundingBox<glm::vec2> (
        glm::vec2
        (
            float(BoundaryConditions::bndBox.minX()-0.1*BoundaryConditions::bndBox.dx()),
            float(BoundaryConditions::bndBox.minY()-0.1*BoundaryConditions::bndBox.dx())
        ),
        glm::vec2
        (
            float(BoundaryConditions::bndBox.maxX()+0.1*BoundaryConditions::bndBox.dx()),
            float(double(height)/double(width)*BoundaryConditions::bndBox.maxX()+0.1*BoundaryConditions::bndBox.dx())
        )
    );
}


// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <vector>
#include <iostream>

#include "mldb/plugins/jml/jml/classifier.h"
#include "mldb/plugins/jml/jml/dense_features.h"
#include "mldb/utils/distribution.h"
#include "mldb/utils/environment.h"

using namespace ML;
using namespace std;

const string tmpDir = Environment::instance()["TMP"];
const string binDir = Environment::instance()["BIN"];

float rand_prob() {
    return float(rand()) / float(RAND_MAX);
}

BOOST_AUTO_TEST_CASE( weighted_training_test )
{
    string filename1 = tmpDir + "/weighted_training_test_dataset1.txt";
    string filename2 = tmpDir + "/weighted_training_test_dataset2.txt";
    string modelname1 = tmpDir + "/weighted_training_test_model1.cls";
    string modelname2 = tmpDir + "/weighted_training_test_model2.cls";
    string modelname3 = tmpDir + "/weighted_training_test_model3.cls";
    string configname = tmpDir + "/weighted_training_test_config.cls";

    ofstream filestream;
    filestream.open(filename1);

    filestream << "LABEL:k=BOOLEAN ExampleId:k=STRING/o=BIASED dim1:k=REAL dim2:k=REAL" << endl;
    filestream << "0 a 0.000000 0.000000" << endl;
    filestream << "0 b 0.100000 0.100000" << endl;
    filestream << "0 c 0.100000 0.400000" << endl;
    filestream << "0 d 0.400000 0.100000" << endl;
    filestream << "1 e 0.500000 0.500000" << endl;
    filestream << "1 u 1.000000 1.000000" << endl;
    filestream << "1 v 0.900000 0.600000" << endl;
    filestream << "1 x 0.600000 0.800000" << endl;
    filestream << "1 x 0.800000 0.700000" << endl;
    filestream << "1 x 0.900000 0.800000" << endl;

    filestream.close();
    filestream.open(filename2);

    filestream << "LABEL:k=BOOLEAN WEIGHT:k=REAL ExampleId:k=STRING/o=BIASED dim1:k=REAL dim2:k=REAL" << endl;
    filestream << "0 1.000 a 0.000000 0.000000" << endl;
    filestream << "0 1.000 b 0.100000 0.100000" << endl;
    filestream << "0 1.000 c 0.100000 0.400000" << endl;
    filestream << "0 1.000 d 0.400000 0.100000" << endl;
    filestream << "1 1.000 e 0.500000 0.500000" << endl;
    filestream << "1 1.000 u 1.000000 1.000000" << endl;
    filestream << "1 1.000 v 0.900000 0.600000" << endl;
    filestream << "1 1.000 x 0.600000 0.800000" << endl;
    filestream << "1 1.000 x 0.800000 0.700000" << endl;
    filestream << "1 1.000 x 0.900000 0.800000" << endl;

    filestream.close();
    filestream.open(configname);

    filestream << "glz {type=glz;\nverbosity=3;\nnormalize = true;\nregularization = l2;\n}" << endl;

    filestream.close();

    vector<string> probabilizer = {"-p 3", "-p 1 -Q 1"};
    for(int probIdx=0; probIdx<probabilizer.size(); probIdx++) {
        cout << MLDB::format(" ----------------------- TESTING WITH PROBABILIZER IN MODE %s (%s) "
                "-----------------------", probabilizer[probIdx], (probIdx == 0 ? "off" : "on")) << endl;

        // weights: LABEL, ExampleId
        // ignore: ExampleId
        string cmd1 = binDir + "/classifier_training_tool " + filename1 + " -c " + configname + " -n glz glz.link_function=linear glz.regularization=none -E 1 "+probabilizer[probIdx]+" -o " + modelname1 + " glz.verbosity=10  -W LABEL/F,ExampleId/F -z ExampleId";

        // weights: LABEL, WEIGHT, ExampleId
        // ignore: ExampleId, WEIGHT
        string cmd2 = binDir + "/classifier_training_tool " + filename2 + " -c " + configname + " -n glz glz.link_function=linear glz.regularization=none -E 1 "+probabilizer[probIdx]+" -o " + modelname2 + " glz.verbosity=10  -W LABEL/F,ExampleId/F,WEIGHT/V -z ExampleId -z WEIGHT";

        // weights: LABEL, WEIGHT
        // ignore: ExampleId, WEIGHT
        string cmd3 = binDir + "/classifier_training_tool " + filename2 + " -c " + configname + " -n glz glz.link_function=linear glz.regularization=none -E 1 "+probabilizer[probIdx]+" -o " + modelname3 + " glz.verbosity=10  -W LABEL/F,WEIGHT/V -z ExampleId -z WEIGHT";

        cout << cmd1 << endl;
        cout << cmd2 << endl;
        cout << cmd3 << endl;

        if ( system(cmd1.c_str()) == -1 )
            throw Exception("Call 1 to classifier_training_tool failed");

        if ( system(cmd2.c_str()) == -1 )
            throw Exception("Call 2 to classifier_training_tool failed");

        if ( system(cmd3.c_str()) == -1 )
            throw Exception("Call 2 to classifier_training_tool failed");

        Classifier class1;
        Classifier class2;
        Classifier class3;

        class1.load(modelname1);
        class2.load(modelname2);//"build/x86_64/tmp/pclick_predictor_test_model_ew.cls");
        class3.load(modelname3);

        for (int i=0; i<5; ++i){
            vector<float> point = {1.0, 0.0, rand_prob(), rand_prob()};
            distribution<float> point1(point);
            distribution<float> point2(point);
            point2.insert(point2.begin(), 1.0);

             cerr << "point1: " << point1 << endl;
            float prediction1 = class1.impl->predict(0, *class1.impl->feature_space<Dense_Feature_Space>()->encode(point1));

             cerr << "point2: " << point2 << endl;
            float prediction2 = class2.impl->predict(0, *class2.impl->feature_space<Dense_Feature_Space>()->encode(point2));

            BOOST_CHECK_EQUAL(prediction1, prediction2);

            float prediction3 = class3.impl->predict(0, *class3.impl->feature_space<Dense_Feature_Space>()->encode(point2));
            cout << "pred2: " << prediction2 << endl;
            cout << "pred3: " << prediction3 << endl;
            BOOST_CHECK_PREDICATE( std::not_equal_to<float>(), (prediction2)(prediction3) );
        }

        cout << "removing " << modelname1 << endl;
        remove(modelname1.c_str());
        cout << "removing " << modelname2 << endl;
        remove(modelname2.c_str());
        cout << "removing " << modelname3 << endl;
        remove(modelname3.c_str());
    }

    cout << "removing " << filename1 << endl;
    remove(filename1.c_str());
    cout << "removing " << filename2 << endl;
    remove(filename2.c_str());
    cout << "removing " << configname << endl;
    remove(configname.c_str());
}


#pragma once

#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable:4127) // 定数条件式
#endif

// Eigen
#define EIGEN_NO_DEBUG 1
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#if defined(_MSC_VER)
#pragma warning(pop)
#endif


#include "neighborhood_spatial_hashing.hpp"
#include "generic/eigen.hpp"

#include <vector>
#include <unordered_map>
#include <algorithm>
#include <Eigen/Core>
#include <boost/log/trivial.hpp>

namespace GooBalls {

namespace Spatial {

NeighborhoodSpatialHashing::NeighborhoodSpatialHashing(){
    // empty
}

void NeighborhoodSpatialHashing::inRange(const Coordinates2d& points, Float h){
    // compute
    /// We use a grid of size h and a hash table for near linear complexity
    GridCoordinates queryGridCoords = (points.array()/h).floor().cast<int>();
    computeInRange(points, points, queryGridCoords, queryGridCoords, h);
    assert(points.rows() == m_indexes.size());
    for(int i = 0; i < points.rows(); ++i){
        assert(!m_indexes[i].empty());
    }
}

void NeighborhoodSpatialHashing::inRange(const Coordinates2d& query, const Coordinates2d& target, Float h){
    // compute
    /// We use a grid of size h and a hash table for near linear complexity
    GridCoordinates queryGridCoords = (query.array()/h).floor().cast<int>();
    GridCoordinates targetGridCoords = (target.array()/h).floor().cast<int>();
    computeInRange(query, target, queryGridCoords, targetGridCoords, h);
    assert(query.rows() == m_indexes.size());
}

void NeighborhoodSpatialHashing::populateGrid(const GridCoordinates& targetGrid){
    auto& grid = m_grid;
    std::for_each(grid.begin(), grid.end(), [](auto& e){e.second.clear();});
    // fill grid
    for(int i = 0; i < targetGrid.rows(); ++i){
        grid[GridCoord(targetGrid(i,0), targetGrid(i,1))].push_back(i);
    }
}

inline void addCell(const std::vector<int>& cell, const Coordinates2d& target, const TranslationVector iv, const Float h2, std::vector<int>& indexes){
    for(int jj : cell){
        // we now have the coordinates of all candidates
        // compute squared distance and find good ones
        Float d2 = (target.row(jj) - iv).squaredNorm();
        if(d2 <= h2){
            indexes.push_back(jj);
        }
    }
}

void NeighborhoodSpatialHashing::computeInRange(
    const Coordinates2d& query, const Coordinates2d& target,
    const GridCoordinates& queryGrid, const GridCoordinates& targetGrid,
    Float h) {
    // reset output member members
    resetIndexes(query.rows());
    populateGrid(targetGrid);
    // guarantee: the indices in the per grid lists are sorted in ascending order (we get that for free)
    // for each point, find all neighbors in the ball with radius h
    const Float h2 = h*h;
    const auto& grid = m_grid;

    //#pragma omp parallel for schedule(static)
    for(int i = 0; i < query.rows(); ++i){
        auto& indexes = m_indexes[i];
        int ix = queryGrid(i, 0);
        int ixs = ix-1;
        int ixe = ix+1;
        int iy = queryGrid(i, 1);
        int iys = iy-1;
        int iye = iy+1;
        TranslationVector iv = query.row(i);
        for(int iix = ixs; iix <= ixe; iix++){
            for(int iiy = iys; iiy <= iye; iiy++){
                const auto coord = GridCoord(iix, iiy);
                const auto cell = grid.find(coord);
                if(cell == grid.end()){
                    continue;
                }
                addCell(cell->second, target, iv, h2, indexes);
            }
        }
    }

}

} // Spatial
} // GooBalls


// Fair Exponential Smoothing with Small Alpha by Juha Reunanen, 2015

#include "../fessa_opencv_mat.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(opencv_mat)
{
    typedef float T;
    const auto type = CV_32FC1;

    fessa::MatFilter<> filter(0.001, type);
    cv::Mat m(2, 2, type);

    m.setTo(1);
    const auto filtered1 = filter.filter(m);
    BOOST_REQUIRE_EQUAL(filtered1.type(), m.type());
    BOOST_REQUIRE_EQUAL(filtered1.rows, m.rows);
    BOOST_REQUIRE_EQUAL(filtered1.at<T>(0, 0), 1);

    m.setTo(2);
    const auto filtered2 = filter.filter(m);
    BOOST_REQUIRE_GT(filtered2.at<T>(0, 0), 1.5);
    BOOST_REQUIRE_LT(filtered2.at<T>(0, 0), 1.501);

    m.setTo(3);
    const auto filtered3 = filter.filter(m);
    BOOST_REQUIRE_GT(filtered3.at<T>(0, 0), 2.0);
    BOOST_REQUIRE_LT(filtered3.at<T>(0, 0), 2.001);
}


#define CATCH_CONFIG_MAIN

#include "KKSFreeEnergyFunctionDiluteBinary.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <string>

namespace pt = boost::property_tree;

TEST_CASE("Dilute binary equilibrium", "[dilute binary equilibrium]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::LINEAR;

    // read dilute alloy informaton
    pt::ptree conc_db;
    try
    {
        pt::read_json("dilute_binary.json", conc_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }
    pt::write_json(std::clog, conc_db);

    double temperature = 1710.;

    Thermo4PFM::KKSFreeEnergyFunctionDiluteBinary cafe(
        conc_db, energy_interp_func_type, conc_interp_func_type);

    const double tol = 1.e-5;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;

    // compute equilibrium compositions
    double ceq[2];
    cafe.computeCeqT(temperature, &ceq[0]);
    std::cout << "   ceL = " << ceq[0] << std::endl;
    std::cout << "   ceS = " << ceq[1] << std::endl;

    // test if chemical potentials are equal for equilibrium compositions
    double derivL;
    double derivS;
    cafe.computeDerivFreeEnergy(temperature, &ceq[0], pi0, &derivL);
    cafe.computeDerivFreeEnergy(temperature, &ceq[1], pi1, &derivS);
    std::cout << "   dfL/dcL = " << derivL << std::endl;
    std::cout << "   dfS/dcS = " << derivS << std::endl;
    CHECK(derivS == Approx(derivL).margin(tol));

    // test if driving force is 0 for equilibrium compositions
    double fl = cafe.computeFreeEnergy(temperature, &ceq[0], pi0, false);
    double fa = cafe.computeFreeEnergy(temperature, &ceq[1], pi1, false);
    std::cout << "   fL = " << fl << std::endl;
    std::cout << "   fS = " << fa << std::endl;
    double diff = fa - fl - derivS * (ceq[1] - ceq[0]);
    CHECK(std::abs(diff) == Approx(0.).margin(tol));

    // compute second derivatives for info only
    std::vector<double> d2fdc2(1);
    cafe.computeSecondDerivativeFreeEnergy(
        temperature, &ceq[0], pi0, d2fdc2.data());
    std::cout << "-------------------------------" << std::endl;
    std::cout << "Second derivatives" << std::endl;
    std::cout << "At ceL: " << d2fdc2[0] << std::endl;
    cafe.computeSecondDerivativeFreeEnergy(
        temperature, &ceq[1], pi1, d2fdc2.data());
    std::cout << "At ceS: " << d2fdc2[0] << std::endl;
}


#include "conex/hermitian_psd.h"
#include "conex/dense_lmi_constraint.h"

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "conex/cone_program.h"

namespace conex {

using JordanTypes = testing::Types<Real, Complex, Quaternions, Octonions>;

using Eigen::MatrixXd;

MatrixXd ToMat(const Real::Matrix& x) { return x.at(0); }

std::vector<MatrixXd> ToMat(const std::vector<Real::Matrix>& x) {
  std::vector<MatrixXd> y;
  for (const auto& e : x) {
    y.push_back(ToMat(e));
  }
  return y;
}
int CompareRealHermitianWithLMI(int rank, int dim) {
  using T = Real;
  using Matrix = typename T::Matrix;
  SolverConfiguration config;
  config.inv_sqrt_mu_max = std::sqrt(1.0 / 1e-4);
  config.final_centering_tolerance = 1e-8;
  config.prepare_dual_variables = true;
  int m = dim;
  std::vector<Matrix> constraint_matrices(m);
  Matrix constraint_affine = T::Identity(rank);
  for (int i = 0; i < m; i++) {
    constraint_matrices.at(i) = T::Random(rank, rank);
    constraint_matrices.at(i) =
        T::Add(constraint_matrices.at(i),
               T::ConjugateTranspose(constraint_matrices.at(i)));
  }
  HermitianPsdConstraint<T> T2(rank, constraint_matrices, constraint_affine);

  Program prog(m);
  DenseMatrix y(m, 1);
  prog.AddConstraint(T2);

  auto b = GetFeasibleObjective(&prog);
  bool solved_1 = Solve(b, prog, config, y.data());
  MatrixXd X_1(rank, rank);
  prog.GetDualVariable(0, &X_1);

  Program prog2(m);
  DenseMatrix y2(m, 1);
  prog2.AddConstraint(DenseLMIConstraint(rank, ToMat(constraint_matrices),
                                         ToMat(constraint_affine)));

  bool solved_2 = Solve(b, prog2, config, y2.data());
  MatrixXd X_2(rank, rank);
  prog2.GetDualVariable(0, &X_2);
  EXPECT_NEAR((y2 - y).norm(), 0, 1e-11);
  EXPECT_NEAR((X_2 - X_1).norm(), 0, 1e-11);

  return solved_1 && solved_2;
}

template <typename T>
class TestCases : public testing::Test {
 public:
  using Type = T;

  void DoSolve(int rank, int m) {
    using Matrix = typename T::Matrix;
    SolverConfiguration config;

    config.inv_sqrt_mu_max = 1000;
    config.final_centering_steps = 4;
    config.max_iterations = 100;

    std::vector<Matrix> constraint_matrices(m);
    Matrix constraint_affine = T::Identity(rank);
    for (int i = 0; i < m; i++) {
      constraint_matrices.at(i) = T::Random(rank, rank);
      constraint_matrices.at(i) =
          T::Add(constraint_matrices.at(i),
                 T::ConjugateTranspose(constraint_matrices.at(i)));
    }
    HermitianPsdConstraint<T> T2(rank, constraint_matrices, constraint_affine);

    Program prog(m);
    DenseMatrix y(m, 1);
    prog.AddConstraint(T2);

    auto b = GetFeasibleObjective(&prog);
    EXPECT_TRUE(Solve(b, prog, config, y.data()));
  }
  void SolveRandomInstances(int n, int m) {
    for (int i = 0; i < 1; i++) {
      DoSolve(n, m);
    }
  }
};

TYPED_TEST_CASE(TestCases, JordanTypes);
TYPED_TEST(TestCases, SolveRandomInstances) {
  for (int i = 0; i < 3; i++) {
    if (!std::is_same<typename TestFixture::Type, Octonions>::value) {
      TestFixture::SolveRandomInstances(3 + i * 40, 2 + 3 * i);
    } else {
      TestFixture::SolveRandomInstances(3, 2 + 3 * i);
    }
  }
}

GTEST_TEST(Hermitian, CompareWithLMI) {
  srand(0);
  for (int i = 0; i < 4; i++) {
    int num_vars = 4;
    int rank = 8;
    EXPECT_TRUE(CompareRealHermitianWithLMI(rank, num_vars));
  }
}

}  // namespace conex


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/math/matrix_serialization.h>  // for << >> ops of matrices
#include <mrpt/opengl/CEllipsoidInverseDepth2D.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE(
	CEllipsoidInverseDepth2D, CRenderizableShaderWireFrame, mrpt::opengl)

/*---------------------------------------------------------------
							transformFromParameterSpace
  ---------------------------------------------------------------*/
void CEllipsoidInverseDepth2D::transformFromParameterSpace(
	const std::vector<BASE::array_parameter_t>& in_pts,
	std::vector<BASE::array_point_t>& out_pts) const
{
	MRPT_START

	// (inv_range,yaw) --> (x,y)
	const size_t N = in_pts.size();
	out_pts.resize(N);
	for (size_t i = 0; i < N; i++)
	{
		const double inv_range = in_pts[i][0];
		const double yaw = in_pts[i][1];
		const double range = inv_range < 0
								 ? m_underflowMaxRange
								 : (inv_range != 0 ? 1. / inv_range : 0);
		out_pts[i][0] = d2f(range * cos(yaw));
		out_pts[i][1] = d2f(range * sin(yaw));
	}

	MRPT_END
}

uint8_t CEllipsoidInverseDepth2D::serializeGetVersion() const { return 0; }
void CEllipsoidInverseDepth2D::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	BASE::thisclass_writeToStream(out);

	out << m_underflowMaxRange;
}
void CEllipsoidInverseDepth2D::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			readFromStreamRender(in);
			BASE::thisclass_readFromStream(in);

			in >> m_underflowMaxRange;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizable::notifyChange();
}


/*
 * Copyright (c) 2013-2020 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef OPTIMIZE_HPP
#define OPTIMIZE_HPP

#include <iostream>
#include <list>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/autodif.hpp>

// 0: Do not use TRIM
// 1: use TRIM and use slow trim algorithm
// 2: use TRIM and use fast bat slightly inefficient trim algorithm
// 3: use TRIM and use new trim algorithm

#ifndef OPTIMIZE_TRIM
#define OPTIMIZE_TRIM 0
#endif

#ifndef OPTIMIZE_ZERODIVIDE
#define OPTIMIZE_ZERODIVIDE 2
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

// change scalar function f to -f
template <class F> struct Opt_MakeMinus {
	F f;
	Opt_MakeMinus(F f) : f(f) {}
	template <class T> T operator()(const ub::vector<T>& x) {
		return -f(x);
	}
};

// change scalar function T -> T
// to 1-dimentional vector function T^1 -> T
template <class F> struct Opt_MakeVec {
	F f;
	Opt_MakeVec(F f) : f(f) {}
	template <class T> T operator()(const ub::vector<T>& x) {
		return f(x(0));
	}
};

template <class T, class F>
std::list< ub::vector< interval<T> > >
optimize(const ub::vector< interval<T> >& init, F f, T limit, bool unify = true, int verbose = 0)
{
	std::list< ub::vector< interval<T> > > targets;
	targets.push_back(init);
	return optimize_list(targets, f, limit, unify, verbose);
}

template <class T, class F>
std::list< ub::vector< interval<T> > >
optimize_list(std::list< ub::vector< interval<T> > > targets, F f, T limit, bool unify = true, int verbose = 0)
{
	int s = (targets.front()).size();
	ub::vector< interval<T> > I, C, I1, I2, IR, fdi, C2;
	interval<T> fc, fi, mvf, fc2;
	T tmp, tmp2;
	std::list< ub::vector< interval<T> > > solutions;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	int i, j, k, mi;
	bool flag, errflag;
	interval<T> A, B, J, J2, Itmp;
#if OPTIMIZE_TRIM == 3
	ub::vector< interval<T> > A0, A1, A2; // for new trim algorithm
#endif // OPTIMIZE_TRIM == 3

	C2.resize(s);

	T delta = std::numeric_limits<T>::max();

	while (!targets.empty()) {
		I = targets.front();
		targets.pop_front();
		errflag = false; // evaluation error occurs or not

		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			errflag = true;
			goto label;
		}

		if (fi.lower() > delta) {
			continue;
		}

		C = mid(I);
		try {
			fc = f(C);
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
		}
		catch (std::domain_error& e) {
			// errflag = true;
			goto label;
		}

		fdi.resize(s); // prepare for constant f
		mvf = fc + inner_prod(fdi, I - C);
		if (mvf.lower() > delta) {
			continue;
		}

		// update delta at C
		tmp = fc.upper();
		if (tmp < delta) delta = tmp;

		// C2 is likely to give small value
		for (i=0; i<s; i++) {
			tmp = mid(fdi(i));
			if (tmp > 0.) tmp2 = I(i).lower();
			else if (tmp < 0.) tmp2 = I(i).upper();
			else tmp2 = mid(I(i));
			C2(i).assign(tmp2, tmp2);
		}

		// update delta at C2
		try {
			fc2 = f(C2);
		}
		catch (std::domain_error& e) {
			// errflag = true;
			goto label;
		}
		tmp = fc2.upper();
		if (tmp < delta) delta = tmp;

#if OPTIMIZE_TRIM >= 1
		// interval shrinking

#if OPTIMIZE_TRIM == 3
		// prepare for new trim algorithm
		A0.resize(s);
		A1.resize(s);
		A2.resize(s);
		for (j=0; j<s; j++) {
			A0(j) = fdi(j) * (I(j)-C(j));
		}
		Itmp = 0.;
		for (j=0; j<s; j++) {
			A1(j) = Itmp;
			Itmp += A0(j);
		}
		Itmp = 0.;
		for (j=s-1; j>=0; j--) {
			A2(j) = Itmp;
			Itmp += A0(j);
		}
#endif // OPTIMIZE_TRIM == 3

		IR = I;
		flag = false; // non-existence in I turns out or not
		for (j=0; j<s; j++) {
			B = fdi(j);

#if OPTIMIZE_TRIM == 1
			// calculate A simply
			// simple but slow
			A = 0.;
			for (k=0; k<s; k++) {
				if (k == j) continue;
				A += fdi(k) * (I(k)-C(k));
			}
			A += fc;
#endif // OPTIMIZE_TRIM == 1
#if OPTIMIZE_TRIM == 2
			// old trim algorithm
			// calculate back A from mvf
			A = mvf;
			Itmp = B * (I(j)-C(j));
			rop<T>::begin();
			tmp = rop<T>::sub_down(A.lower(), Itmp.lower());
			tmp2 = rop<T>::sub_up(A.upper(), Itmp.upper());
			rop<T>::end();
			A.assign(tmp, tmp2);
#endif // OPTIMIZE_TRIM == 2
#if OPTIMIZE_TRIM == 3
			// new trim algorithm
			A = fc + A1(j) + A2(j);
#endif // OPTIMIZE_TRIM == 3

			// A -= delta;
			A -= interval<T>(-std::numeric_limits<T>::infinity(), delta);

			if (zero_in(B)) {
#if OPTIMIZE_ZERODIVIDE >= 1
				bool bdummy;
				if (rad(B) <= 0) continue;
				J = C(j) - division_part1(A, B, bdummy);
				J2 = C(j) - division_part2(A, B);
				if (overlap(IR(j), J)) {
					if (overlap(IR(j), J2)) {
#if OPTIMIZE_ZERODIVIDE == 2
						if (overlap(J, J2)) continue;
						// interval division
						I1 = IR;
						I2 = IR;
						I1(j) = intersect(IR(j), J);
						I2(j) = intersect(IR(j), J2);
						targets.push_back(I1);
						targets.push_back(I2);
						flag = true;
						break;
#else
						continue;
#endif
					} else {
						IR(j) = intersect(IR(j), J);
					}
				} else {
					if (overlap(IR(j), J2)) {
						IR(j) = intersect(IR(j), J2);
					} else {
						flag = true;
						break;
					}
				}
#else
				continue;
#endif
			} else {
				J = C(j) - A/B;
				if (overlap(IR(j), J)) {
					IR(j) = intersect(IR(j), J);
				} else {
					flag = true;
					break;
				}
			}
		}

		// non-existence in I turns out
		if (flag == true) {
			continue;
		}

		I = IR;

#endif // OPTIMIZE_TRIM >= 1

		label:;

		tmp2 = 0.;
		for (i=0; i<s; i++) {
			tmp = width(I(i));
			if (tmp > tmp2) {
				tmp2 = tmp; mi = i;
			}
		}

		if (tmp2 < limit && errflag == false) {
			if (verbose >= 1) {
				std::cout << I << "\n";
			}
			if (unify) {
				while (true) {
					flag = false;
                                        p = solutions.begin();
					while (p != solutions.end()) {
						if (overlap(*p, I)) {
							I = hull(I, *p);
							p = solutions.erase(p);
							flag = true;
							continue;
						}
						p++;
					}
					if (flag == false) break;
				}
			}
			solutions.push_back(I);
			continue;
		}

		tmp = mid(I(mi));
		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
		targets.push_back(I1);
		targets.push_back(I2);
	}

	if (verbose >= 1) {
		std::cout << delta << "\n";
	}

	return solutions;
}

// rename of optimize
template <class T, class F>
std::list< ub::vector< interval<T> > >
minimize(const ub::vector< interval<T> >& x, F f, T limit, bool unify = true, int verbose = 0)
{
        return optimize(x, f, limit, unify, verbose);
}

// specify maximum number of subdivision instead of width limit
template <class T, class F>
std::list< ub::vector< interval<T> > >
minimize(const ub::vector< interval<T> >& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	int i;
	T tmp;
	tmp = 1.;
	for (i=0; i<x.size(); i++) {
		tmp *= width(x(i));
	}
	T limit = (T)std::pow(tmp / n, 1.0/x.size());
        return minimize(x, f, limit, unify, verbose);
}

// return only mininum value
template <class T, class F>
interval<T>
minimize_value(const ub::vector< interval<T> >& x, F f, T limit, int verbose = 0) {
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	T ret_l, ret_u;
	interval<T> tmp;

	ret_l = std::numeric_limits<T>::infinity();
	ret_u = std::numeric_limits<T>::infinity();

	result = minimize(x, f, limit, false, verbose); // disable unify
	p = result.begin();
	while (p != result.end()) {
		tmp = f(*p);
		ret_u = std::min(ret_u, tmp.upper());
		ret_l = std::min(ret_l, tmp.lower());
		p++;
	}

	return kv::interval<T>(ret_l, ret_u);
}

// return only mininum value
// specify maximum number of subdivision instead of width limit
template <class T, class F>
interval<T>
minimize_value(const ub::vector< interval<T> >& x, F f, int n = 1000, int verbose = 0) {
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	T ret_l, ret_u;
	interval<T> tmp;

	ret_l = std::numeric_limits<T>::infinity();
	ret_u = std::numeric_limits<T>::infinity();

	result = minimize(x, f, n, false, verbose); // disable unify
	p = result.begin();
	while (p != result.end()) {
		tmp = f(*p);
		ret_u = std::min(ret_u, tmp.upper());
		ret_l = std::min(ret_l, tmp.lower());
		p++;
	}

	return kv::interval<T>(ret_l, ret_u);
}

// maximize/maximize_value

template <class T, class F>
std::list< ub::vector< interval<T> > >
maximize(const ub::vector< interval<T> >& x, F f, T limit, bool unify = true, int verbose = 0)
{
        return minimize(x, Opt_MakeMinus<F>(f), limit, unify, verbose);
}

template <class T, class F>
std::list< ub::vector< interval<T> > >
maximize(const ub::vector< interval<T> >& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
        return minimize(x, Opt_MakeMinus<F>(f), n, unify, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const ub::vector< interval<T> >& x, F f, T limit, int verbose = 0) {
	return -minimize_value(x, Opt_MakeMinus<F>(f), limit, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const ub::vector< interval<T> >& x, F f, int n = 1000, int verbose = 0) {
	return -minimize_value(x, Opt_MakeMinus<F>(f), n, verbose);
}

// one dimensional version of minimize/minimize_value

template <class T, class F>
std::list< interval<T> >
minimize(const interval<T>& x, F f, T limit, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = minimize(v, g, limit, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
std::list< interval<T> >
minimize(const interval<T>& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = minimize(v, g, n, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
interval<T>
minimize_value(const interval<T>& x, F f, T limit, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return minimize_value(v, g, limit, verbose);
}

template <class T, class F>
interval<T>
minimize_value(const interval<T>& x, F f, int n = 1000, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return minimize_value(v, g, n, verbose);
}

// one dimensional version of maximize/maximize_value

template <class T, class F>
std::list< interval<T> >
maximize(const interval<T>& x, F f, T limit, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = maximize(v, g, limit, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
std::list< interval<T> >
maximize(const interval<T>& x, F f, int n = 1000, bool unify = true, int verbose = 0)
{
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	std::list< ub::vector< interval<T> > > result;
	typename std::list< ub::vector< interval<T> > >::iterator p;
	std::list< interval<T> > result2;
	v(0) = x;
        result = maximize(v, g, n, unify, verbose);
	p = result.begin();
	while (p != result.end()) {
		result2.push_back((*(p++))(0));
	}
	return result2;
}

template <class T, class F>
interval<T>
maximize_value(const interval<T>& x, F f, T limit, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return maximize_value(v, g, limit, verbose);
}

template <class T, class F>
interval<T>
maximize_value(const interval<T>& x, F f, int n = 1000, int verbose = 0) {
	Opt_MakeVec<F> g(f);
	ub::vector< interval<T> > v(1);
	v(0) = x;
	return maximize_value(v, g, n, verbose);
}

} // namespace kv

#endif // OPTIMIZE_HPP


#pragma once

#include <boost/random.hpp>

#include <vector>
#include <map>

namespace verilog {

  struct Generator {
    Generator(): bd(0.5) {}
    boost::random::mt19937 rng;
    boost::random::bernoulli_distribution<> bd;
    //  Generates a random input of a given size
    void generate_inputs(
        const std::vector<std::string> & names,
        std::map<std::string, bool> & inputs) {
      for(auto n : names) {
        inputs[n] = bd(rng);
      }
    }
  };

}


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

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/bitwise_notand.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/twoto31.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int32_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE bd::as_floating_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return _mm256_cvtepi32_ps(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint32_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE bd::as_floating_t<A0> operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      using si_type = bd::as_integer_t<A0, signed>;
      si_type signed_a = bitwise_cast<si_type>(a0);
      si_type fixed_a  = bitwise_notand(Signmask<si_type>(), a0);
      return if_plus(is_ltz(signed_a), tofloat(fixed_a), Twoto31<bd::as_floating_t<A0>>());
    }
  };
} } }

#endif


#ifndef IEOMPP_TYPES_EIGEN_TRAITS_HPP_
#define IEOMPP_TYPES_EIGEN_TRAITS_HPP_

#include "ieompp/types/matrix.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace ieompp
{
    namespace types
    {
        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsMatrix<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Cols, int Options, int MaxCols>
        struct IsMatrix<Eigen::Matrix<Scalar, 1, Cols, Options, 1, MaxCols>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Rows, int Options, int MaxRows>
        struct IsMatrix<Eigen::Matrix<Scalar, Rows, 1, Options, MaxRows, 1>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsMatrix<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsVector<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Cols, int Options, int MaxCols>
        struct IsVector<Eigen::Matrix<Scalar, 1, Cols, Options, 1, MaxCols>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Rows, int Options, int MaxRows>
        struct IsVector<Eigen::Matrix<Scalar, Rows, 1, Options, MaxRows, 1>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsVector<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsRowVector<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Cols, int Options, int MaxCols>
        struct IsRowVector<Eigen::Matrix<Scalar, 1, Cols, Options, 1, MaxCols>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsRowVector<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsColumnVector<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Rows, int Options, int MaxRows>
        struct IsColumnVector<Eigen::Matrix<Scalar, Rows, 1, Options, MaxRows, 1>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsColumnVector<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct ScalarType<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            using type = Scalar;
        };

        template <typename Scalar, int Options, typename Index>
        struct ScalarType<Eigen::SparseMatrix<Scalar, Options, Index>> {
            using type = Scalar;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IndexType<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            using type = int;
        };

        template <typename Scalar, int Options, typename Index>
        struct IndexType<Eigen::SparseMatrix<Scalar, Options, Index>> {
            using type = Index;
        };

        template <typename T>
        struct IsEigenMatrix {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsEigenMatrix<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsEigenMatrix<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = false;
        };

        template <typename T>
        struct IsEigenDenseMatrix {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsEigenDenseMatrix<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = true;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsEigenDenseMatrix<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = false;
        };

        template <typename T>
        struct IsEigenSparseMatrix {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
        struct IsEigenSparseMatrix<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
            static constexpr bool value = false;
        };

        template <typename Scalar, int Options, typename Index>
        struct IsEigenSparseMatrix<Eigen::SparseMatrix<Scalar, Options, Index>> {
            static constexpr bool value = true;
        };
    }
}

#endif