#include "dasrandomnaivebayes.h"
#include "naivebayeshyperplane.h"
#include "naivebayesfeature.h"
#include <boost/foreach.hpp>

DASRandomNaiveBayes::DASRandomNaiveBayes(const HyperParameters &hp) : RandomNaiveBayes( hp ), m_sim(hp)
{
}

DASRandomNaiveBayes::~DASRandomNaiveBayes()
{
}

void DASRandomNaiveBayes::train(const matrix<float>& dataTr, const std::vector<int>& labelsTr, const matrix<float>& dataTs,
                                const std::vector<int>& labelsTs,bool use_gpu)
{
    // Initialize
    initialize(dataTr.size1());

    if (m_hp.useGPU || use_gpu)
    {
        std::vector<double> weights(labelsTr.size(),1);
        //trainByGPU(data,labels, weights);
        std::cout << "ERROR: Not implemented yet! Jakob, do it!" << std::endl;
    }
    else
        trainByCPU(dataTr,labelsTr,dataTs,labelsTs);

}

void DASRandomNaiveBayes::trainByCPU(const matrix<float>& dataTr, const std::vector<int>& labelsTr,
                                     const matrix<float>& dataTs, const std::vector<int>& labelsTs)
{
    matrix<float> trainConf(dataTr.size1(), m_hp.numClasses);
    std::vector<int> trainPre(dataTr.size1());
    // First round without DA
    bool verbose = m_hp.verbose;
    HyperParameters tmpHP = m_hp;
    tmpHP.verbose = false;
    std::vector<int> outOfBagVoteCount(dataTr.size1(), 0);
    matrix<float> outOfBagConfidences(dataTr.size1(),m_hp.numClasses);

    for ( unsigned int i = 0; i < dataTr.size1(); i++)
    {
        for ( int j = 0; j < m_hp.numClasses; j++)
        {
            outOfBagConfidences(i,j) = 0.0;
        }
    }

    m_naiveBayesClassifiers.clear(); // be sure that they are empty
    for (int i = 0; i < m_hp.numTrees; i++)
    {
        NaiveBayes::Ptr naiveClassifier;
        if (m_hp.useRandProj)
        {
            naiveClassifier = NaiveBayes::Ptr(new NaiveBayesHyperplane(tmpHP));
        }
        else
        {
            naiveClassifier = NaiveBayes::Ptr(new NaiveBayesFeature(tmpHP));
        }
        naiveClassifier->train(dataTr,labelsTr, m_confidences, outOfBagConfidences, outOfBagVoteCount);
        m_naiveBayesClassifiers.push_back(naiveClassifier);
    }

    eval(dataTs,labelsTs);
    double rnbError = computeError(labelsTs);
    cout << "Supervised Error: " << rnbError << endl;

    // Compute OOBE
    double rnbOOBE = computeOOBE(labelsTr);
    double dasrnbOOBE = rnbOOBE;
    if (m_hp.verbose)
    {
        cout << "\tNaive Bayes OOBE = " << rnbOOBE << endl;
    }

    // Make Similarity
    if (m_hp.lambda)
    {
        m_sim = Similarity(m_hp);
        m_sim.train(dataTr, labelsTr);
        m_simConf = m_sim.getSimConf(dataTr, labelsTs);
        //m_simConf = m_sim.getSimConfChi2(dataTr, labelsTs);
    }

    std::vector<std::vector<double> > tmpWeights;
    std::vector<std::vector<int> > tmpLabels;
    std::vector<int> classifiersToBeTrained;
    double numberOfSwitches = 0.0;
    bool init = true;
    int numTrainedClassifiers = 0;
    // Iterative Semi-Supervised Training
    for (int nEpoch = 0; nEpoch < m_hp.numEpochs; nEpoch++)
    {
        cout << "Epoch: " << nEpoch << endl;
        calcLabelsAndWeights(trainConf, trainPre, tmpLabels, tmpWeights, nEpoch, labelsTr, numberOfSwitches, classifiersToBeTrained);

        numTrainedClassifiers = 0;
        BOOST_FOREACH(int nClassifier, classifiersToBeTrained)
        {
            m_naiveBayesClassifiers[nClassifier]->retrain(dataTr,tmpLabels[numTrainedClassifiers], tmpWeights[numTrainedClassifiers], m_confidences, outOfBagConfidences, outOfBagVoteCount,init);
            numTrainedClassifiers++;
        }

        init = false;

        if (numTrainedClassifiers)
        {
            // Evaluate the training set
            m_hp.verbose = false;
            eval(dataTr, labelsTr);
            m_hp.verbose = verbose;
            trainConf = m_confidences;
            trainPre = m_predictions;

            // Compute the OOBE
            dasrnbOOBE = computeOOBE(labelsTr);
            if (m_hp.verbose)
            {
                cout << "\t#Retrained classifiers = " << numTrainedClassifiers << ", #switches/temperature = ";
                cout << numberOfSwitches/(double) numTrainedClassifiers << ", SRF OOBE = " << dasrnbOOBE << "\t";
            }

            eval(dataTs, labelsTs);
            /*
            // Check for the stopping condition
            if (dasrnbOOBE > rnbOOBE + m_hp.maxOOBEIncrease)
            {
                // Reset the classifiers
                m_naiveBayesClassifiers.clear();
                for (int i = 0; i < m_hp.numTrees; i++)
                {
                    Tree t(tmpHP);
                    t.train(dataTr, labelsTr);
                    m_trees.push_back(t);
                }

                // Evaluate the training set
                m_hp.verbose = false;
                eval(dataTr, labelsTr);
                m_hp.verbose = verbose;
                trainConf = m_confidences;
                trainPre = m_predictions;

                rfOOBE = computeOOBE(labelsTr);
                srfOOBE = rfOOBE;
                if (m_hp.verbose)
                {
                    cout << "\tForest OOBE = " << rfOOBE << endl;
                }

                eval(dataTs, labelsTs, false);
                rfError = computeError(labelsTs);

                if (curTrial > numTrials)   // Give back the RF
                {
                    break;
                }
                else   // Make a reset and start the DA again
                {
                    init = true;
                    nEpoch = -1;
                }
            }
            */
        }
        else
        {
            if (m_hp.verbose)
            {
                cout << "\tno naive classifiers were retrained" << endl;
            }
            break;
        }
    }

    eval(dataTs, labelsTs);
    double dasrnbError = computeError(labelsTs);

    if (m_hp.verbose)
    {
        cout << endl << "\tTraining error = " << dasrnbError << endl;
        cout << "\tSemi-supervised improvement = " << 100*(rnbError - dasrnbError)/rnbError << "%" << endl;
    }
}

void DASRandomNaiveBayes::calcLabelsAndWeights(const matrix<float>& RFConf, const std::vector<int>& RFPre,
        std::vector<std::vector<int> >& tmpLabels, std::vector<std::vector<double> >& tmpWeights, const int nEpoch,
        const std::vector<int>& labels, double& numberOfSwitchs,
        std::vector<int>& classifiersToBeTrained)
{
    double p, entropy;
    double alpha = (double) m_hp.numLabeled / ((double) (RFPre.size() - m_hp.numLabeled));
    alpha *= m_hp.alpha;
    bool useEntropy = true;
    std::vector<double> cumSum(m_hp.numClasses);
    tmpLabels.clear();
    tmpWeights.clear();
    classifiersToBeTrained.clear();
    numberOfSwitchs = 0;
    std::vector<int> Y(RFPre.size());
    std::vector<double> W(RFPre.size());
    for (int nSamp = 0; nSamp < m_hp.numLabeled; nSamp++)
    {
        Y[nSamp] = labels[nSamp];
        W[nSamp] = 1.0;
    }

    for (int nTree = 0; nTree < m_hp.numTrees; nTree++)
    {
        if (shouldITrain(nEpoch))   // We are training this tree
        {
            classifiersToBeTrained.push_back(nTree);
            for (int nSamp = m_hp.numLabeled; nSamp < (int) RFPre.size(); nSamp++)
            {
                if (shouldISwitch(nEpoch))
                {
                    numberOfSwitchs++;
                    p = _rand();
                    if (m_hp.lambda)
                    {
                        cumSum[0] = (1 - m_hp.lambda)*RFConf(nSamp, 0) + m_hp.lambda*m_simConf(nSamp - m_hp.numLabeled, 0);
                        for (int m = 1; m < m_hp.numClasses; m++)
                        {
                            cumSum[m] = cumSum[m - 1] + (1 - m_hp.lambda)*RFConf(nSamp, m) + m_hp.lambda*m_simConf(nSamp - m_hp.numLabeled, m);
                        }
                    }
                    else
                    {
                        cumSum[0] = RFConf(nSamp, 0);
                        for (int m = 1; m < m_hp.numClasses; m++)
                        {
                            cumSum[m] = cumSum[m - 1] + RFConf(nSamp, m);
                        }
                    }
                    for (int m = 0; m < m_hp.numClasses; m++)
                    {
                        if (cumSum[m] > p)
                        {
                            Y[nSamp] = m;
                            break;
                        }
                    }
                }
                else   // Pick the prediction of the RF
                {
                    Y[nSamp] = RFPre[nSamp];
                }

                if (useEntropy)
                {
                    entropy = 0;
                    if (m_hp.lambda)
                    {
                        for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
                        {
                            if (RFConf(nSamp, nClass))
                            {
                                entropy -= ((1 - m_hp.lambda)*RFConf(nSamp, nClass) + m_hp.lambda*m_simConf(nSamp - m_hp.numLabeled, nClass))*
                                           log((1 - m_hp.lambda)*RFConf(nSamp, nClass) + m_hp.lambda*m_simConf(nSamp - m_hp.numLabeled, nClass));
                            }
                        }
                    }
                    else
                    {
                        for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
                        {
                            if (RFConf(nSamp, nClass))
                            {
                                entropy -= RFConf(nSamp, nClass)*log(RFConf(nSamp, nClass));
                            }
                        }
                    }

                    W[nSamp] = alpha*(log((double) m_hp.numClasses) - entropy);
                }
                else
                {
                    W[nSamp] = alpha;
                }
            }

            tmpLabels.push_back(Y);
            tmpWeights.push_back(W);
        }
    }
}

double DASRandomNaiveBayes::temperature(const int nEpoch)
{
    double TMax = 100, coolingRate = 0.25;
    double T = pow(coolingRate, (double) nEpoch)*TMax;
    return (T < 1e-6) ? 1e-6 : T;
}

bool DASRandomNaiveBayes::shouldISwitch(const int nEpoch)
{
    double maxP = m_hp.sampMaxP, minP = 0.0, CF = m_hp.sampCF;
    double p = exp(CF*nEpoch);
    p = (p > 0.0) ? p : 0.0;
    if (p > maxP)
    {
        p = maxP;
    }
    if (p < minP)
    {
        p = minP*(1 - (double) nEpoch/(double) m_hp.numEpochs);
    }

    return (_rand() < p) ? true : false;
}

bool DASRandomNaiveBayes::shouldITrain(const int nEpoch)
{
    double maxP = m_hp.treeMaxP, minP = 0.0, CF = m_hp.treeCF;
    double p = exp(CF*nEpoch);
    p = (p > 0.0) ? p : 0.0;
    if (p > maxP)
    {
        p = maxP;
    }
    if (p < minP)
    {
        p = minP*(1 - (double) nEpoch/(double) m_hp.numEpochs);
    }

    return (_rand() < p) ? true : false;
}

double DASRandomNaiveBayes::computeOOBE(const std::vector<int>& labels)
{
    double oobe = 0;
    matrix<float> confidence(m_hp.numLabeled, m_hp.numClasses);
    std::vector<int> voteNum(m_hp.numLabeled, 0), treePre, treeOBS;
    matrix<float> treeConf;
    for (int n = 0; n < m_hp.numLabeled; n++)
    {
        for (int m = 0; m < m_hp.numClasses; m++)
        {
            confidence(n, m) = 0.0;
        }
    }

    BOOST_FOREACH(NaiveBayes::Ptr nb, m_naiveBayesClassifiers)
    {
        treePre = nb->getPredictions();
        treeOBS = nb->getOutOfBagSamples();
        treeConf = nb->getConfidences();
        BOOST_FOREACH(int m, treeOBS)
        {
            if (m < m_hp.numLabeled)
            {
                if (m_hp.useSoftVoting)
                {
                    for (int nClass = 0; nClass < m_hp.numClasses; nClass++)
                    {
                        confidence(m, nClass) += treeConf(m, nClass);
                    }
                }
                else
                {
                    confidence(m, treePre[m])++;
                }
                voteNum[m]++;
            }
            else
            {
                break;
            }
        }
    }

    int bestClass, totalNum = 0;
    double bestConf;
    std::vector<int>::const_iterator labelItr(labels.begin()), labelEnd(labels.end());
    for (int n = 0; labelItr != labelEnd; labelItr++, n++)
    {
        if (n < m_hp.numLabeled && voteNum[n])
        {
            bestClass = 0;
            bestConf = 0;
            for (int m = 0; m < m_hp.numClasses; m++)
            {
                if (confidence(n, m) > bestConf)
                {
                    bestConf = confidence(n, m);
                    bestClass = m;
                }
            }

            if (*labelItr != bestClass)
            {
                oobe++;
            }

            totalNum++;
        }
    }
    oobe /= (double) totalNum;

    return oobe;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_AUTH_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_AUTH_HPP

#include <cassert>
#include <cmath>

#include <boost/geometry/core/assert.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

static const int APA_SIZE = 3;

/* determine latitude from authalic latitude */
template <typename T>
inline bool pj_authset(T const& es, T* APA)
{
    BOOST_GEOMETRY_ASSERT(0 != APA);

    static const T P00 = .33333333333333333333;
    static const T P01 = .17222222222222222222;
    static const T P02 = .10257936507936507936;
    static const T P10 = .06388888888888888888;
    static const T P11 = .06640211640211640211;
    static const T P20 = .01641501294219154443;

    T t = 0;

    // if (APA = (double *)pj_malloc(APA_SIZE * sizeof(double)))
    {
        APA[0] = es * P00;
        t = es * es;
        APA[0] += t * P01;
        APA[1] = t * P10;
        t *= es;
        APA[0] += t * P02;
        APA[1] += t * P11;
        APA[2] = t * P20;
    }
    return true;
}

template <typename T>
inline T pj_authlat(T const& beta, const T* APA)
{
    BOOST_GEOMETRY_ASSERT(0 != APA);

    T const t = beta + beta;

    return(beta + APA[0] * sin(t) + APA[1] * sin(t + t) + APA[2] * sin(t + t + t));
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_AUTH_HPP


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

#include <boost/simd/toolbox/swar/functions/comparator.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/any.hpp>
#include <boost/simd/include/functions/simd/is_not_equal_with_equal_nans.hpp>
#include <boost/fusion/include/vector.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::comparator_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<arithmetic_<A0>,X>))
                             ((simd_<arithmetic_<A0>,X>))
                             ((scalar_<integer_<A1> >))
                             ((simd_<arithmetic_<A0>,X>))
                             ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef bool result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,const A1 & a2, A0 & r0, A0 & r1) const
    {
      if (a2)
      {
        r1 =  boost::simd::min(a0, a1);
        r0 =  boost::simd::max(a0, a1);
      }
      else
      {
        r0 =  boost::simd::min(a0, a1);
        r1 =  boost::simd::max(a0, a1);
      }
      return boost::simd::any(is_not_equal_with_equal_nans(a0, r0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::comparator_, tag::cpu_,
                             (A0)(A1)(X),
                             ((simd_<arithmetic_<A0>,X>))
                             ((simd_<arithmetic_<A0>,X>))
                             ((scalar_<integer_<A1> >))
                            )
  {
    typedef boost::fusion::vector<A0,A0,bool>           result_type;
    inline result_type operator()(A0 const& a0,A0 const & a1,const A1 & a2) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) = boost::simd::comparator(a0, a1, a2, boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }
#endif


#include "IterativeClosestPoint.h"

#include <Eigen/Geometry>

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/BooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/DataContainerSelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/IntFilterParameter.h"
#include "SIMPLib/FilterParameters/StringFilterParameter.h"
#include "SIMPLib/Geometry/VertexGeom.h"

#include "DREAM3DReview/DREAM3DReviewConstants.h"
#include "DREAM3DReview/DREAM3DReviewVersion.h"

#include "DREAM3DReview/DREAM3DReviewFilters/util/nanoflann.hpp"

namespace
{
template <typename Derived>
struct VertexGeomAdaptor
{
  const Derived& obj;

  VertexGeomAdaptor(const Derived& obj_)
  : obj(obj_)
  {
  }

  inline const Derived& derived() const
  {
    return obj;
  }

  inline size_t kdtree_get_point_count() const
  {
    return derived()->getNumberOfVertices();
  }

  inline float kdtree_get_pt(const size_t idx, const size_t dim) const
  {
    if(dim == 0)
    {
      return derived()->getVertexPointer(idx)[0];
    }
    if(dim == 1)
    {
      return derived()->getVertexPointer(idx)[1];
    }

    return derived()->getVertexPointer(idx)[2];
  }

  template <class BBOX>
  bool kdtree_get_bbox(BBOX& /*bb*/) const
  {
    return false;
  }
};
} // namespace

enum createdPathID : RenameDataPath::DataID_t
{
  AttributeMatrixID20 = 20,
  ArrayID21 = 21,
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::IterativeClosestPoint()
{
  initialize();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::~IterativeClosestPoint() = default;
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::initialize()
{
  clearErrorCode();
  clearWarningCode();
  setCancel(false);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setupFilterParameters()
{
  FilterParameterVectorType parameters;
  DataContainerSelectionFilterParameter::RequirementType dcsReq;
  dcsReq.dcGeometryTypes = {IGeometry::Type::Vertex};
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Moving Vertex Geometry", MovingVertexGeometry, FilterParameter::Category::RequiredArray, IterativeClosestPoint, dcsReq));
  parameters.push_back(SIMPL_NEW_DC_SELECTION_FP("Target Vertex Geometry", TargetVertexGeometry, FilterParameter::Category::RequiredArray, IterativeClosestPoint, dcsReq));
  parameters.push_back(SIMPL_NEW_INTEGER_FP("Number of Iterations", Iterations, FilterParameter::Category::Parameter, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_BOOL_FP("Apply Transform to Moving Geometry", ApplyTransform, FilterParameter::Category::Parameter, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_STRING_FP("Transform Attribute Matrix Name", TransformAttributeMatrixName, FilterParameter::Category::CreatedArray, IterativeClosestPoint));
  parameters.push_back(SIMPL_NEW_STRING_FP("Transform Array Name", TransformArrayName, FilterParameter::Category::CreatedArray, IterativeClosestPoint));
  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::dataCheck()
{
  clearErrorCode();
  clearWarningCode();

  getDataContainerArray()->getPrereqGeometryFromDataContainer<VertexGeom>(this, m_MovingVertexGeometry);
  getDataContainerArray()->getPrereqGeometryFromDataContainer<VertexGeom>(this, m_TargetVertexGeometry);

  if(getIterations() < 1)
  {
    setErrorCondition(-1, "Number if iterations must be at least 1");
  }

  DataContainer::Pointer dc = getDataContainerArray()->getPrereqDataContainer(this, m_MovingVertexGeometry);

  if(getErrorCode() < 0)
  {
    return;
  }

  AttributeMatrix::Pointer am = dc->createNonPrereqAttributeMatrix(this, DataArrayPath(m_MovingVertexGeometry.getDataContainerName(), getTransformAttributeMatrixName(), ""), {1},
                                                                   AttributeMatrix::Type::Generic, AttributeMatrixID20);

  if(getErrorCode() < 0)
  {
    return;
  }

  am->createNonPrereqArray<DataArray<float>>(this, getTransformArrayName(), 0, {4, 4}, ArrayID21);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::execute()
{
  initialize();
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  VertexGeom::Pointer moving = getDataContainerArray()->getDataContainer(m_MovingVertexGeometry.getDataContainerName())->getGeometryAs<VertexGeom>();
  VertexGeom::Pointer movingCopy = std::dynamic_pointer_cast<VertexGeom>(moving->deepCopy());
  VertexGeom::Pointer target = getDataContainerArray()->getDataContainer(m_TargetVertexGeometry.getDataContainerName())->getGeometryAs<VertexGeom>();

  float* movingPtr = moving->getVertexPointer(0);
  float* movingCopyPtr = movingCopy->getVertexPointer(0);
  float* targetPtr = target->getVertexPointer(0);

  size_t numMovingVerts = moving->getNumberOfVertices();
  std::vector<size_t> cDims(1, 3);
  FloatArrayType::Pointer dynTarget = FloatArrayType::CreateArray(numMovingVerts, cDims, "tmp", true);
  dynTarget->initializeWithZeros();
  float* dynTargetPtr = dynTarget->getPointer(0);

  using Adaptor = VertexGeomAdaptor<VertexGeom::Pointer>;
  const Adaptor adaptor(target);

  notifyStatusMessage("Building kd-tree index...");

  using KDtree = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Adaptor<float, Adaptor>, Adaptor, 3>;
  KDtree index(3, adaptor, nanoflann::KDTreeSingleIndexAdaptorParams(30));
  index.buildIndex();

  size_t iters = m_Iterations;
  const size_t nn = 1;

  typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::ColMajor> PointCloud;
  typedef Eigen::Matrix<float, 4, 4, Eigen::ColMajor> UmeyamaTransform;

  UmeyamaTransform globalTransform;
  globalTransform << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;

  int64_t progIncrement = iters / 100;
  int64_t prog = 1;
  int64_t progressInt = 0;
  int64_t counter = 0;

  for(size_t i = 0; i < iters; i++)
  {
    if(getCancel())
    {
      return;
    }

    for(size_t j = 0; j < numMovingVerts; j++)
    {
      size_t id;
      float dist;
      nanoflann::KNNResultSet<float> results(nn);
      results.init(&id, &dist);
      index.findNeighbors(results, movingCopyPtr + (3 * j), nanoflann::SearchParams());
      dynTargetPtr[3 * j + 0] = targetPtr[3 * id + 0];
      dynTargetPtr[3 * j + 1] = targetPtr[3 * id + 1];
      dynTargetPtr[3 * j + 2] = targetPtr[3 * id + 2];
    }

    Eigen::Map<PointCloud> moving_(movingCopyPtr, 3, numMovingVerts);
    Eigen::Map<PointCloud> target_(dynTargetPtr, 3, numMovingVerts);

    UmeyamaTransform transform = Eigen::umeyama(moving_, target_, false);

    for(size_t j = 0; j < numMovingVerts; j++)
    {
      Eigen::Vector4f position(movingCopyPtr[3 * j + 0], movingCopyPtr[3 * j + 1], movingCopyPtr[3 * j + 2], 1);
      Eigen::Vector4f transformedPosition = transform * position;
      std::memcpy(movingCopyPtr + (3 * j), transformedPosition.data(), sizeof(float) * 3);
    }

    globalTransform = transform * globalTransform;

    if(counter > prog)
    {
      progressInt = static_cast<int64_t>((static_cast<float>(counter) / iters) * 100.0f);
      QString ss = QObject::tr("Performing Registration Iterations || %1% Completed").arg(progressInt);
      notifyStatusMessage(ss);
      prog = prog + progIncrement;
    }
    counter++;
  }

  float* transformPtr = getDataContainerArray()
                            ->getDataContainer(m_MovingVertexGeometry.getDataContainerName())
                            ->getAttributeMatrix(m_TransformAttributeMatrixName)
                            ->getAttributeArrayAs<DataArray<float>>(m_TransformArrayName)
                            ->getPointer(0);

  if(m_ApplyTransform)
  {
    for(size_t j = 0; j < numMovingVerts; j++)
    {
      Eigen::Vector4f position(movingPtr[3 * j + 0], movingPtr[3 * j + 1], movingPtr[3 * j + 2], 1);
      Eigen::Vector4f transformedPosition = globalTransform * position;
      std::memcpy(movingPtr + (3 * j), transformedPosition.data(), sizeof(float) * 3);
    }
  }

  globalTransform.transposeInPlace();
  std::memcpy(transformPtr, globalTransform.data(), sizeof(float) * 16);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer IterativeClosestPoint::newFilterInstance(bool copyFilterParameters) const
{
  IterativeClosestPoint::Pointer filter = IterativeClosestPoint::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getCompiledLibraryName() const
{
  return DREAM3DReviewConstants::DREAM3DReviewBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getBrandingString() const
{
  return "DREAM3DReview";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << DREAM3DReview::Version::Major() << "." << DREAM3DReview::Version::Minor() << "." << DREAM3DReview::Version::Patch();
  return version;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getGroupName() const
{
  return SIMPL::FilterGroups::ReconstructionFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getSubGroupName() const
{
  return SIMPL::FilterSubGroups::AlignmentFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getHumanLabel() const
{
  return "Iterative Closest Point";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid IterativeClosestPoint::getUuid() const
{
  return QUuid("{6c8fb24b-5b12-551c-ba6d-ae2fa7724764}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
IterativeClosestPoint::Pointer IterativeClosestPoint::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
std::shared_ptr<IterativeClosestPoint> IterativeClosestPoint::New()
{
  struct make_shared_enabler : public IterativeClosestPoint
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getNameOfClass() const
{
  return QString("IterativeClosestPoint");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::ClassName()
{
  return QString("IterativeClosestPoint");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setMovingVertexGeometry(const DataArrayPath& value)
{
  m_MovingVertexGeometry = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DataArrayPath IterativeClosestPoint::getMovingVertexGeometry() const
{
  return m_MovingVertexGeometry;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTargetVertexGeometry(const DataArrayPath& value)
{
  m_TargetVertexGeometry = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
DataArrayPath IterativeClosestPoint::getTargetVertexGeometry() const
{
  return m_TargetVertexGeometry;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setIterations(const int& value)
{
  m_Iterations = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
int IterativeClosestPoint::getIterations() const
{
  return m_Iterations;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setApplyTransform(const bool& value)
{
  m_ApplyTransform = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
bool IterativeClosestPoint::getApplyTransform() const
{
  return m_ApplyTransform;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTransformAttributeMatrixName(const QString& value)
{
  m_TransformAttributeMatrixName = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getTransformAttributeMatrixName() const
{
  return m_TransformAttributeMatrixName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void IterativeClosestPoint::setTransformArrayName(const QString& value)
{
  m_TransformArrayName = value;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString IterativeClosestPoint::getTransformArrayName() const
{
  return m_TransformArrayName;
}


#include "drake/solvers/moby_lcp_solver.h"

#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <vector>

#include <Eigen/LU>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

#include "drake/common/autodiff.h"
#include "drake/common/drake_assert.h"
#include "drake/common/never_destroyed.h"
#include "drake/common/text_logging.h"

namespace drake {
namespace solvers {

namespace {

template <typename Scalar>
bool CheckLemkeTrivial(int n, const Scalar& zero_tol, const VectorX<Scalar>& q,
                       VectorX<Scalar>* z) {
  // see whether trivial solution exists
  if (q.minCoeff() > -zero_tol) {
    z->resize(n);
    z->fill(0);
    return true;
  }

  return false;
}

// AutoDiff-supported linear system solver for performing principle pivoting
// transformations. The matrix is supposed to be a linear basis, but it's
// possible that the basis becomes degenerate (meaning that the matrix becomes
// singular) due to accumulated roundoff error from pivoting. Recovering from
// a degenerate basis is currently an open problem;
// see http://www.optimization-online.org/DB_FILE/2011/03/2948.pdf, for
// example. The caller would ideally terminate at this point, but
// compilation of householderQr().rank() with AutoDiff currently generates
// template errors. Continuing on blindly means that the calling pivoting
// algorithm might continue on for some time.
template <class T>
VectorX<T> LinearSolve(const MatrixX<T>& M, const VectorX<T>& b) {
  // Special case necessary because Eigen doesn't always handle empty matrices
  // properly.
  if (M.rows() == 0) {
    DRAKE_ASSERT(b.size() == 0);
    return VectorX<T>(0);
  }
  return M.householderQr().solve(b);
}

// Linear system solver, specialized for double types. This method is faster
// than the QR factorization necessary for AutoDiff support. It is assumed that
// the matrix is full rank (see notes for generic LinearSolve() above).
template <>
VectorX<double> LinearSolve(
    const MatrixX<double>& M, const VectorX<double>& b) {
  // Special case necessary because Eigen doesn't always handle empty matrices
  // properly.
  if (M.rows() == 0) {
    DRAKE_ASSERT(b.size() == 0);
    return VectorX<double>(0);
  }
  return M.partialPivLu().solve(b);
}

// Utility function for copying part of a matrix (designated by the indices
// in rows and cols) from in to a target matrix, out. This template approach
// allows selecting parts of both sparse and dense matrices for input; only
// a dense matrix is returned.
template <typename Derived, typename T>
void selectSubMat(const Eigen::MatrixBase<Derived>& in,
                  const std::vector<unsigned>& rows,
                  const std::vector<unsigned>& cols, MatrixX<T>* out) {
  const int num_rows = rows.size();
  const int num_cols = cols.size();
  out->resize(num_rows, num_cols);

  for (int i = 0; i < num_rows; i++) {
    const auto row_in = in.row(rows[i]);
    auto row_out = out->row(i);
    for (int j = 0; j < num_cols; j++) {
      row_out(j) = row_in(cols[j]);
    }
  }
}

// TODO(sammy-tri) this could also use a more efficient implementation.
template <typename T>
void selectSubVec(const VectorX<T>& in,
                  const std::vector<unsigned>& rows, VectorX<T>* out) {
  const int num_rows = rows.size();
  out->resize(num_rows);
  for (int i = 0; i < num_rows; i++) {
    (*out)(i) = in(rows[i]);
  }
}

template <typename Derived>
Eigen::SparseVector<double> makeSparseVector(
    const Eigen::MatrixBase<Derived>& in) {
  DRAKE_ASSERT(in.cols() == 1);
  Eigen::SparseVector<double> out(in.rows());
  for (int i = 0; i < in.rows(); i++) {
    if (in(i) != 0.0) {
      out.coeffRef(i) = in(i);
    }
  }
  return out;
}

template <typename Derived>
Eigen::Index minCoeffIdx(const Eigen::MatrixBase<Derived>& in) {
  Eigen::Index idx;
  in.minCoeff(&idx);
  return idx;
}

const double kSqrtEps = std::sqrt(std::numeric_limits<double>::epsilon());
}  // anonymous namespace

template <typename T>
void MobyLCPSolver<T>::SetLoggingEnabled(bool enabled) {
  log_enabled_ = enabled; }

template <typename T>
std::ostream& MobyLCPSolver<T>::Log() const {
  if (log_enabled_) {
    return std::cerr;
  }
  return null_stream_;
}

template <typename T>
void MobyLCPSolver<T>::ClearIndexVectors() const {
  // clear all vectors
  all_.clear();
  tlist_.clear();
  bas_.clear();
  nonbas_.clear();
  j_.clear();
}

template <>
void MobyLCPSolver<Eigen::AutoDiffScalar<Vector1d>>::DoSolve(
    const MathematicalProgram&, const Eigen::VectorXd&,
    const SolverOptions&, MathematicalProgramResult*) const {
  throw std::logic_error(
      "MobyLCPSolver cannot yet be used in a MathematicalProgram "
      "while templatized as an AutoDiff");
}

// TODO(edrumwri): Break the following code out into a special
// MobyLcpMathematicalProgram class.
template <typename T>
void MobyLCPSolver<T>::DoSolve(
    const MathematicalProgram& prog,
    const Eigen::VectorXd& initial_guess,
    const SolverOptions& merged_options,
    MathematicalProgramResult* result) const {
  if (!prog.GetVariableScaling().empty()) {
    static const logging::Warn log_once(
      "MobyLCPSolver doesn't support the feature of variable scaling.");
  }

  // Moby doesn't use initial guess or the solver options.
  unused(initial_guess);
  unused(merged_options);

  // Solve each individual LCP, writing the result back to the decision
  // variables through the binding and returning true iff all LCPs are
  // feasible.
  //
  // If any is infeasible, returns false and does not alter the decision
  // variables.
  //
  // TODO(ggould-tri) This could also be solved by constructing a single large
  // square matrix and vector, and then copying the elements of the individual
  // Ms and qs into the appropriate places.  That would be equivalent to this
  // implementation but might perform better if the solver were to parallelize
  // internally.

  const auto& bindings = prog.linear_complementarity_constraints();
  Eigen::VectorXd x_sol(prog.num_vars());
  for (const auto& binding : bindings) {
    Eigen::VectorXd constraint_solution(binding.GetNumElements());
    const std::shared_ptr<LinearComplementarityConstraint> constraint =
        binding.evaluator();
    bool solved = SolveLcpLemkeRegularized(
        constraint->M(), constraint->q(), &constraint_solution);
    if (!solved) {
      result->set_solution_result(SolutionResult::kUnknownError);
      return;
    }
    for (int i = 0; i < binding.evaluator()->num_vars(); ++i) {
      const int variable_index =
          prog.FindDecisionVariableIndex(binding.variables()(i));
      x_sol(variable_index) = constraint_solution(i);
    }
  }
  result->set_optimal_cost(0.0);
  result->set_x_val(x_sol);
  result->set_solution_result(SolutionResult::kSolutionFound);
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpFast(const MatrixX<T>& M,
                                    const VectorX<T>& q, VectorX<T>* z,
                                    const T& zero_tol) const {
  using std::abs;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> zz, w, qbas;
  MatrixX<T> Mmix, Msub;

  const unsigned N = q.rows();
  const unsigned UINF = std::numeric_limits<unsigned>::max();

  if (M.rows() != N || M.cols() != N)
    throw std::logic_error("M's dimensions do not match that of q.");

  Log() << "MobyLCPSolver::SolveLcpFast() entered" << std::endl;

  // look for trivial solution
  if (N == 0) {
    Log() << "MobyLCPSolver::SolveLcpFast() - empty problem" << std::endl;
    z->resize(0);
    return true;
  }

  // set zero tolerance if necessary
  T mod_zero_tol = zero_tol;
  if (mod_zero_tol < 0)
    mod_zero_tol = ComputeZeroTolerance(M);

  // prepare to setup basic and nonbasic variable indices for z
  nonbas_.clear();
  bas_.clear();

  // see whether to warm-start
  if (z->size() == q.size()) {
    Log() << "MobyLCPSolver::SolveLcpFast() - warm starting activated"
          << std::endl;

    for (unsigned i = 0; i < z->size(); i++) {
      if (abs((*z)[i]) < mod_zero_tol) {
        bas_.push_back(i);
      } else {
        nonbas_.push_back(i);
      }
    }

    if (log_enabled_) {
      std::ostringstream str;
      str << " -- non-basic indices:";
      for (unsigned i = 0; i < nonbas_.size(); i++) str << " " << nonbas_[i];
      Log() << str.str() << std::endl;
    }
  } else {
    // get minimum element of q (really w)
    Eigen::Index minw;
    const T minw_val = q.minCoeff(&minw);
    if (minw_val > -mod_zero_tol) {
      Log() << "MobyLCPSolver::SolveLcpFast() - trivial solution found"
            << std::endl;
      z->resize(N);
      z->fill(0);
      return true;
    }

    // setup basic and nonbasic variable indices
    nonbas_.push_back(minw);
    bas_.resize(N - 1);
    for (unsigned i = 0, j = 0; i < N; i++) {
      if (i != minw) {
        bas_[j++] = i;
      }
    }
  }

  // Loop for the maximum number of pivots.
  const unsigned MAX_PIV = 2 * N;
  for (pivots_ = 1; pivots_ <= MAX_PIV; pivots_++) {
    // select nonbasic indices
    selectSubMat(M, nonbas_, nonbas_, &Msub);
    selectSubMat(M, bas_, nonbas_, &Mmix);
    selectSubVec(q, nonbas_, &zz);
    selectSubVec(q, bas_, &qbas);
    zz *= -1;

    // Solve for nonbasic z.
    zz = LinearSolve(Msub, zz.eval());

    // Eigen doesn't handle empty matrices properly, which causes the code
    // below to abort in the absence of the conditional.
    unsigned minw;
    if (Mmix.rows() == 0) {
      w = VectorX<T>();
      minw = UINF;
    } else {
      w = Mmix * zz;
      w += qbas;
      minw = minCoeffIdx(w);
    }

    // TODO(sammy-tri) this log can't print when minw is UINF.
    // LOG() << "MobyLCPSolver::SolveLcpFast() - minimum w after pivot: "
    // << _w[minw] << std::endl;

    // if w >= 0, check whether any component of z < 0
    if (minw == UINF || w[minw] > -mod_zero_tol) {
      // find the (a) minimum of z
      unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF;
      if (log_enabled_ && zz.rows() > 0) {
        Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: "
              << zz[minz] << std::endl;
      }
      if (minz < UINF && zz[minz] < -mod_zero_tol) {
        // get the original index and remove it from the nonbasic set
        unsigned idx = nonbas_[minz];
        nonbas_.erase(nonbas_.begin() + minz);

        // move index to basic set and continue looping
        bas_.push_back(idx);
        std::sort(bas_.begin(), bas_.end());
      } else {
        // found the solution
        z->resize(N);
        z->fill(0);

        // set values of z corresponding to _z
        for (unsigned i = 0, j = 0; j < nonbas_.size(); i++, j++) {
          (*z)[nonbas_[j]] = zz[i];
        }

        Log() << "MobyLCPSolver::SolveLcpFast() - solution found!" << std::endl;
        return true;
      }
    } else {
      Log() << "(minimum w too negative)" << std::endl;

      // one or more components of w violating w >= 0
      // move component of w from basic set to nonbasic set
      unsigned idx = bas_[minw];
      bas_.erase(bas_.begin() + minw);
      nonbas_.push_back(idx);
      std::sort(nonbas_.begin(), nonbas_.end());

      // look whether any component of z needs to move to basic set
      unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF;
      if (log_enabled_ && zz.rows() > 0) {
        Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: "
              << zz[minz] << std::endl;
      }
      if (minz < UINF && zz[minz] < -mod_zero_tol) {
        // move index to basic set and continue looping
        unsigned k = nonbas_[minz];
        Log() << "MobyLCPSolver::SolveLcpFast() - moving index " << k
              << " to basic set" << std::endl;

        nonbas_.erase(nonbas_.begin() + minz);
        bas_.push_back(k);
        std::sort(bas_.begin(), bas_.end());
      }
    }
  }

  Log() << "MobyLCPSolver::SolveLcpFast() - maximum allowable pivots exceeded"
        << std::endl;

  // if we're here, then the maximum number of pivots has been exceeded
  z->setZero(N);
  return false;
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpFastRegularized(const MatrixX<T>& M,
                                               const VectorX<T>& q,
                                               VectorX<T>* z, int min_exp,
                                               unsigned step_exp, int max_exp,
                                               const T& zero_tol) const {
  Log() << "MobyLCPSolver::SolveLcpFastRegularized() entered" << std::endl;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> wx;
  MatrixX<T> MM;

  // look for fast exit
  if (q.size() == 0) {
    z->resize(0);
    return true;
  }

  // copy MM
  MM = M;

  // A discourse on the zero tolerance in the context of regularization:
  // The zero tolerance is used to determine when an element of w or z is
  // effectively zero though its floating point value is negative. The question
  // is whether the regularization process will change the zero tolerance
  // necessary to solve the problem numerically. In such a case, the infinity
  // norm of M would be small while the infinity norm of MM (regularized M)
  // would be large. Consider the case of a symmetric, indefinite matrix with
  // maximum and minimum eigenvalues of a and -a, respectively. The matrix could
  // be made positive definite (and thereby guaranteed to possess a solution to
  // the linear complementarity problem) by adding an identity matrix times
  // (a+ε) to the LCP matrix, where ε > 0 (its magnitude will depend upon the
  // magnitude of a). The infinity norm (and hence the zero tolerance) could
  // then be expected to grow by a factor of approximately two during the
  // regularization process. In other words, recomputing the zero tolerance
  // for each regularization update to the LCP matrix appears wasteful. For
  // this reason, we compute it only once below, but a practical effect is
  // not discernible at this time.

  // Assign value for zero tolerance, if necessary.
  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  Log() << " zero tolerance: " << mod_zero_tol << std::endl;

  // store the total pivots
  unsigned total_piv = 0;

  // try non-regularized version first
  bool result = SolveLcpFast(MM, q, z, mod_zero_tol);
  if (result) {
    // verify that solution truly is a solution -- check z
    if (z->minCoeff() >= -mod_zero_tol) {
      // check w
      wx = (M * (*z)) + q;
      if (wx.minCoeff() >= -mod_zero_tol) {
        // Check element-wise operation of z*wx.
        wx = z->array() * wx.eval().array();
        const T wx_min = wx.minCoeff();
        const T wx_max = wx.maxCoeff();

        if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) {
          Log() << "  solved with no regularization necessary!" << std::endl;
          Log() << "  pivots / total pivots: " << pivots_ << " " << pivots_
                << std::endl;
          Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited"
                << std::endl;

          return true;
        } else {
          Log() << "MobyLCPSolver::SolveLcpFastRegularized() - "
                << "'<w, z> not within tolerance(min value: " << wx_min
                << " max value: " << wx_max << ")" << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
              << "'w' not solved to desired tolerance" << std::endl;
        Log() << "  minimum w: " << wx.minCoeff() << std::endl;
      }
    } else {
      Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
            << "'z' not solved to desired tolerance" << std::endl;
      Log() << "  minimum z: " << z->minCoeff() << std::endl;
    }
  } else {
    Log() << "  MobyLCPSolver::SolveLcpFastRegularized() "
          << "- solver failed with zero regularization" << std::endl;
  }

  // update the pivots
  total_piv += pivots_;

  // start the regularization process
  int rf = min_exp;
  while (rf < max_exp) {
    // setup regularization factor
    double lambda =
        std::pow(static_cast<double>(10.0), static_cast<double>(rf));

    Log() << "  trying to solve LCP with regularization factor: " << lambda
          << std::endl;

    // regularize M
    MM = M;
    for (unsigned i = 0; i < M.rows(); i++) {
      MM(i, i) += lambda;
    }

    // try to solve the LCP
    result = SolveLcpFast(MM, q, z, mod_zero_tol);

    // update total pivots
    total_piv += pivots_;

    if (result) {
      // verify that solution truly is a solution -- check z
      if (z->minCoeff() > -mod_zero_tol) {
        // check w
        wx = (MM * (*z)) + q;
        if (wx.minCoeff() > -mod_zero_tol) {
          // Check element-wise operation of z*wx.
          wx = z->array() * wx.eval().array();
          const T wx_min = wx.minCoeff();
          const T wx_max = wx.maxCoeff();

          if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) {
            Log() << "  solved with regularization factor: " << lambda
                  << std::endl;
            Log() << "  pivots / total pivots: " << pivots_ << " " << total_piv
                  << std::endl;
            Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited"
                  << std::endl;
            pivots_ = total_piv;
            return true;
          } else {
            Log() << "MobyLCPSolver::SolveLcpFastRegularized() - "
                  << "'<w, z> not within tolerance(min value: " << wx_min
                  << " max value: " << wx_max << ")" << std::endl;
          }
        } else {
          Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
                << "'w' not solved to desired tolerance" << std::endl;
          Log() << "  minimum w: " << wx.minCoeff() << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpFastRegularized() - "
              << "'z' not solved to desired tolerance" << std::endl;
        Log() << "  minimum z: " << z->minCoeff() << std::endl;
      }
    }

    // increase rf
    rf += step_exp;
  }

  Log() << "  unable to solve given any regularization!" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited" << std::endl;

  // store total pivots
  pivots_ = total_piv;

  // still here?  failure...
  return false;
}

// Retrieves the solution computed by Lemke's Algorithm.
// T is irrelevant for this method (necessary only for the member function).
// MatrixType allows both dense and sparse matrices to be used.
// Scalar allows this method to be used for when the T is AutoDiffXd but
// the caller wants to use sparse methods.
// TODO(edrumwri): Address this kludge when calling sparse LCP solves from
//                 MobyLCPSolver<AutoDiffXd> has been prevented.
template <typename T>
template <typename MatrixType, typename Scalar>
void MobyLCPSolver<T>::FinishLemkeSolution(const MatrixType& M,
                                           const VectorX<Scalar>& q,
                                           const VectorX<Scalar>& x,
                                           VectorX<Scalar>* z) const {
  using std::abs;
  using std::max;
  std::vector<unsigned>::iterator iiter;
  int idx;
  for (idx = 0, iiter = bas_.begin(); iiter != bas_.end(); iiter++, idx++) {
    (*z)(*iiter) = x(idx);
  }

  // TODO(sammy-tri) Is there a more efficient way to resize and
  // preserve the data?
  z->conservativeResize(q.size());

  // check to see whether tolerances are satisfied
  if (log_enabled_) {
    VectorX<T> wl = (M * (*z)) + q;
    const T minw = wl.minCoeff();
    const T w_dot_z = abs(wl.dot(*z));
    Log() << "  z: " << z << std::endl;
    Log() << "  w: " << wl << std::endl;
    Log() << "  minimum w: " << minw << std::endl;
    Log() << "  w'z: " << w_dot_z << std::endl;
  }
}

template <typename T>
bool MobyLCPSolver<T>::SolveLcpLemke(const MatrixX<T>& M,
                                     const VectorX<T>& q, VectorX<T>* z,
                                     const T& piv_tol,
                                     const T& zero_tol) const {
  using std::max;

  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> result, dj, dl, x, xj, Be, u, z0;
  MatrixX<T> Bl, t1, t2;

  if (log_enabled_) {
    Log() << "MobyLCPSolver::SolveLcpLemke() entered" << std::endl;
    Log() << "  M: " << std::endl << M;
    Log() << "  q: " << q << std::endl;
  }

  const unsigned n = q.size();
  const unsigned max_iter = std::min(unsigned{1000}, 50 * n);

  if (M.rows() != n || M.cols() != n)
    throw std::logic_error("M's dimensions do not match that of q.");

  // update the pivots
  pivots_ = 0;

  // look for immediate exit
  if (n == 0) {
    z->resize(0);
    return true;
  }

  // come up with a sensible value for zero tolerance if none is given
  T mod_zero_tol = zero_tol;
  if (mod_zero_tol <= 0)
    mod_zero_tol = ComputeZeroTolerance(M);

  if (CheckLemkeTrivial(n, mod_zero_tol, q, z)) {
    Log() << " -- trivial solution found" << std::endl;
    Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
    return true;
  }

  // Lemke's algorithm doesn't seem to like warmstarting
  //
  // TODO(sammy-tri) this is not present in the sparse solver, and it
  // causes subtle dead code below.
  z->fill(0);

  // copy z to z0
  z0 = *z;

  ClearIndexVectors();

  // initialize variables
  z->resize(n * 2);
  z->fill(0);
  unsigned t = 2 * n;
  unsigned entering = t;
  unsigned leaving = 0;
  for (unsigned i = 0; i < n; i++) {
    all_.push_back(i);
  }
  unsigned lvindex;
  unsigned idx;
  std::vector<unsigned>::iterator iiter;

  // determine initial basis
  if (z0.size() != n) {
    // setup the nonbasic indices
    for (unsigned i = 0; i < n; i++) nonbas_.push_back(i);
  } else {
    for (unsigned i = 0; i < n; i++) {
      if (z0[i] > 0) {
        bas_.push_back(i);
      } else {
        nonbas_.push_back(i);
      }
    }
  }

  // determine initial values
  if (!bas_.empty()) {
    Log() << "-- initial basis not empty (warmstarting)" << std::endl;

    // start from good initial basis
    Bl.resize(n, n);
    Bl.setIdentity();
    Bl *= -1;

    // select columns of M corresponding to z vars in the basis
    selectSubMat(M, all_, bas_, &t1);

    // select columns of I corresponding to z vars not in the basis
    selectSubMat(Bl, all_, nonbas_, &t2);

    // setup the basis matrix
    Bl.resize(n, t1.cols() + t2.cols());
    Bl.block(0, 0, t1.rows(), t1.cols()) = t1;
    Bl.block(0, t1.cols(), t2.rows(), t2.cols()) = t2;

    // Solve B*x = -q.
    x = LinearSolve(Bl, q);
  } else {
    Log() << "-- using basis of -1 (no warmstarting)" << std::endl;

    // use standard initial basis
    Bl.resize(n, n);
    Bl.setIdentity();
    Bl *= -1;
    x = q;
  }

  // check whether initial basis provides a solution
  if (x.minCoeff() >= 0.0) {
    Log() << " -- initial basis provides a solution!" << std::endl;
    FinishLemkeSolution(M, q, x, z);
    Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
    return true;
  }

  // use a new pivot tolerance if necessary
  const T naive_piv_tol = n * max(T(1), M.template lpNorm<Eigen::Infinity>()) *
      std::numeric_limits<double>::epsilon();
  const T mod_piv_tol = (piv_tol > 0) ? piv_tol : naive_piv_tol;

  // determine initial leaving variable
  Eigen::Index min_x;
  const T min_x_val = x.topRows(n).minCoeff(&min_x);
  const T tval = -min_x_val;
  for (size_t i = 0; i < nonbas_.size(); i++) {
    bas_.push_back(nonbas_[i] + n);
  }
  lvindex = min_x;
  iiter = bas_.begin();
  std::advance(iiter, lvindex);
  leaving = *iiter;
  Log() << " -- x: " << x << std::endl;
  Log() << " -- first pivot: leaving index=" << lvindex
        << "  entering index=" << entering << " minimum value: " << tval
        << std::endl;

  // pivot in the artificial variable
  *iiter = t;  // replace w var with _z0 in basic indices
  u.resize(n);
  for (unsigned i = 0; i < n; i++) {
    u[i] = (x[i] < 0) ? 1 : 0;
  }
  Be = (Bl * u) * -1;
  u *= tval;
  x += u;
  x[lvindex] = tval;
  Bl.col(lvindex) = Be;
  Log() << "  new q: " << x << std::endl;

  // main iterations begin here
  for (pivots_ = 0; pivots_ < max_iter; pivots_++) {
    if (log_enabled_) {
      std::ostringstream basic;
      for (unsigned i = 0; i < bas_.size(); i++) {
        basic << " " << bas_[i];
      }
      Log() << "basic variables:" << basic.str() << std::endl;
      Log() << "leaving: " << leaving << " t:" << t << std::endl;
    }

    // check whether done; if not, get new entering variable
    if (leaving == t) {
      Log() << "-- solved LCP successfully!" << std::endl;
      FinishLemkeSolution(M, q, x, z);
      Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
      return true;
    } else if (leaving < n) {
      entering = n + leaving;
      Be.resize(n);
      Be.fill(0);
      Be[leaving] = -1;
    } else {
      entering = leaving - n;
      Be = M.col(entering);
    }
    dl = Be;

    // See comments above on the possibility of this solve failing.
    dl = LinearSolve(Bl, dl.eval());

    // ** find new leaving variable
    j_.clear();
    for (unsigned i = 0; i < dl.size(); i++) {
      if (dl[i] > mod_piv_tol) {
        j_.push_back(i);
      }
    }

    // check for no new pivots; ray termination
    if (j_.empty()) {
      Log()
          << "MobyLCPSolver::SolveLcpLemke() - no new pivots (ray termination)"
          << std::endl;
      Log() << "MobyLCPSolver::SolveLcpLemke() exiting" << std::endl;
      z->setZero(n);
      return false;
    }

    if (log_enabled_) {
      std::ostringstream j;
      for (unsigned i = 0; i < j_.size(); i++) j << " " << j_[i];
      Log() << "d: " << dl << std::endl;
      Log() << "j (before min ratio):" << j.str() << std::endl;
    }

    // select elements j from x and d
    selectSubVec(x, j_, &xj);
    selectSubVec(dl, j_, &dj);

    // compute minimal ratios x(j) + EPS_DOUBLE ./ d(j), d > 0
    result.resize(xj.size());
    result.fill(mod_zero_tol);
    result = xj.eval().array() + result.array();
    result = result.eval().array() / dj.array();
    const T theta = result.minCoeff();

    // NOTE: lexicographic ordering is not used here to prevent
    // cycling (see [Cottle 1992], pp. 340-342). Cycling is indirectly prevented
    // by (a) limiting the maximum number of pivots and (b) using
    // regularized solvers, as necessary. In other words, cycling may cause
    // solver to fail when the LCP is theoretically solvable, but wrapping the
    // solver with regularization practically addresses the problem (albeit,
    // at the cost of additional computation).

    // find indices of minimal ratios, d> 0
    //   divide _x(j) ./ d(j) -- remove elements above the minimum ratio
    for (int i = 0; i < result.size(); i++) {
      result(i) = xj(i) / dj(i);
    }

    for (iiter = j_.begin(), idx = 0; iiter != j_.end();) {
      if (result[idx++] <= theta) {
        iiter++;
      } else {
        iiter = j_.erase(iiter);
      }
    }
    if (log_enabled_) {
      std::ostringstream j;
      for (unsigned i = 0; i < j_.size(); i++) {
        j << " " << j_[i];
      }
      Log() << "j (after min ratio):" << j.str() << std::endl;
    }

    // if j is empty, then likely the zero tolerance is too low
    if (j_.empty()) {
      Log() << "zero tolerance too low?" << std::endl;
      Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
      z->setZero(n);
      return false;
    }

    // check whether artificial index among these
    tlist_.clear();
    for (size_t i = 0; i < j_.size(); i++) {
      tlist_.push_back(bas_[j_[i]]);
    }
    if (std::find(tlist_.begin(), tlist_.end(), t) != tlist_.end()) {
      iiter = std::find(bas_.begin(), bas_.end(), t);
      lvindex = iiter - bas_.begin();
    } else {
      // several indices pass the minimum ratio test, pick one randomly
      //      lvindex = _j[rand() % _j.size()];
      // NOTE: solver seems *much* more capable of solving when we pick the
      // first
      // element rather than picking a random one
      lvindex = j_[0];
    }

    // set leaving = bas(lvindex)
    iiter = bas_.begin();
    std::advance(iiter, lvindex);
    leaving = *iiter;

    // ** perform pivot
    const T ratio = x[lvindex] / dl[lvindex];
    dl *= ratio;
    x -= dl;
    x[lvindex] = ratio;
    Bl.col(lvindex) = Be;
    *iiter = entering;
    Log() << " -- pivoting: leaving index=" << lvindex
          << "  entering index=" << entering << std::endl;
  }

  Log() << " -- maximum number of iterations exceeded (n=" << n
        << ", max=" << max_iter << ")" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl;
  z->setZero(n);
  return false;
}

template <class T>
bool MobyLCPSolver<T>::SolveLcpLemkeRegularized(const MatrixX<T>& M,
                                                const VectorX<T>& q,
                                                VectorX<T>* z, int min_exp,
                                                unsigned step_exp, int max_exp,
                                                const T& piv_tol,
                                                const T& zero_tol) const {
  // Variables that will be reused multiple times, thus hopefully allowing
  // Eigen to keep from freeing/reallocating memory repeatedly.
  VectorX<T> wx;

  Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() entered" << std::endl;

  // look for fast exit
  if (q.size() == 0) {
    z->resize(0);
    return true;
  }

  // copy MM
  MatrixX<T> MM = M;

  // Assign value for zero tolerance, if necessary. See discussion in
  // SolveLcpFastRegularized() to see why this tolerance is computed here once,
  // rather than for each regularized version of M.
  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  Log() << " zero tolerance: " << mod_zero_tol << std::endl;

  // store the total pivots
  unsigned total_piv = 0;

  // try non-regularized version first
  bool result = SolveLcpLemke(MM, q, z, piv_tol, mod_zero_tol);
  if (result) {
    // verify that solution truly is a solution -- check z
    if (z->minCoeff() >= -mod_zero_tol) {
      // check w
      wx = (M * (*z)) + q;
      if (wx.minCoeff() >= -mod_zero_tol) {
        // Check element-wise operation of z*wx.
        wx = z->array() * wx.eval().array();

        const T wx_min = wx.minCoeff();
        const T wx_max = wx.maxCoeff();
        if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) {
          Log() << "  solved with no regularization necessary!" << std::endl;
          Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited"
                << std::endl;

          return true;
        } else {
          Log() << "MobyLCPSolver::SolveLcpLemke() - "
                << "'<w, z> not within tolerance(min value: " << wx_min
                << " max value: " << wx_max << ")" << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpLemke() - 'w' not solved to desired "
            "tolerance"
              << std::endl;
        Log() << "  minimum w: " << wx.minCoeff() << std::endl;
      }
    } else {
      Log() << "  MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired "
          "tolerance"
            << std::endl;
      Log() << "  minimum z: " << z->minCoeff() << std::endl;
    }
  }

  // update the pivots
  total_piv += pivots_;

  // start the regularization process
  int rf = min_exp;
  while (rf < max_exp) {
    // setup regularization factor
    double lambda =
        std::pow(static_cast<double>(10.0), static_cast<double>(rf));

    Log() << "  trying to solve LCP with regularization factor: " << lambda
          << std::endl;

    // regularize M
    MM = M;
    for (unsigned i = 0; i < M.rows(); i++) {
      MM(i, i) += lambda;
    }

    // try to solve the LCP
    result = SolveLcpLemke(MM, q, z, piv_tol, mod_zero_tol);

    // update total pivots
    total_piv += pivots_;

    if (result) {
      // verify that solution truly is a solution -- check z
      if (z->minCoeff() > -mod_zero_tol) {
        // check w
        wx = (MM * (*z)) + q;
        if (wx.minCoeff() > -mod_zero_tol) {
          // Check element-wise operation of z*wx.
          wx = z->array() * wx.eval().array();

          const T wx_min = wx.minCoeff();
          const T wx_max = wx.maxCoeff();
          if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) {
            Log() << "  solved with regularization factor: " << lambda
                  << std::endl;
            Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited"
                  << std::endl;
            pivots_ = total_piv;
            return true;
          } else {
            Log() << "MobyLCPSolver::SolveLcpLemke() - "
                  << "'<w, z> not within tolerance(min value: " << wx_min
                  << " max value: " << wx_max << ")" << std::endl;
          }
        } else {
          Log() << "  MobyLCPSolver::SolveLcpLemke() - 'w' not solved to "
              "desired tolerance"
                << std::endl;
          Log() << "  minimum w: " << wx.minCoeff() << std::endl;
        }
      } else {
        Log() << "  MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired "
            "tolerance"
              << std::endl;
        Log() << "  minimum z: " << z->minCoeff() << std::endl;
      }
    }

    // increase rf
    rf += step_exp;
  }

  Log() << "  unable to solve given any regularization!" << std::endl;
  Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited" << std::endl;

  // store total pivots
  pivots_ = total_piv;

  // still here?  failure...
  return false;
}

template <typename T>
MobyLCPSolver<T>::MobyLCPSolver()
    : SolverBase(&id, &is_available, &is_enabled,
                 &ProgramAttributesSatisfied) {}

template <typename T>
MobyLCPSolver<T>::~MobyLCPSolver() = default;

SolverId MobyLcpSolverId::id() {
  static const never_destroyed<SolverId> singleton{"Moby LCP"};
  return singleton.access();
}

template <typename T>
SolverId MobyLCPSolver<T>::id() {
  return MobyLcpSolverId::id();
}

template <typename T>
bool MobyLCPSolver<T>::is_available() {
  return true;
}

template <typename T>
bool MobyLCPSolver<T>::is_enabled() {
  return true;
}

template <typename T>
bool MobyLCPSolver<T>::ProgramAttributesSatisfied(
    const MathematicalProgram& prog) {
  // This solver currently imposes restrictions that its problem:
  //
  // (1) Contains only linear complementarity constraints,
  // (2) Has no element of any decision variable appear in more than one
  //     constraint, and
  // (3) Has every element of every decision variable in a constraint.
  //
  // Restriction 1 could reasonably be relaxed by reformulating other
  // constraint types that can be expressed as LCPs (eg, convex QLPs),
  // although this would also entail adding an output stage to convert
  // the LCP results back to the desired form.  See eg. @RussTedrake on
  // how to convert a linear equality constraint of n elements to an
  // LCP of 2n elements.
  //
  // There is no obvious way to relax restriction 2.
  //
  // Restriction 3 could reasonably be relaxed to simply let unbound
  // variables sit at 0.
  if (prog.required_capabilities() != ProgramAttributes({
        ProgramAttribute::kLinearComplementarityConstraint})) {
    return false;
  }

  // Check that the available LCPs cover the program and no two LCPs cover the
  // same variable.
  const auto& bindings = prog.linear_complementarity_constraints();
  for (int i = 0; i < static_cast<int>(prog.num_vars()); ++i) {
    int coverings = 0;
    for (const auto& binding : bindings) {
      if (binding.ContainsVariable(prog.decision_variable(i))) {
        coverings++;
      }
    }
    if (coverings != 1) {
      return false;
    }
  }

  return true;
}

// Instantiate templates.
template class MobyLCPSolver<double>;
template class MobyLCPSolver<Eigen::AutoDiffScalar<Vector1d>>;

}  // namespace solvers
}  // namespace drake


/*
* (C) 2009 Jack Lloyd
*
* Distributed under the terms of the Botan license
*/

#include <fstream>
#include <iostream>
#include <sstream>
#include <boost/regex.hpp>

#include <botan/botan.h>
#include <botan/eax.h>

using namespace Botan;

namespace {

unsigned from_string(const std::string& s)
   {
   std::istringstream stream(s);
   unsigned n;
   stream >> n;
   return n;
   }

std::string seq(unsigned n)
   {
   std::string s;

   for(unsigned i = 0; i != n; ++i)
      {
      unsigned char b = (i & 0xFF);

      const char bin2hex[] = { '0', '1', '2', '3', '4', '5', '6', '7',
                               '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };

      s += bin2hex[(b >> 4)];
      s += bin2hex[(b & 0x0f)];
      }

   return s;
   }

void eax_test(const std::string& algo,
              const std::string& key_str,
              const std::string& nonce_str,
              const std::string& header_str,
              const std::string& tag_str,
              const std::string& plaintext_str,
              const std::string& ciphertext)
   {
   /*
   printf("EAX(algo=%s key=%s nonce=%s header=%s tag=%s pt=%s ct=%s)\n",
          algo.c_str(), key_str.c_str(), nonce_str.c_str(), header_str.c_str(), tag_str.c_str(),
          plaintext_str.c_str(), ciphertext.c_str());
   */

   SymmetricKey key(key_str);
   InitializationVector iv(nonce_str);

   EAX_Encryption* enc;

   Pipe pipe(new Hex_Decoder,
             enc = new EAX_Encryption(get_block_cipher(algo)),
             new Hex_Encoder);

   enc->set_key(key);
   enc->set_iv(iv);

   OctetString header(header_str);

   enc->set_header(header.begin(), header.length());

   pipe.start_msg();
   pipe.write(plaintext_str);
   pipe.end_msg();

   std::string out = pipe.read_all_as_string();

   if(out != ciphertext + tag_str)
      {
      printf("BAD enc %s '%s' != '%s%s'\n", algo.c_str(),
             out.c_str(), ciphertext.c_str(), tag_str.c_str());
      }
   else
      printf("OK enc %s\n", algo.c_str());

   try
      {
      EAX_Decryption* dec;
      Pipe pipe2(new Hex_Decoder,
                 dec = new EAX_Decryption(get_block_cipher(algo)),
                 new Hex_Encoder);

      dec->set_key(key);
      dec->set_iv(iv);

      dec->set_header(header.begin(), header.length());

      pipe2.start_msg();
      pipe2.write(ciphertext);
      pipe2.write(tag_str);
      pipe2.end_msg();

      std::string out2 = pipe2.read_all_as_string();

      if(out2 != plaintext_str)
         {
         printf("BAD decrypt %s '%s'\n", algo.c_str(), out2.c_str());
         }
      else
         printf("OK decrypt %s\n", algo.c_str());
      }
   catch(std::exception& e)
      {
      printf("%s\n", e.what());
      }

   }

std::pair<std::string, int> translate_algo(const std::string& in)
   {
   if(in == "aes (16 byte key)")
      return std::make_pair("AES-128", 16);

   if(in == "blowfish (8 byte key)")
      return std::make_pair("Blowfish", 8);

   if(in == "rc2 (8 byte key)")
      return std::make_pair("RC2", 8);

   if(in == "rc5 (8 byte key)")
      return std::make_pair("RC5", 8);

   if(in == "rc6 (16 byte key)")
      return std::make_pair("RC6", 16);

   if(in == "safer-sk128 (16 byte key)")
      return std::make_pair("SAFER-SK(10)", 16);

   if(in == "twofish (16 byte key)")
      return std::make_pair("Twofish", 16);

   if(in == "des (8 byte key)")
      return std::make_pair("DES", 8);

   if(in == "3des (24 byte key)")
      return std::make_pair("TripleDES", 24);

   // These 3 are disabled due to differences in base algorithm.

#if 0
   // XTEA: LTC uses little endian, Botan (and Crypto++) use big-endian
   // I swapped to LE in XTEA and the vectors did match
   if(in == "xtea (16 byte key)")
      return std::make_pair("XTEA", 16);

   // Skipjack: LTC uses big-endian, Botan (and Crypto++) use
   // little-endian I am not sure if that was the full difference
   // though, was unable to replicate LTC's EAX vectors with Skipjack
   if(in == "skipjack (10 byte key)")
      return std::make_pair("Skipjack", 10);

   // Noekeon: unknown cause, though LTC's lone test vector does not
   // match Botan

   if(in == "noekeon (16 byte key)")
      return std::make_pair("Noekeon", 16);

#endif

   return std::make_pair("", 0);
   }

std::string rep(const std::string& s_in, unsigned n)
   {
   std::string s_out;

   for(unsigned i = 0; i != n; ++i)
      s_out += s_in[i % s_in.size()];

   return s_out;
   }

void run_tests(std::istream& in)
   {
   std::string algo;
   std::string key;

   while(in.good())
      {
      std::string line;

      std::getline(in, line);

      if(line == "")
         continue;

      if(line.size() > 5 && line.substr(0, 4) == "EAX-")
         {
         std::pair<std::string, int> name_and_keylen =
            translate_algo(line.substr(4));

         algo = name_and_keylen.first;
         key = seq(name_and_keylen.second);
         }
      else if(algo != "")
         {
         boost::regex vec_regex("^([ 0-9]{3}): (.*), (.*)$");

         boost::smatch what;

         if(boost::regex_match(line, what, vec_regex, boost::match_extra))
            {
            unsigned n = from_string(what[1]);
            std::string ciphertext = what[2];
            std::string tag = what[3];

            std::string plaintext = seq(n);
            std::string header = seq(n);
            std::string nonce = seq(n);

            eax_test(algo, key, nonce, header, tag,
                     plaintext, ciphertext);

            key = rep(tag, key.size()); // repeat as needed
            }
         }
      }

   }

}

int main()
   {
   std::ifstream in("eax.vec");

   Botan::LibraryInitializer init;

   if(!in)
      {
      std::cerr << "Couldn't read input file\n";
      return 1;
      }

   run_tests(in);

   }


#include <boost/multiprecision/cpp_int/multiply.hpp>


#pragma once

#include <map>
#include <vector>
#include "base/node.hpp"
#include "glog/logging.h"
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <base/context.hpp>
#include "base/third_party/general_fstream.hpp"
#include <math.h>
#include <driver/simple_id_mapper.hpp>

namespace minips {

    class ProgressTracker {
    public:
        void Init(const std::vector<uint32_t> &tids);

        /**
         * Advance the progress of a worker thread
         * Return -1 if min_clock_ does not change,
         * return min_clock_ otherwise.
         *
         * @param tid worker thread id
         */
        int AdvanceAndGetChangedMinClock(int tid);

        /**
         * Get the progress of a worker thread
         *
         * @param tid worker thread id
         */
        int GetProgress(int tid) const;

        /**
         * Get the progress of the slowest worker
         */
        int GetMinClock() const;

        /**
         * Get the number of workers in the trace
         */
        int GetNumThreads() const;

        /**
         * Check whether a worker thread is the only one with the slowest progress
         *
         * @param tid worker thread id
         */
        bool IsUniqueMin(int tid) const;

        /**
         * Check whether a worker thread is in the trace (thus valid to interact with the model)
         *
         * @param tid worker thread id
         */
        bool CheckThreadValid(int tid) const;

        int DeleteNode(uint32_t node_id);

        int Update(int failed_node_id, std::vector<Node> &nodes);

        int32_t RoundHundred(uint32_t input) {
            return 100 * ::round(input / 100.0);
        }

        void Dump() {
            if (!Context::get_instance().get_bool("checkpoint_toggle")) {
                return;
            }

            auto dump_prefix = Context::get_instance().get_string("checkpoint_file_prefix");
            auto node_id = Context::get_instance().get_int32("my_id");
            auto dump_file = dump_prefix + "server_progress_" + std::to_string(node_id);
            LOG(INFO) << "Dump Progress Storage To " << dump_file;

            petuum::io::ofstream w_stream(dump_file, std::ofstream::out | std::ofstream::trunc);
            w_stream << "min_clock" << ":" << RoundHundred(min_clock_) << " ";
            CHECK(w_stream);
            for (auto it = progresses_.begin(); it != progresses_.end(); it++) {
                w_stream << it->first << ":" << RoundHundred(it->second) << " ";
            }
            w_stream.close();
        }

        void Restore() {
            auto dump_prefix = Context::get_instance().get_string("checkpoint_file_prefix");
            auto node_id = Context::get_instance().get_int32("my_id");
            auto dump_file = dump_prefix + "server_progress_" + std::to_string(node_id);
            LOG(INFO) << "Restore Progress Storage From: " << dump_file;

            petuum::io::ifstream input(dump_file.c_str());
            std::string line;
            while (std::getline(input, line)) {
                std::vector<std::string> pairs;
                boost::split(pairs, line, boost::is_any_of(" "));
                for (std::string str : pairs) {
                    std::vector<std::string> pair;
                    boost::split(pair, str, boost::is_any_of(":"));

                    if (pair.size() < 2) {
                        continue;
                    }

                    if (pair[0].compare("min_clock") == 0) {
                        min_clock_ = std::atoi(pair[1].c_str());
                    } else {
                        int model_id = std::atoi(pair[0].c_str());
                        progresses_[model_id] = std::atoi(pair[1].c_str());

                        if (Context::get_instance().get_bool("has_scale_node")) {
                            int scale_node_id = Context::get_instance().get_int32("scale_node_id");
                            int mid = scale_node_id * SimpleIdMapper::kMaxThreadsPerNode + model_id % SimpleIdMapper::kMaxThreadsPerNode;
                            progresses_[mid] = progresses_[model_id];
                            LOG(INFO) << "Add scale node tracker=" << mid << ", progress=" << progresses_[mid];
                        }
                    }
                }
            }
            input.close();
        }

        void DebugString() {
            auto node_id = Context::get_instance().get_int32("my_id");
            LOG(INFO) << "DebugString on node=" << std::to_string(node_id);
            for (auto it = progresses_.begin(); it != progresses_.end(); it++) {
                LOG(INFO) << "tid:" << it->first << ", progress:" << it->second;
            }
        }

    private:
        std::map<int, int> progresses_;  // {tid: progress}
        int min_clock_;                  // the slowest progress
    };

}  // namespace minips


#include "create_box.h"

#include "dynamic_value/get_value_as.h"
#include "geometry_component.h"
#include "geometry_operations/create_box.h"
#include "geometry_system.h"
#include "hierarchical_component.h"
#include "hierarchical_system.h"
#include "procedural_component.h"
#include "procedural_object_system.h"

#include <boost/qvm/map_vec_mat.hpp>

namespace pagoda
{
const std::string CreateBoxGeometry::outputGeometry("out");
const char* CreateBoxGeometry::name = "CreateBoxGeometry";

CreateBoxGeometry::CreateBoxGeometry(ProceduralObjectSystemPtr objectSystem) : ProceduralOperation(objectSystem)
{
	START_PROFILE;
	RegisterValues({{"xSize", std::make_shared<FloatValue>(0.0f)},
	                {"ySize", std::make_shared<FloatValue>(0.0f)},
	                {"zSize", std::make_shared<FloatValue>(0.0f)}});

	CreateOutputInterface(outputGeometry);
}

CreateBoxGeometry::~CreateBoxGeometry() {}

void CreateBoxGeometry::DoWork()
{
	START_PROFILE;

	float xSize = get_value_as<float>(*GetValue("xSize"));
	float ySize = get_value_as<float>(*GetValue("ySize"));
	float zSize = get_value_as<float>(*GetValue("zSize"));

	auto geometrySystem = m_proceduralObjectSystem->GetComponentSystem<GeometrySystem>();
	auto hierarchicalSystem = m_proceduralObjectSystem->GetComponentSystem<HierarchicalSystem>();

	CreateBox<Geometry> createBox(xSize, ySize, zSize);
	auto geometry = std::make_shared<Geometry>();
	createBox.Execute(geometry);

	ProceduralObjectPtr object = CreateOutputProceduralObject(outputGeometry);
	std::shared_ptr<GeometryComponent> geometry_component =
	    geometrySystem->CreateComponentAs<GeometryComponent>(object);

	geometry_component->SetGeometry(geometry);
	geometry_component->SetScope(
	    Scope::FromGeometryAndConstrainedRotation(geometry, Mat3x3F(boost::qvm::diag_mat(Vec3F{1.0f, 1.0f, 1.0f}))));

	std::shared_ptr<HierarchicalComponent> hierarchical_component =
	    hierarchicalSystem->CreateComponentAs<HierarchicalComponent>(object);
}
}  // namespace pagoda