#include "electromagnet_calibration.h"

#include <iomanip>
#include <Eigen/Jacobi>

ElectromagnetCalibration::MagneticWorkSpace::MagneticWorkSpace()
{
    xMin = xMax = yMin = yMax = zMin = zMax = 0;
}

ElectromagnetCalibration::MagneticWorkSpace::MagneticWorkSpace(double size)
{
    xMin = -size;
    xMax = size;
    yMin = -size;
    yMax = size;
    zMin = -size;
    zMax = size;
}

ElectromagnetCalibration::MagneticWorkSpace::MagneticWorkSpace(double xMin_, double xMax_, double yMin_, double yMax_, double zMin_, double zMax_ )
{
    xMin = xMin_;
    xMax = xMax_;
    yMin = yMin_;
    yMax = yMax_;
    zMin = zMin_;
    zMax = zMax_;
}

MagneticMeasurement::MagneticMeasurement()
{
    Field.setZero(3,1);
    Position.setZero(3,1);
    AppliedCurrentVector.setZero(0,1);

}

MagneticMeasurement::MagneticMeasurement(const Eigen::Vector3d &F, const Eigen::Vector3d &P, const Eigen::VectorXd &C)
{
    Field = F;
    Position = P;
    AppliedCurrentVector = C;
}

ElectromagnetCalibration::ElectromagnetCalibration( std::string calibrationFileName )
{
    bool calibrationFileLoadSucessful = loadCalibration(calibrationFileName);

    assert( calibrationFileLoadSucessful );

    checkSourcePositions();
}

ElectromagnetCalibration::ElectromagnetCalibration()
{
    /*< Default Constructor only available to inheriting classes **/
    this->name = "NONE";
    this->coilList.clear();
    this->use_offset = false;

    checkSourcePositions();
}

//
// \brief the constructor from a coil list.
// \param coilList The list of coils and their respective sources.
// \param dc_field_offset The dc offset field, if any.
//
ElectromagnetCalibration::ElectromagnetCalibration(std::string systemName_, const MagneticWorkSpace& workSpace_, const std::vector<ScalorPotential>& coilList_, const ScalorPotential& dc_field_offset_ )
{
    /*< Default Constructor only available to inheriting classes **/
    this->coilList = coilList_;
    this->name = systemName_ ;
    this->offset = dc_field_offset_;
    this->use_offset = ( 0 != offset.getNumberOfSources());
    this->workSpace = workSpace_;

    checkSourcePositions();
}

// The following functions return the field and/or gradient given a current vector and a position.
//   If no position is provided, it is assumed to be at the workspace origin. For the combined vector,
//   the gradient matrix has been repacked into a 5 element vetor form (because it is symetric and has zero trace).
//   The order of the gradient terms is: [dBx/dx, dBx/dy, dBx/dz, dBy/dy, dBy/dz].
Eigen::Vector3d ElectromagnetCalibration::fieldAtPoint(    const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{
    assert( currentVector.size() == coilList.size() );
    Eigen::Vector3d field(0,0,0);
    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        field += coilIT->getGradient(position)*currentVector(coilNum);
    }
    if( use_offset )
    {
        field += offset.getGradient(position);
    }

    return field;
}

Eigen::Matrix3d ElectromagnetCalibration::gradientAtPoint( const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{

    assert( currentVector.size() == coilList.size() ); // make sure there are as many currents as coils
    Eigen::MatrixXd actuationMatrix = gradientCurrentJacobian( position );

    return remapGradientVector(actuationMatrix*currentVector + offsetFieldAndGradientAtPoint(position).tail<5>());

}

Vector8d ElectromagnetCalibration::fieldAndGradientAtPoint( const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{
    assert( currentVector.size() == coilList.size() ); // make sure there are as many currents as coils
    Eigen::MatrixXd actuationMatrix = fieldAndGradientCurrentJacobian( position );

    return actuationMatrix*currentVector + offsetFieldAndGradientAtPoint(position);
}

Vector8d ElectromagnetCalibration::offsetFieldAndGradientAtPoint( const Eigen::Vector3d& position ) const
{
    Vector8d offsetFieldAndGradient = Vector8d::Zero();

    if( use_offset )
    {

        ScalorPotentialState fieldInfo = offset.getState(position);

        offsetFieldAndGradient.head<3>() = fieldInfo.firstSpatialDerivative;
        offsetFieldAndGradient.tail<5>() = remapGradientMatrix(fieldInfo.secondSpatialDerivative);
    }

    return offsetFieldAndGradient;
}

// The following functions return the Current Jacobian of the field and/or gradient.  This matrix can be inverted to determine the
//   currents necessary to achieve a desired field and gradient.  The gradient portion of the matrix has been vectorized into 5 elements.
//   They are [dBx/dx, dBx/dy, dBx/dz, dBy/dy, dBy/dz].
Eigen::MatrixXd ElectromagnetCalibration::fieldCurrentJacobian( const Eigen::Vector3d& position ) const
{
    Eigen::MatrixXd actuationMatrix = Eigen::MatrixXd::Zero(3,coilList.size());

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);
        actuationMatrix.block<3,1>(0,coilNum) = fieldInfo.firstSpatialDerivative;

    }

    return actuationMatrix;
}

Eigen::MatrixXd ElectromagnetCalibration::gradientCurrentJacobian( const Eigen::Vector3d& position ) const
{

    Eigen::MatrixXd actuationMatrix = Eigen::MatrixXd::Zero(5,coilList.size());

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);
        actuationMatrix.block<5,1>(0,coilNum) = remapGradientMatrix(fieldInfo.secondSpatialDerivative);

    }

    return actuationMatrix;
}

Eigen::MatrixXd ElectromagnetCalibration::fieldAndGradientCurrentJacobian( const Eigen::Vector3d& position ) const
{

    Eigen::MatrixXd actuationMatrix = Eigen::MatrixXd::Zero(8,coilList.size());

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);
        actuationMatrix.block<3,1>(0,coilNum) = fieldInfo.firstSpatialDerivative;
        actuationMatrix.block<5,1>(3,coilNum) = remapGradientMatrix(fieldInfo.secondSpatialDerivative);

    }

    return actuationMatrix;
}

// This function returns the 5x3 Jacobian describing how gradient changes with position.  The gradient change is a 5x3 packing of a 3x3x3 tensor.
//  The first column is how the gradient vector packing [dBx/dx, dBx/dy, dBx/dz, dBy/dy, dBy/dz] changes with x, the second is how it changes with y, and third is
//  how it changes with z.
Eigen::Matrix<double,5,3> ElectromagnetCalibration::gradientPositionJacobian( const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{

    assert( currentVector.size() == coilList.size() );// make sure there are as many currents as coils

    Eigen::Matrix<double,5,3> gradJacobian = Eigen::MatrixXd::Zero(5,3);

    if( currentVector.norm() == 0 && !useOffset() )
    {
        // NOthing to do!
        return gradJacobian;
    }

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);
        gradJacobian += fieldInfo.thirdSpatialDerivative*currentVector(coilNum);

    }

    if( use_offset )
    {
        ScalorPotentialState fieldInfo = offset.getState(position);
        gradJacobian += fieldInfo.thirdSpatialDerivative;
    }

    return gradJacobian;
}

// This function returns the field, gradient, gradientJacobian, and field/gradient current jacobain.  It is more efficient than requesting them seporately
void ElectromagnetCalibration::fullMagneticState( Eigen::Vector3d& fieldAtPoint, Eigen::Matrix<double,8,3>& fieldGradientPositionJacobian, Eigen::MatrixXd& fieldGradientCurrentJacobian, const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{
    assert( currentVector.size() == coilList.size() );// make sure there are as many currents as coils

    fieldAtPoint.setZero();
    fieldGradientPositionJacobian.setZero();
    fieldGradientCurrentJacobian.setZero(8,coilList.size());

    Vector5d gradAtPt;
    gradAtPt.setZero();

    if( currentVector.norm() == 0 && !this->useOffset() )
    {
        // NOthing to do!
        return ;
    }

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    int coilNum = 0;
    for( ; coilIT != coilList.end(); coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);

        fieldGradientCurrentJacobian.block<3,1>(0,coilNum) = fieldInfo.firstSpatialDerivative;
        fieldGradientCurrentJacobian.block<5,1>(3,coilNum) = remapGradientMatrix(fieldInfo.secondSpatialDerivative);

        fieldGradientPositionJacobian.block<5,3>(3,0) += fieldInfo.thirdSpatialDerivative*currentVector(coilNum);
        fieldGradientPositionJacobian.block<3,3>(0,0) += fieldInfo.secondSpatialDerivative*currentVector(coilNum);
        fieldAtPoint += fieldInfo.firstSpatialDerivative*currentVector(coilNum);
    }

    if( use_offset )
    {
        ScalorPotentialState fieldInfo = offset.getState(position);

        fieldGradientPositionJacobian.block<5,3>(3,0) += fieldInfo.thirdSpatialDerivative;
        fieldGradientPositionJacobian.block<3,3>(0,0) += fieldInfo.secondSpatialDerivative;
        fieldAtPoint += fieldInfo.firstSpatialDerivative;

    }

}

MagneticState ElectromagnetCalibration::fullMagneticState(const Eigen::VectorXd& currentVector, const Eigen::Vector3d& position ) const
{
    assert( currentVector.size() == coilList.size() );// make sure there are as many currents as coils

    MagneticState returnState;
    returnState.FieldGradientActuationMatrix.setZero(8,coilList.size());

    if( !pointInWorkspace(position) )
    {
        cout << "Warning: Requesting point out side of magnetic workspace. " << position.transpose() << " (Xmin,Xmax): (" << this->workSpace.xMin << ", " << this->workSpace.xMax << ") " <<
                " (Ymin,Ymax): (" << this->workSpace.yMin << ", " << this->workSpace.yMax << ") " <<
                " (Zmin,Zmax): (" << this->workSpace.zMin << ", " << this->workSpace.zMax << ") " << endl;
    }

    std::vector<ScalorPotential>::const_iterator coilIT;
    int coilNum;
    for( coilIT = coilList.begin(), coilNum = 0;
         coilIT != coilList.end();
         coilIT++, coilNum ++ )
    {
        ScalorPotentialState fieldInfo = coilIT->getState(position);

        returnState.FieldGradientActuationMatrix.block<3,1>(0,coilNum) = fieldInfo.firstSpatialDerivative;
        returnState.FieldGradientActuationMatrix.block<5,1>(3,coilNum) =  remapGradientMatrix(fieldInfo.secondSpatialDerivative);

        returnState.Field += fieldInfo.firstSpatialDerivative * currentVector(coilNum);
        returnState.GradientPositionJacobian += fieldInfo.thirdSpatialDerivative*currentVector(coilNum);
        returnState.Gradient += fieldInfo.secondSpatialDerivative*currentVector(coilNum);

    }

    if( use_offset )
    {
        ScalorPotentialState fieldInfo = offset.getState(position);

        returnState.Field += fieldInfo.firstSpatialDerivative;
        returnState.GradientPositionJacobian += fieldInfo.thirdSpatialDerivative;
        returnState.Gradient += fieldInfo.secondSpatialDerivative;
    }

    return returnState;
}

// These two functions convert a vector packing of the gradient to a matrix packing and vice versa.
Eigen::Matrix3d ElectromagnetCalibration::remapGradientVector(const Vector5d& gradVector )
{
    Eigen::Matrix3d gradientMatrix;
    // repack vector gradient into matrix gradient
    gradientMatrix.leftCols<1>() = gradVector.topRows<3>();
    gradientMatrix.block<2,1>(1,1) = gradVector.bottomRows<2>();
    gradientMatrix(0,1) = gradientMatrix(1,0);
    gradientMatrix(0,2) = gradientMatrix(2,0);
    gradientMatrix(1,2) = gradientMatrix(2,1);
    gradientMatrix(2,2) = -1.0*(gradientMatrix(0,0)+gradientMatrix(1,1));

    return gradientMatrix;
}

Vector5d ElectromagnetCalibration::remapGradientMatrix( const Eigen::Matrix3d& gradMatrix)
{
    Vector5d gradVec;
    gradVec.topRows<3>() = gradMatrix.leftCols<1>();
    gradVec(3) = gradMatrix(1,1);
    gradVec(4) = gradMatrix(2,1);
    return gradVec;
}

Eigen::MatrixXd ElectromagnetCalibration::packForceMatrix(const Eigen::Vector3d& moment)
{
    Eigen::MatrixXd ForceMatrix(3,5);
    ForceMatrix <<  moment(0),  moment(1), moment(2),  0,         0,
            0,          moment(0), 0,          moment(1), moment(2),
            -moment(2), 0,         moment(0), -moment(2), moment(1);
    return ForceMatrix;
}

bool ElectromagnetCalibration::loadCalibration(std::string fileName)
{
    coilList.clear();

    // reinitialize offset
    offset = ScalorPotential();

    YAML::Node systemDefinition = YAML::LoadFile(fileName);
    this->name = systemDefinition["System_Name"].as<std::string>();
    int numCoils = systemDefinition["Coil_List"].size();
    this->workSpace.xMin = systemDefinition["Workspace_Dimensions"][0][0].as<double>();
    this->workSpace.xMax = systemDefinition["Workspace_Dimensions"][0][1].as<double>();
    this->workSpace.yMin = systemDefinition["Workspace_Dimensions"][1][0].as<double>();
    this->workSpace.yMax = systemDefinition["Workspace_Dimensions"][1][1].as<double>();
    this->workSpace.zMin = systemDefinition["Workspace_Dimensions"][2][0].as<double>();
    this->workSpace.zMax = systemDefinition["Workspace_Dimensions"][2][1].as<double>();

    for( unsigned int coil = 0; coil<numCoils; coil++ )
    {
        std::string coilTag = systemDefinition["Coil_List"][coil].as<std::string>();

        ScalorPotential newCoil;

        int numSources = systemDefinition[coilTag]["Source_List"].size();

        for( unsigned int src = 0; src < numSources; src ++ )
        {
            ScalorPotential::srcStruct newSrc;
            std::string srcTag = systemDefinition[coilTag]["Source_List"][src].as<std::string>();

            std::vector<double> coeff = systemDefinition[coilTag][srcTag]["A_Coeff"].as< std::vector<double> >();
            for( unsigned int i=0; i<coeff.size(); i++ )
                newSrc.A_Coeff.push_back(ScalorPotential::srcCoeff(coeff[i],i+1));

            coeff = systemDefinition[coilTag][srcTag]["B_Coeff"].as< std::vector<double> >();
            for( unsigned int i=0; i<coeff.size(); i++ )
                newSrc.B_Coeff.push_back(ScalorPotential::srcCoeff(coeff[i],i+1));

            newSrc.srcDirection = systemDefinition[coilTag][srcTag]["Source_Direction"].as< Eigen::Vector3d >();
            newSrc.srcDirection.normalize(); // normalize in place
            newSrc.srcPosition = systemDefinition[coilTag][srcTag]["Source_Position"].as< Eigen::Vector3d >();

            newCoil.setSourceStruct(src,newSrc); // add to coil's source list
        }

        if( coilTag.find("Offset") == std::string::npos )
        {
            // "Offset" not found
            this->coilList.push_back(newCoil); // add to coil List
        } else
        {
            // Offset tag is found
            offset = newCoil;
        }
    }

    use_offset = offset.getNumberOfSources();
    return coilList.size() > 0 || use_offset;
}

///@brief  writes a new calibration file
/// @param a string pointing to the location for the yaml formated calbiration file
bool ElectromagnetCalibration::writeCalibration(std::string fileName) const
{
    YAML::Node systemDefinition;
    systemDefinition["System_Name"] = this->name;

    std::vector<ScalorPotential>::const_iterator coilIT = coilList.begin();
    unsigned int coilNum =0;
    for( ; coilIT != coilList.end(); coilIT ++, coilNum ++ )
    {
        stringstream coilName;
        coilName << "Coil_" << coilNum;
        systemDefinition["Coil_List"].push_back(coilName.str());

        for( unsigned int srcNum = 0; srcNum < coilIT->getNumberOfSources(); srcNum ++ )
        {
            ScalorPotential::srcStruct src = coilIT->getSourceStruct(srcNum);
            stringstream srcName;
            srcName << "Src_" << srcNum;
            systemDefinition[coilName.str()]["Source_List"].push_back(srcName.str());
            std::vector<double> coeff;
            for( unsigned int i=0; i<src.A_Coeff.size(); i++ )
                coeff.push_back(src.A_Coeff[i].coeff);
            systemDefinition[coilName.str()][srcName.str()]["A_Coeff"] = coeff;

            coeff.clear();
            for( unsigned int i=0; i<src.B_Coeff.size(); i++ )
                coeff.push_back(src.B_Coeff[i].coeff);
            systemDefinition[coilName.str()][srcName.str()]["B_Coeff"] = coeff;
            systemDefinition[coilName.str()][srcName.str()]["Source_Direction"] = src.srcDirection;
            systemDefinition[coilName.str()][srcName.str()]["Source_Position"] = src.srcPosition;
        }
    }

    if( this->hasOffset() )
    {
        stringstream coilName;
        coilName << "Offset";
        systemDefinition["Coil_List"].push_back(coilName.str());

        for( unsigned int srcNum = 0; srcNum < offset.getNumberOfSources(); srcNum ++ )
        {
            ScalorPotential::srcStruct src = offset.getSourceStruct(coilNum);
            stringstream srcName;
            srcName << "Src_" << srcNum;
            systemDefinition[coilName.str()]["Source_List"].push_back(srcName.str());
            std::vector<double> coeff;
            for( unsigned int i=0; i<src.A_Coeff.size(); i++ )
                coeff.push_back(src.A_Coeff[i].coeff);
            systemDefinition[coilName.str()][srcName.str()]["A_Coeff"] = coeff;

            coeff.clear();
            for( unsigned int i=0; i<src.A_Coeff.size(); i++ )
                coeff.push_back(src.B_Coeff[i].coeff);
            systemDefinition[coilName.str()][srcName.str()]["B_Coeff"] = coeff;
            systemDefinition[coilName.str()][srcName.str()]["Source_Direction"] = src.srcDirection;
            systemDefinition[coilName.str()][srcName.str()]["Source_Position"] = src.srcPosition;
        }
    }

    systemDefinition["Workspace_Dimensions"][0][0] = this->workSpace.xMin;
    systemDefinition["Workspace_Dimensions"][0][1] = this->workSpace.xMax;
    systemDefinition["Workspace_Dimensions"][1][0] = this->workSpace.yMin;
    systemDefinition["Workspace_Dimensions"][1][1] = this->workSpace.yMax;
    systemDefinition["Workspace_Dimensions"][2][0] = this->workSpace.zMin;
    systemDefinition["Workspace_Dimensions"][2][1] = this->workSpace.zMax;

    std::ofstream fout;
    fout.open(fileName.c_str(),std::ofstream::out|std::ofstream::trunc);
    fout << systemDefinition;
    fout.close();

    return fout.good();
}

int ElectromagnetCalibration::getNumberOfCoils() const
{
    return coilList.size();
}

///@brief returns the number of sources for the given coil
int ElectromagnetCalibration::getNumberOfSources( unsigned int coilNum ) const
{
    if( coilNum >= (coilList.size() + useOffset()) )
        return 0;
    else if( coilNum == coilList.size() )
        return offset.getNumberOfSources();
    else
        return coilList[coilNum].getNumberOfSources();
}

///@brief returns the number of coefficients for the given source
int ElectromagnetCalibration::getNumberOfCoeffients( unsigned int coilNum, unsigned int srcNum ) const
{
    if( coilNum >= (coilList.size() + useOffset()) )
        return 0;
    else if( coilNum == coilList.size() )
    {
        if( srcNum < offset.getNumberOfSources() )
            return offset.getSourceStruct(srcNum).A_Coeff.size() + offset.getSourceStruct(srcNum).B_Coeff.size();
    }
    else if( srcNum < coilList[coilNum].getNumberOfSources() )
        return coilList[coilNum].getSourceStruct(srcNum).A_Coeff.size() + coilList[coilNum].getSourceStruct(srcNum).B_Coeff.size();

    return 0;

}

bool ElectromagnetCalibration::hasOffset() const
{
    return offset.getNumberOfSources();
}

std::string ElectromagnetCalibration::getName() const
{
    return name;
}

bool ElectromagnetCalibration::pointInWorkspace( const Eigen::Vector3d& position ) const
{
    return workSpace.xMin <= position(0) && workSpace.xMax >= position(0)
            && workSpace.yMin <= position(1) && workSpace.yMax >= position(1)
            && workSpace.zMin <= position(2) && workSpace.zMax >= position(2);
}

void ElectromagnetCalibration::setWorkSpace(const ElectromagnetCalibration::MagneticWorkSpace& ws)
{
    workSpace = ws;
    return;
}

ElectromagnetCalibration::MagneticWorkSpace ElectromagnetCalibration::getWorkSpace() const
{
    return workSpace;
}

void ElectromagnetCalibration::useOffset(bool offsetOn)
{
    use_offset = offsetOn;
}

void ElectromagnetCalibration::useOffset(const ScalorPotential& newOffset )
{
    offset = newOffset;
    use_offset = true;
    return;
}

bool ElectromagnetCalibration::useOffset() const
{
    return use_offset;
}

bool ElectromagnetCalibration::checkSourcePositions(bool printWarning) const
{
    std::vector<ScalorPotential>::const_iterator coilIterator;
    std::vector<ScalorPotential::srcStruct>::const_iterator srcIterator;
    int numTooClose = 0;
    int numTooFar = 0;
    // normalize directions, check positions relative to workspace, and source counts
    for( coilIterator  = coilList.begin();
         coilIterator != coilList.end();
         coilIterator ++)
    {
        for(srcIterator  = coilIterator->srcList.begin();
            srcIterator != coilIterator->srcList.end();
            srcIterator ++ )
        {

            double distanceSq = (srcIterator->srcPosition - pCenter).squaredNorm();

            if( distanceSq < rMinSq)
            {
                numTooClose ++;
            }else if( distanceSq > rMaxSq )
            {
                numTooFar ++;
            }
        }
    }

    if(printWarning && (numTooClose+numTooFar) )
    {
        cout << "There are " << numTooClose << " sources too close to the workspace center and " << numTooFar << " too far from the workspace center." << endl;
    }

    return (numTooClose+numTooFar == 0);
}

void ElectromagnetCalibration::calibrate(std::string calibrationName, const std::vector<MagneticMeasurement>& dataList, bool printProgress, bool printStats_, calibration_constraints constraint,double minimumSourceToCenterDistance, double maximumSourceToCenterDistance, double converganceTolerance, int maxAttempts, int numberOfConvergedIterations)
{
    name = calibrationName;

    if( constraint == HEADING_AND_POSITION )
        minRadIsActive = true;
    else
        minRadIsActive = false;
    nConst = 2;

    posWeight = 1;

    double sqrtEpsilon = std::sqrt(std::numeric_limits<double>::epsilon());
    double rmsError_this, rmsError_last;
    int iteration = 0;
    bool converged;
    int convergedIterations = 0;

    do{
        numberOfMeasurements = dataList.size();
        numberOfParameters = 0;
        numberOfConstraints = 0;
        numberOfSources = 0;

        std::vector<ScalorPotential>::iterator coilIT = coilList.begin();
        std::vector<MagneticMeasurement>::const_iterator measIT;

        // count problem size
        int coilNum = 0;
        for( ; coilIT != coilList.end(); coilIT ++, coilNum ++ )
        {
            numberOfSources += coilIT->getNumberOfSources();
            numberOfConstraints += nConst*coilIT->getNumberOfSources();
            numberOfParameters += coilIT->getNumCalibrationParameters();
        }
        if( useOffset() )
        {
            numberOfSources += offset.getNumberOfSources();
            numberOfConstraints += nConst*offset.getNumberOfSources();
            numberOfParameters += offset.getNumCalibrationParameters();
        }

        // Define Workspace Size off of DATA
        workSpace.xMax = -std::numeric_limits<double>::max();
        workSpace.yMax = -std::numeric_limits<double>::max();
        workSpace.zMax = -std::numeric_limits<double>::max();
        workSpace.xMin = std::numeric_limits<double>::max();
        workSpace.yMin = std::numeric_limits<double>::max();
        workSpace.zMin = std::numeric_limits<double>::max();
        for( measIT = dataList.begin(); measIT != dataList.end(); measIT ++)
        {
            workSpace.xMax = std::max(workSpace.xMax, measIT->Position.x());
            workSpace.xMin = std::min(workSpace.xMin, measIT->Position.x());

            workSpace.yMax = std::max(workSpace.yMax, measIT->Position.y());
            workSpace.yMin = std::min(workSpace.yMin, measIT->Position.y());

            workSpace.zMax = std::max(workSpace.zMax, measIT->Position.z());
            workSpace.zMin = std::min(workSpace.zMin, measIT->Position.z());
        }

        // Calculate Workspace Info
        pCenter = Eigen::Vector3d(workSpace.xMax+workSpace.xMin,workSpace.yMax+workSpace.yMin,workSpace.zMax+workSpace.zMin)/2.0;

        if( minimumSourceToCenterDistance < 0)
        {
            rMinSq = 0;
            for( measIT = dataList.begin(); measIT != dataList.end(); measIT ++)
            {
                rMinSq = std::max(rMinSq, 1.001*(measIT->Position-pCenter).squaredNorm()); // limit it to no closer than 5% bigger than the wkspace size
            }
        } else
        {
            rMinSq = 1.001 * std::pow(minimumSourceToCenterDistance,2);
        }

        if( maximumSourceToCenterDistance < 0  || (constraint != HEADING_AND_POSITION ))
        {
            rMaxSq = rMinSq;

            if( maximumSourceToCenterDistance > 0 )
                rMaxSq = std::max(std::pow(maximumSourceToCenterDistance,2),rMaxSq);

            int srcNum;
            for(coilIT = coilList.begin(); coilIT != coilList.end(); coilIT ++ )
            {
                ScalorPotential::srcStruct src;
                for( srcNum = 0; srcNum < coilIT->getNumberOfSources(); srcNum ++ )
                {
                    src = coilIT->getSourceStruct(srcNum);
                    rMaxSq = std::max(rMaxSq, 100*(src.srcPosition-pCenter).squaredNorm()); // limit it to 30x the farthest initial guess

                    if( rMaxSq == 0 )
                        rMaxSq = 1;
                }
            }

        } else
        {
            rMaxSq = 0.999 * std::pow(maximumSourceToCenterDistance,2);
        }

        assert(('Electromagnet_Calibration::calibrate: The minimum source to center distance is greator than the maximum allowable source to center distance.', rMaxSq >= rMinSq || constraint == UNIT_HEADING_ONLY ));

        // initialize coefficients with linear least squares
        linearLeastSquareCoeffFit(dataList);

        // preallocate vectors and matricies for solution
        Eigen::MatrixXd J;
        Eigen::MatrixXd JtJ;
        Eigen::VectorXd E;

        // ************ OPTIMIZE WITH NONLINEAR LEAST SQUARES ***************** //

        JtJ.setZero(0,0);
        J.setZero(3*numberOfMeasurements+numberOfConstraints,numberOfParameters);
        E.setZero(0);

        // initialize PHI leave all Lambda's Initialized to Zero
        Eigen::VectorXd states(numberOfParameters), states_last(numberOfParameters), delta_States(numberOfParameters), delta_States_last(numberOfParameters), error_this(numberOfMeasurements*3+numberOfConstraints), delta_error(numberOfMeasurements*3+numberOfConstraints);
        Eigen::VectorXd deltaErrExp(numberOfMeasurements*3+numberOfConstraints);
        Eigen::VectorXd ds_tmp;
        Eigen::VectorXd error_last = error_this;
        Eigen::VectorXd delta_error_last = delta_error;
        Eigen::VectorXd error_last_last = error_last;

        obtainPHI(states);
        applyPHI(states);
        obtainPHI(states);
        states_last = states;

        delta_States.setZero(numberOfParameters);
        delta_States_last.setZero(numberOfParameters);

        packError(error_this, dataList);
        delta_error.setZero(error_this.rows(),error_this.cols());
        error_last = error_this;

        rmsError_this = std::sqrt(error_this.squaredNorm()/error_this.rows());
        rmsError_last = rmsError_this;

        double Lambda = 1;
        double LambdaChangeRange = 1 + sqrtEpsilon;
        double lambdaRangeCount = 0;

        double deltaRmsError = rmsError_this = rmsError_last;

        double predictionFactor = 1.0;
        double percentStateChange = 0.0;
        double percentErrorChange = 0.0;
        int errorIncreaseCounter = 0;

        if( printProgress )
        {
            std::cout << std::setprecision(3) << "Iteration: " << iteration << "   RMS Error: " << rmsError_this << std::endl;// << "   Delta RMS Error: " <<  deltaRmsError  << "\tError Change: " << percentErrorChange << "%" << "\tState Change: " << percentStateChange << "%";
        }

        do {
            converged = true;

            // get updated jacobian
            packErrorJacobian(J, dataList);

            // Remove Zero columns and rescale
            std::vector<int> HtoJ_index;
            std::vector<double> H_col_scale;
            Eigen::MatrixXd H(J.rows(),J.cols());
            unsigned int count = 0;
            for( unsigned int i=0; i<J.cols(); i++ )
            {
                double J_col_norm = J.col(i).norm();
                //cout << "Col: " << i << " Norm: " << J_col_norm << (J_col_norm > std::numeric_limits<double>::epsilon()*10.0?"\tKeep":"\tLoose")<< endl;
                if( J_col_norm > std::numeric_limits<double>::epsilon()*10.0 /*!=0*/ )
                {
                    H.col(count) = J.col(i)/J_col_norm;
                    H_col_scale.push_back(J_col_norm);
                    HtoJ_index.push_back(i);
                    count ++;
                }
            }

            delta_States_last = delta_States;

            int H_rows = H.rows();
            int H_cols = HtoJ_index.size();

            assert( H_rows > 0 && H_cols > 0 );

            JtJ = H.block(0,0,H_rows,H_cols).transpose()*H.block(0,0,H_rows,H_cols);
            JtJ += Lambda*Eigen::MatrixXd::Identity(H_cols,H_cols);

            ds_tmp = -JtJ.fullPivLu().solve(H.block(0,0,H_rows,H_cols).transpose()*error_this);

            //ds_tmp = -JtJ.ldlt().solve(H.block(0,0,H_rows,H_cols).transpose()*error_this);

            delta_States.setZero(delta_States.rows());
            for( unsigned int i=0; i<H_cols; i++ )
            {
                delta_States(HtoJ_index[i]) = ds_tmp(i)/H_col_scale[i];
            }

            states += delta_States;

            delta_error_last = delta_error;
            error_last_last = error_last;
            error_last = error_this;

            applyPHI(states);
            packError(error_this, dataList);

            double rmsError_last_last = rmsError_last;
            rmsError_last = rmsError_this;
            rmsError_this = std::sqrt(error_this.squaredNorm()/error_this.rows());

            delta_error = error_this-error_last;

            deltaRmsError = rmsError_this-rmsError_last;

            deltaErrExp =  J*delta_States + error_last;

            double delta_error_prediction = std::sqrt(deltaErrExp.squaredNorm()/error_this.rows())-rmsError_last;
            predictionFactor = deltaRmsError/( delta_error_prediction );

            delta_States = states-states_last; // just incase BC/Constraint inforcement changed something

            percentErrorChange = deltaRmsError / rmsError_this * 100.0;
            if( std::isnan(percentErrorChange) )
                percentErrorChange = deltaRmsError;

            double normStates = states.norm();
            percentStateChange = delta_States.norm()/normStates * 100.0;
            if( std::isnan(percentStateChange) )
                percentStateChange = sqrt(delta_States.squaredNorm()/delta_States.rows());

            if( deltaRmsError <= 0  )
            {
                errorIncreaseCounter = 1;

                // UPDATE Lambda based on convergance criteria

                // handel boundry conditions that occure at initialization
                if( std::isinf(predictionFactor) || std::isnan(predictionFactor)  )
                {
                    predictionFactor = 1.0-3.0/4.0*LambdaChangeRange;
                }

                if( predictionFactor > 1+LambdaChangeRange/4.0 )
                {
                    double e_this_sqNorm = error_this.squaredNorm();
                    double deltaError = e_this_sqNorm-error_last.squaredNorm();

                    std::vector<double> e,ac,bc,cc;
                    e.push_back(e_this_sqNorm-deltaError);
                    e.push_back(e_this_sqNorm);
                    ac.push_back(0);
                    ac.push_back(1);
                    bc.push_back(0);
                    bc.push_back(1);
                    cc.push_back(1);
                    cc.push_back(1);

                    double factor = 1.0;
                    while (  deltaError < 0 )
                    {
                        factor *= 3.0;
                        states = states_last + factor*delta_States;

                        // update error
                        error_last = error_this;
                        applyPHI(states);
                        packError(error_this, dataList);

                        double e_this_sqNorm2 = error_this.squaredNorm();

                        e.push_back(e_this_sqNorm2);
                        ac.push_back(factor*factor);
                        bc.push_back(factor);
                        cc.push_back(1.0);

                        deltaError = e_this_sqNorm2-e_this_sqNorm;
                        e_this_sqNorm = e_this_sqNorm2;
                    }

                    Eigen::MatrixXd A(e.size(),3);
                    Eigen::VectorXd E(e.size(),1);
                    for( int i=0; i< e.size(); i++ )
                    {
                        A.block(i,0,1,3) << ac[i], bc[i], cc[i];
                        E(i) = e[i];
                    }
                    Eigen::Vector3d coeff = A.householderQr().solve(E);

                    factor = -coeff(1)/(2.0*coeff(0));

                    delta_States *= factor;
                    states = states_last + delta_States;

                    // update error
                    error_last = error_this;
                    applyPHI(states);
                    packError(error_this, dataList);

                    delta_States = states-states_last; // just incase BC/Constraint inforcement changed something

                    error_last = error_last_last;
                    rmsError_this = std::sqrt(error_this.squaredNorm()/error_this.rows());//solutionRMSError();

                    delta_error = error_this-error_last;

                    deltaRmsError = rmsError_this-rmsError_last;

                    lambdaRangeCount ++;

                    percentErrorChange = deltaRmsError / rmsError_this * 100.0;
                    if( std::isnan(percentErrorChange) )
                        percentErrorChange = deltaRmsError;

                    double normStates = states.norm();
                    percentStateChange = delta_States.norm()/normStates * 100.0;
                    if( std::isnan(percentStateChange) )
                        percentStateChange = sqrt(delta_States.squaredNorm()/delta_States.rows());

                }
                else if( std::abs(1.0-predictionFactor) > LambdaChangeRange )
                {
                    // Error change was too small compared to a linear change. Getting close to an over-step.  Be more gradient decent.
                    double factor =  2;
                    Lambda = std::max( Lambda*factor, std::numeric_limits<double>::epsilon() );

                }
                else if( std::abs(1.0-predictionFactor) < LambdaChangeRange/2.0 )
                {
                    // Error change was too close to linear.  Get more aggresive!
                    double factor  =  1/2.0;
                    Lambda = std::max( Lambda*factor, std::numeric_limits<double>::epsilon() );

                    lambdaRangeCount ++;

                }

                if( lambdaRangeCount > 3 )
                {
                    LambdaChangeRange = std::min(1.0, LambdaChangeRange*2);
                    lambdaRangeCount = 0;
                }

                states_last = states;
            }
            else
            {
                errorIncreaseCounter ++;
                // change made things worse!

                Lambda *= 2.0*errorIncreaseCounter;
                //Lambda *= std::abs(deltaRmsError-delta_error_prediction)/Lambda+2.0;//2.0;
                LambdaChangeRange = std::max(LambdaChangeRange*0.75, sqrtEpsilon);

                states = states_last;
                delta_States = delta_States_last;

                error_this = error_last;
                error_last = error_last_last;
                delta_error = delta_error_last;

                rmsError_this = rmsError_last;
                rmsError_last = rmsError_last_last;

                deltaRmsError = rmsError_this-rmsError_last;

                delta_States_last.setZero(delta_States_last.rows(),delta_States_last.cols());
                lambdaRangeCount = 0;

                converged = false;
            }

            iteration ++;

            assert( !std::isinf(Lambda) && !std::isnan(Lambda));
            assert( !std::isinf(rmsError_this) && !std::isnan(rmsError_this));

            converged = converged && (
                        iteration >= maxAttempts ||
                        rmsError_this <converganceTolerance ||
                        (std::abs(deltaRmsError)*(1+Lambda) < converganceTolerance && delta_error.norm()!=0) ||
                        (std::abs(percentErrorChange)*(1+Lambda)/100.0 < converganceTolerance && percentErrorChange >= std::numeric_limits<double>::epsilon()*rmsError_this ) ||
                        (std::abs(percentStateChange)*(1+Lambda)/100.0 < converganceTolerance && percentStateChange >= std::numeric_limits<double>::epsilon()*normStates ) );

            // make sure we have multiple converged runs before declaring done
            if( deltaRmsError < 0 )
            {
                convergedIterations = converged*(convergedIterations+1);
                converged = converged && (convergedIterations >= numberOfConvergedIterations);
            }

            if( iteration == 1 && maxAttempts > 1 )
                converged = false;

            if( Lambda > 10000 )
                converged = true;

            if( printProgress && iteration%100 == 1)
            {
                std::cout <</* "Conv: " << (converged?"YES   ":"NO   ") << */std::setprecision(3) << "Iteration: " << iteration << "\tRMS Error: " << 1000*std::sqrt(error_this.head(3*numberOfMeasurements).squaredNorm()/(3*numberOfMeasurements)) << " mT\tDelta RMS Error: " <<  deltaRmsError*1000  << " mT\t  Error Change: " << percentErrorChange << "%" << "\tState Change: " << percentStateChange << "%";
                std::cout << std::endl;
            }

        }while( !converged );

        if( printProgress )
        {
            std::cout << std::setprecision(3) << "Iteration: " << iteration << "\tRMS Error: " << 1000*std::sqrt(error_this.head(3*numberOfMeasurements).squaredNorm()/(3*numberOfMeasurements)) << " mT\tDelta RMS Error: " <<  deltaRmsError*1000  << " mT\tPercent Error Change: " << percentErrorChange << "%" << "\tPercent State Change: " << percentStateChange << "%\tTolerance: " << converganceTolerance << "\tDONE!"<<std::endl<<std::endl;
        }

        applyPHI(states);

        if( constraint != UNIT_HEADING_ONLY && ! checkSourcePositions( printProgress ) )
        {
            if( maximumSourceToCenterDistance > 0 )
                rMaxSq = std::max(rMinSq*1.001, std::pow(maximumSourceToCenterDistance,2));

            minRadIsActive = true;
            nConst = 2;
            posWeight *= 2;
            converged = false;

            if( printProgress )
                cout << "Resolving to move sources out of workspace" << endl;
        }

    }while(!converged);

    // remove any residual errors
    // initialize coefficients with linear least squares
    linearLeastSquareCoeffFit(dataList);

    if( printStats_ )
        printStats( dataList);

    return;
}

void ElectromagnetCalibration::linearLeastSquareCoeffFit(const std::vector<MagneticMeasurement> &dataList)
{
    // ************ INITIALIZE COEFFICIENTS WITH LINEAR LEAST SQUARES ***************** //
    Eigen::VectorXd E(numberOfMeasurements*3);
    Eigen::MatrixXd JtJ(numberOfParameters - numberOfSources*6, numberOfParameters-numberOfSources*6);
    Eigen::MatrixXd J(3*numberOfMeasurements, numberOfParameters - numberOfSources*6);

    // Pack Error
    unsigned int phiRowNum = 0;
    unsigned int coilNum;
    std::vector<ScalorPotential>::iterator coilIT;
    std::vector<MagneticMeasurement>::const_iterator measIT;
    for( coilIT = coilList.begin(), coilNum = 0;
         coilIT != coilList.end();
         coilIT ++, coilNum ++ )
    {
        // for each source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = coilIT->srcList.begin(), srcNum = 0;
             srcIT != coilIT->srcList.end();
             srcIT ++, srcNum++ )
        {
            int numParam = coilIT->getNumCalibrationParameters( srcNum );

            int measColInd;
            for( measIT = dataList.begin(), measColInd = 0;
                 measIT != dataList.end();
                 measIT ++, measColInd+=3)
            {
                Eigen::MatrixXd Ft,Hessian;
                Ft.setZero(numParam,3);
                Hessian.setZero(numParam,numParam);
                coilIT->packJacobians(srcNum, measIT->AppliedCurrentVector(coilNum), measIT->Position, -measIT->Field, Ft, Hessian);

                J.block(measColInd, phiRowNum, 3, numParam-6 ) = Ft.topRows(numParam-6).transpose();

                E.segment(measColInd,3) = -measIT->Field;
            }
            phiRowNum += numParam-6;
        }
    }

    if( useOffset() )
    {
        // for each offset source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = offset.srcList.begin(), srcNum = 0;
             srcIT != offset.srcList.end();
             srcIT ++, srcNum++ )
        {
            int numParam = offset.getNumCalibrationParameters( srcNum );

            int measColInd;
            for( measIT = dataList.begin(), measColInd = 0;
                 measIT != dataList.end();
                 measIT ++,  measColInd+=3)
            {
                Eigen::MatrixXd Ft,Hessian;
                Ft.setZero(numParam,3);
                Hessian.setZero(numParam,numParam);
                offset.packJacobians(srcNum, 1.0, measIT->Position, -measIT->Field, Ft, Hessian);

                J.block(measColInd, phiRowNum, 3, numParam-6) = Ft.topRows(numParam-6).transpose();

                E.segment(measColInd, 3) = (-measIT->Field);
            }
            phiRowNum += numParam-6;
        }
    }

    JtJ = J.transpose()*J;
    Eigen::VectorXd PHI = -JtJ.fullPivLu().solve(J.transpose()*E);

    // apply initial coefficient fit to parameters
    phiRowNum = 0;
    for( coilIT = coilList.begin(), coilNum = 0;
         coilIT != coilList.end();
         coilIT ++, coilNum ++ )
    {
        // for each source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = coilIT->srcList.begin(), srcNum = 0;
             srcIT != coilIT->srcList.end();
             srcIT ++, srcNum++ )
        {

            std::vector<ScalorPotential::srcCoeff>::iterator coeffIT;
            for( coeffIT = srcIT->A_Coeff.begin(); coeffIT != srcIT->A_Coeff.end(); coeffIT ++, phiRowNum++ )
            {
                coeffIT->coeff = PHI(phiRowNum);
            }
            for( coeffIT = srcIT->B_Coeff.begin(); coeffIT != srcIT->B_Coeff.end(); coeffIT ++, phiRowNum++ )
            {
                coeffIT->coeff = PHI(phiRowNum);
            }

        }
    }

    if( useOffset() )
    {
        // for each offset source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = offset.srcList.begin(), srcNum = 0;
             srcIT != offset.srcList.end();
             srcIT ++, srcNum++ )
        {
            std::vector<ScalorPotential::srcCoeff>::iterator coeffIT;
            for( coeffIT = srcIT->A_Coeff.begin(); coeffIT != srcIT->A_Coeff.end(); coeffIT ++, phiRowNum++ )
            {
                coeffIT->coeff = PHI(phiRowNum);
            }
            for( coeffIT = srcIT->B_Coeff.begin(); coeffIT != srcIT->B_Coeff.end(); coeffIT ++, phiRowNum++ )
            {
                coeffIT->coeff = PHI(phiRowNum);
            }
        }
    }

}

void ElectromagnetCalibration::applyPHI(const Eigen::VectorXd& PHI )
{
    // initialize PHI leave all Lambda's Initialized to Zero
    unsigned int paramCount = 0;
    for( std::vector<ScalorPotential>::iterator coilIT = coilList.begin(); coilIT != coilList.end(); coilIT ++)
    {
        int numParam = coilIT->getNumCalibrationParameters();
        coilIT->unpackCalibrationState( PHI.segment(paramCount,numParam) );
        paramCount += numParam;
    }
    if( useOffset() )
    {
        int numParam = offset.getNumCalibrationParameters();
        offset.unpackCalibrationState( PHI.segment(paramCount,numParam) );
        paramCount += numParam;
    }
}

void ElectromagnetCalibration::obtainPHI( Eigen::VectorXd& PHI )
{
    // initialize PHI leave all Lambda's Initialized to Zero
    unsigned int paramCount = 0;
    for( std::vector<ScalorPotential>::iterator coilIT = coilList.begin(); coilIT != coilList.end(); coilIT ++)
    {
        int numParam = coilIT->getNumCalibrationParameters();
        coilIT->packCalibrationState( PHI.segment(paramCount,numParam) );
        paramCount += numParam;
    }
    if( useOffset() )
    {
        int numParam = offset.getNumCalibrationParameters();
        offset.packCalibrationState( PHI.segment(paramCount,numParam) );
        paramCount += numParam;
    }
}

void ElectromagnetCalibration::packError( Eigen::VectorXd& error, const std::vector<MagneticMeasurement> & dataList )
{
    // update measurement error
    assert(('Error Vector is the Wrong Size', error.rows() == 3*numberOfMeasurements + numberOfConstraints));

    int measInd;
    std::vector<MagneticMeasurement>::const_iterator measIT;
    for( measIT = dataList.begin(), measInd = 0;
         measIT != dataList.end();
         measIT ++, measInd += 3 )
    {
        assert( ('ElectromagnetCalibration::packError:  Measurement Index Out of Bounds', measInd+2 < 3*dataList.size()) );
        error.segment(measInd,3) = fieldAtPoint(measIT->AppliedCurrentVector,measIT->Position)-measIT->Field;
    }

    if( nConst > 0 )
    {
        // update Constraint Error
        std::vector<ScalorPotential>::iterator coilIT;
        for( coilIT = coilList.begin(); coilIT != coilList.end(); coilIT ++ )
        {
            // for each source
            std::vector<ScalorPotential::srcStruct>::iterator srcIT;
            for( srcIT = coilIT->srcList.begin();
                 srcIT != coilIT->srcList.end();
                 srcIT ++, measInd+=nConst)
            {

                assert( ('ElectromagnetCalibration::packError:  Measurement Index Out of Bounds', measInd < error.rows()) );
                error(measInd) = srcIT->srcDirection.squaredNorm()-1;

                if( nConst == 2 )
                {
                    // srcPosition Constraint Addition
                    double rSqSrc = (srcIT->srcPosition - pCenter).squaredNorm();
                    double rSqRatio = 1;
                    double rSqW = 1;

                    if( minRadIsActive && rSqSrc <= rMinSq && srcIT->A_Coeff.size() == 0 )
                    {
                        rSqRatio = rSqSrc/rMinSq;
                        rSqW = 1.001*rMinSq;
                    }
                    else if(rSqSrc >= rMaxSq )
                    {
                        rSqRatio = rSqSrc/rMaxSq;
                        rSqW = 0.999*rMaxSq;
                    }

                    error(measInd+1) = posWeight*(rSqRatio - 1);

                }
            }
        }

        if( useOffset() )
        {
            // for each offset source
            std::vector<ScalorPotential::srcStruct>::iterator srcIT;
            for( srcIT = offset.srcList.begin(); srcIT != offset.srcList.end(); srcIT ++, measInd += nConst)
            {

                assert( ('ElectromagnetCalibration::packError:  Measurement Index Out of Bounds', measInd < error.rows()) );
                error(measInd) = srcIT->srcDirection.squaredNorm()-1;

                if( nConst == 2)
                {
                    // srcPosition Constraint Addition
                    double rSqSrc = (srcIT->srcPosition - pCenter).squaredNorm();
                    double rSqRatio = 1;
                    double rSqW = 1;

                    if( minRadIsActive && rSqSrc <= rMinSq && srcIT->A_Coeff.size() == 0 )
                    {
                        rSqRatio = rSqSrc/rMinSq;
                        rSqW = 1.001*rMinSq;
                    }
                    else if(rSqSrc >= rMaxSq )
                    {
                        rSqRatio = rSqSrc/rMaxSq;
                        rSqW = 0.999*rMaxSq;
                    }

                    error(measInd+1) = posWeight*(rSqRatio - 1);

                }
            }

        }
    }
}

void ElectromagnetCalibration::packErrorJacobian( Eigen::MatrixXd& J, const std::vector<MagneticMeasurement> & dataList  )
{
    // Pack Jacobian
    unsigned int phiRowNum = 0;
    unsigned int coilNum;
    std::vector<ScalorPotential>::iterator coilIT;
    for( coilIT = coilList.begin(), coilNum = 0;
         coilIT != coilList.end();
         coilIT ++, coilNum ++ )
    {
        // for each source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = coilIT->srcList.begin(), srcNum = 0;
             srcIT != coilIT->srcList.end();
             srcIT ++, srcNum++ )
        {
            int numParam = coilIT->getNumCalibrationParameters( srcNum );

            int measColInd;
            std::vector<MagneticMeasurement>::const_iterator measIT;
            for(measIT = dataList.begin(), measColInd = 0;
                measIT != dataList.end();
                measIT ++, measColInd+=3)
            {
                Eigen::MatrixXd Ft,Hessian;
                Ft.setZero(numParam,3);
                Hessian.setZero(numParam,numParam);
                coilIT->packJacobians(srcNum, measIT->AppliedCurrentVector(coilNum), measIT->Position, Eigen::Vector3d(0,0,0), Ft, Hessian);

                J.block(measColInd, phiRowNum, 3, numParam ) = Ft.transpose();
            }
            phiRowNum += numParam;
        }
    }

    if( useOffset() )
    {
        // for each offset source
        int srcNum;
        std::vector<ScalorPotential::srcStruct>::iterator srcIT;
        for( srcIT = offset.srcList.begin(), srcNum = 0;
             srcIT != offset.srcList.end();
             srcIT ++, srcNum++ )
        {
            int numParam = offset.getNumCalibrationParameters( srcNum );

            int measColInd;
            std::vector<MagneticMeasurement>::const_iterator measIT;
            for(measIT = dataList.begin(), measColInd = 0;
                measIT != dataList.end();
                measIT ++, measColInd+=3)
            {
                Eigen::MatrixXd Ft,Hessian;
                Ft.setZero(numParam,3);
                Hessian.setZero(numParam,numParam);
                offset.packJacobians(srcNum, 1.0, measIT->Position, Eigen::Vector3d(0,0,0), Ft, Hessian);

                J.block(measColInd, phiRowNum, 3, numParam ) = Ft.transpose();;
            }
            phiRowNum += numParam;
        }
    }

    if( nConst > 0 )
    {
        // Add In Constraint Terms
        int constraintNum = 0;
        phiRowNum = 0;
        int measColInd = numberOfMeasurements*3;
        for( coilIT = coilList.begin(); coilIT != coilList.end(); coilIT ++ )
        {
            // for each source
            int srcNum;
            std::vector<ScalorPotential::srcStruct>::iterator srcIT;
            for( srcIT = coilIT->srcList.begin(), srcNum = 0; srcIT != coilIT->srcList.end();
                 srcIT ++, srcNum++, measColInd+=nConst)
            {
                int numParam = coilIT->getNumCalibrationParameters( srcNum );
                J.block(measColInd, phiRowNum+numParam-6, 1, 3) = 2*srcIT->srcDirection.transpose();

                if( nConst ==2 )
                {
                    // srcPosition Constraint Addition
                    double rSqSrc = (srcIT->srcPosition - pCenter).squaredNorm();
                    double rSqRatio = 1;
                    double rSqW = 1;

                    if( minRadIsActive && rSqSrc <= rMinSq && srcIT->A_Coeff.size() == 0 )
                    {
                        rSqRatio = rSqSrc/rMinSq;
                        rSqW = 1.001*rMinSq;
                    }
                    else if(rSqSrc >= rMaxSq )
                    {
                        rSqRatio = rSqSrc/rMaxSq;
                        rSqW = 0.999*rMaxSq;
                    }

                    J.block(measColInd+1, phiRowNum+numParam-3, 1, 3) = 2.0*posWeight*(srcIT->srcPosition - pCenter).transpose()/rSqW;

                }

                phiRowNum += numParam;
            }
        }

        if( useOffset() )
        {
            // for each offset source
            int srcNum;
            std::vector<ScalorPotential::srcStruct>::iterator srcIT;
            for( srcIT = offset.srcList.begin(), srcNum = 0; srcIT != offset.srcList.end();
                 srcIT ++, srcNum++, measColInd += nConst)
            {
                int numParam = offset.getNumCalibrationParameters( srcNum );

                J.block(measColInd, phiRowNum+numParam-6, 1, 3) = 2*srcIT->srcDirection.transpose();

                if( nConst == 2)
                {
                    // srcPosition Constraint Addition
                    double rSqSrc = (srcIT->srcPosition - pCenter).squaredNorm();
                    double rSqRatio = 1;
                    double rSqW = 1;

                    if( minRadIsActive && rSqSrc <= rMinSq && srcIT->A_Coeff.size() == 0 )
                    {
                        rSqRatio = rSqSrc/rMinSq;
                        rSqW = 1.001*rMinSq;
                    }
                    else if(rSqSrc >= rMaxSq )
                    {
                        rSqRatio = rSqSrc/rMaxSq;
                        rSqW = 0.999*rMaxSq;
                    }

                    J.block(measColInd+1, phiRowNum+numParam-3, 1, 3) = 2.0*posWeight*(srcIT->srcPosition - pCenter).transpose()/rSqW;

                }

                phiRowNum += numParam;
            }
        }
    }
}

void ElectromagnetCalibration::printStats( const std::vector< MagneticMeasurement >& dataList) const
{

    double percentError = 0;
    double SS_res = 0;
    double SS_total = 0;
    double SS_field = 0;

    Eigen::Vector3d avgFieldError(0,0,0);
    Eigen::Vector3d fieldTmp;
    Eigen::Vector3d avgField(Eigen::Vector3d::Zero());

    std::vector<MagneticMeasurement>::const_iterator dataIT;

    for( dataIT  = dataList.begin();
         dataIT != dataList.end();
         dataIT++ )
    {
        SS_field += dataIT->Field.squaredNorm();

    }
    double rmsField = std::sqrt(SS_field/(double)dataList.size()/3.0);

    double averageFielddMag = 0;
    for( dataIT  = dataList.begin();
         dataIT != dataList.end();
         dataIT++ )
    {
        avgField += dataIT->Field;
        averageFielddMag += dataIT->Field.norm();
        fieldTmp = fieldAtPoint(dataIT->AppliedCurrentVector, dataIT->Position );
        avgFieldError += (dataIT->Field - fieldTmp);
        percentError += sqrt((dataIT->Field - fieldTmp).squaredNorm()/3.0)/rmsField;//dataIT->Field.norm();
        SS_res += (dataIT->Field - fieldTmp).squaredNorm();
        SS_total += dataIT->Field.squaredNorm();
    }

    avgField /= (double) dataList.size();
    averageFielddMag /= (double) dataList.size();
    avgFieldError /= (double) dataList.size();
    Eigen::Matrix3d errorVariance; errorVariance.setZero(3,3);

    Eigen::Matrix3d fieldVar(Eigen::Matrix3d::Zero());
    for( dataIT  = dataList.begin();
         dataIT != dataList.end();
         dataIT++ )
    {

        fieldTmp = fieldAtPoint(dataIT->AppliedCurrentVector, dataIT->Position );
        Eigen::Vector3d fieldError = (dataIT->Field - fieldTmp);
        Eigen::Vector3d errorDiff = fieldError-avgFieldError;
        errorVariance += errorDiff*errorDiff.transpose();

        errorDiff = dataIT->Field-avgField;
        fieldVar += errorDiff*errorDiff.transpose();
    }

    fieldVar /= ((double) dataList.size()-1);
    errorVariance /= ((double) dataList.size()-1);

    percentError /= (double) dataList.size()/100.0;
    double rmsError_out = std::sqrt(SS_res/(double)dataList.size()/3.0);
    double rmsError_normalized = rmsError_out/std::sqrt(SS_field/(double)dataList.size()/3.0);
    double R_squared_out = 1- SS_res/SS_total;

    Eigen::JacobiSVD<Eigen::Matrix3d> varSVD(errorVariance,Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d S = varSVD.singularValues().asDiagonal();
    for( int i=0;i<3;i++ )
        S(i,i) = sqrt(S(i,i));
    Eigen::Matrix3d stdFieldErr = varSVD.matrixU()*S*varSVD.matrixV().transpose();

    cout << getName() << endl
         << "  Percent Error:\t" << setprecision(4) << percentError << "%" << endl
         << "  RMS Error:\t" << setprecision(4) << rmsError_out << endl
         << "  RMS Error/RMS Field: " << setprecision(4) << rmsError_normalized*100 <<"%"<< endl
         << "  R^2:\t\t" << setprecision(9) << R_squared_out << endl
         << "  Average Field Magnitude: " << setprecision(4) << averageFielddMag*1000 << " mT" << endl
         << "  Average Field Error:\t(" << setprecision(4) << (avgFieldError.transpose()*1000) << ") mT" << endl
         << "  Average Field Error:\t(" << setprecision(4) << (avgFieldError.transpose()/averageFielddMag)*100 << ") %" << endl
         << "  \nField Error Variance:\n" << "---------------\n" << setprecision(4) << avgFieldError << endl << "---------------" << endl
         << "  \nField Error stdev [mT]:\n" << "---------------\n" << setprecision(4) << stdFieldErr*1000 << endl << "---------------" << endl
         << "  \nField Error stdev [%]:\n" << "---------------\n" << setprecision(4) << stdFieldErr/averageFielddMag*100 << endl << "---------------" << endl;

    cout << endl;

    return;
}


#include <boost/random/mersenne_twister.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include <include/grassmann_pca.hpp>
#include <include/private/boost_ublas_row_iterator.hpp>

// generating a matrix of random numbers, each row will contain
// a data vector.
template <class matrix_t>
void generate_matrix(const int nb_elements, const int dimension, matrix_t& mat_data)
{
  static boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<double> dist(-1000, 1000);

  mat_data.resize(nb_elements, dimension);
  for(int i = 0; i < nb_elements; i++)
  {
    for(int j = 0; j < dimension; j++)
    {
      mat_data(i, j) = dist(rng);
    }
  }
}

void example_grassmann_pca_impl()
{
  using namespace grassmann_averages_pca;
  using namespace grassmann_averages_pca::details::ublas_helpers;
  namespace ub = boost::numeric::ublas;

  typedef ub::vector<double> data_t;                         // type of the vectors
  typedef ub::matrix<double> matrix_t;                       // type of the structure holding the data

  typedef grassmann_pca< data_t > grassmann_pca_t;           // the type of the structure for the computation of the grassmann averages pca
                                                             // data_t tells which kind of structure will be used for internal computations
                                                             // and will be received from the data iterators.

  typedef row_iter<const matrix_t> const_row_iter_t;         // iterator on the data, deferencing one
                                                             // of these iterator should yield a structure
                                                             // convertible to data_t. This iterator iterates
                                                             // over the rows of a given matrix.

  const int dimensions = 5;                   // each vector is of dimension 5
  const int max_dimension_to_compute = 3;     // we want only the first 3 basis-vectors
  const int max_iterations = 1000;            // at most 1000 iterations before giving up for the current dimension

  // generating the data points
  matrix_t mat_data;
  generate_matrix(10000, dimensions, mat_data);

  // configure the first points
  const double initial_point[] = {0.2097, 0.3959, 0.5626, 0.2334, 0.6545}; // some dummy initial point
  data_t vec_initial_point(dimensions);
  std::copy(initial_point, initial_point + dimensions, vec_initial_point.begin());
  std::vector< data_t > v_init_points(max_dimension_to_compute, vec_initial_point);

  // allocate the output
  std::vector<data_t> basis_vectors(max_dimension_to_compute);

  // the instance
  grassmann_pca_t instance;

  // some configuration
  if(!instance.set_nb_processors(4))              // using 4 processors
  {
    std::cerr << "Error while setting the number of threads" << std::endl;
    return;
  }

  if(!instance.set_max_chunk_size(1000))          // using max chunk size of 1000
  {
    std::cerr << "Error while setting the chunk size" << std::endl;
    return;
  }

  if(!instance.set_nb_steps_pca(3))               // using 3 PCA steps
  {
    std::cerr << "Error while setting the number of PCA steps" << std::endl;
    return;
  }

  bool return_calue = instance.batch_process(
    max_iterations,
    max_dimension_to_compute,
    const_row_iter_t(mat_data, 0),
    const_row_iter_t(mat_data, mat_data.size1()),
    basis_vectors.begin(),
    &v_init_points);

  if(!return_calue)
  {
    std::cerr << "Error during the computation of the GrassmannAveragesPCA" << std::endl;
  }
}


// Copyright 2002 Rensselaer Polytechnic Institute

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{
  namespace detail {
    template<typename T, typename BinaryPredicate>
    T min_with_compare(const T& x, const T& y, const BinaryPredicate& compare)
    {
      if (compare(x, y)) return x;
      else return y;
    }

    template<typename VertexListGraph, typename DistanceMatrix,
      typename BinaryPredicate, typename BinaryFunction,
      typename Infinity, typename Zero>
    bool floyd_warshall_dispatch(const VertexListGraph& g,
      DistanceMatrix& d, const BinaryPredicate &compare,
      const BinaryFunction &combine, const Infinity& inf,
      const Zero& zero)
    {
      typename graph_traits<VertexListGraph>::vertex_iterator
        i, lasti, j, lastj, k, lastk;

      for (boost::tie(k, lastk) = vertices(g); k != lastk; k++)
        for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
          if(d[*i][*k] != inf)
            for (boost::tie(j, lastj) = vertices(g); j != lastj; j++)
              if(d[*k][*j] != inf)
                d[*i][*j] =
                  detail::min_with_compare(d[*i][*j],
                                           combine(d[*i][*k], d[*k][*j]),
                                           compare);

      for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
        if (compare(d[*i][*i], zero))
          return false;
      return true;
    }
  }

  template <typename VertexListGraph, typename DistanceMatrix,
    typename BinaryPredicate, typename BinaryFunction,
    typename Infinity, typename Zero>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf,
    const Zero& zero)
  {
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexListGraph> ));

    return detail::floyd_warshall_dispatch(g, d, compare, combine,
    inf, zero);
  }

  template <typename VertexAndEdgeListGraph, typename DistanceMatrix,
    typename WeightMap, typename BinaryPredicate,
    typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const WeightMap& w,
    const BinaryPredicate& compare, const BinaryFunction& combine,
    const Infinity& inf, const Zero& zero)
  {
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexAndEdgeListGraph> ));
    BOOST_CONCEPT_ASSERT(( EdgeListGraphConcept<VertexAndEdgeListGraph> ));
    BOOST_CONCEPT_ASSERT(( IncidenceGraphConcept<VertexAndEdgeListGraph> ));

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iterator
      firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iterator first, last;

    for(boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
        d[*firstv][*firstv2] = inf;

    for(boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = zero;

    for(boost::tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf) {
        d[source(*first, g)][target(*first, g)] =
          detail::min_with_compare(
            get(w, *first),
            d[source(*first, g)][target(*first, g)],
            compare);
      } else
        d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<typename
      graph_traits<VertexAndEdgeListGraph>::directed_category,
      undirected_tag>::value;
    if (is_undirected)
    {
      for(boost::tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
          d[target(*first, g)][source(*first, g)] =
            detail::min_with_compare(
              get(w, *first),
              d[target(*first, g)][source(*first, g)],
              compare);
        else
          d[target(*first, g)][source(*first, g)] = get(w, *first);
      }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine,
      inf, zero);
  }

  namespace detail {
    template <class VertexListGraph, class DistanceMatrix,
      class WeightMap, class P, class T, class R>
    bool floyd_warshall_init_dispatch(const VertexListGraph& g,
      DistanceMatrix& d, WeightMap /*w*/,
      const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;
      WM inf =
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION());

      return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>(inf)),
        inf,
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
      class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g,
      DistanceMatrix& d, WeightMap w,
      const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      WM inf =
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION());
      return floyd_warshall_all_pairs_shortest_paths(g, d, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>(inf)),
        inf,
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

  }   // namespace detail

  template <class VertexListGraph, class DistanceMatrix, class P,
    class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_init_dispatch(g, d,
      choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
      params);
  }

  template <class VertexListGraph, class DistanceMatrix>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_init_dispatch(g, d,
      get(edge_weight, g), params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch(g, d,
      choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
      params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_noninit_dispatch(g, d,
      get(edge_weight, g), params);
  }

} // namespace boost

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#define _USE_MATH_DEFINES
#include "mesh.h"
#include "transforms.h"

#include <Eigen/Geometry>
#include <cmath>

namespace
{
  const float PI = static_cast<float>(M_PI);
}

namespace scenepic
{
  Vector interpolate(const Vector& p0, const Vector& p1, float alpha)
  {
    return alpha * p0 + (1. - alpha) * p1;
  }

  void Mesh::add_cube(
    const Color& color,
    const Transform& transform,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);
    Vector p0(-0.5, -0.5, -0.5);
    Vector p1(+0.5, -0.5, -0.5);
    Vector p2(-0.5, +0.5, -0.5);
    Vector p3(+0.5, +0.5, -0.5);
    Vector p4(-0.5, -0.5, +0.5);
    Vector p5(+0.5, -0.5, +0.5);
    Vector p6(-0.5, +0.5, +0.5);
    Vector p7(+0.5, +0.5, +0.5);

    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    m.add_quad(
      color, p0, p2, p3, p1, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p1, p3, p7, p5, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p5, p7, p6, p4, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p6, p2, p0, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p2, p6, p7, p3, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p0, p1, p5, VectorNone(), fill_triangles, add_wireframe);

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_thickline(
    const Color& color,
    const Vector& start_point,
    const Vector& end_point,
    float start_thickness,
    float end_thickness,
    const Transform& transform_init,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // For convenience
    float length = (end_point - start_point).norm() * 0.5f;
    start_thickness *= 0.5f;
    end_thickness *= 0.5f;

    // Create points
    Vector p0(-length, -start_thickness, -start_thickness);
    Vector p1(+length, -end_thickness, -end_thickness);
    Vector p2(-length, +start_thickness, -start_thickness);
    Vector p3(+length, +end_thickness, -end_thickness);
    Vector p4(-length, -start_thickness, +start_thickness);
    Vector p5(+length, -end_thickness, +end_thickness);
    Vector p6(-length, +start_thickness, +start_thickness);
    Vector p7(+length, +end_thickness, +end_thickness);

    // Add quads to new mesh
    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    m.add_quad(
      color, p0, p2, p3, p1, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p1, p3, p7, p5, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p5, p7, p6, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p6, p2, p0, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p2, p6, p7, p3, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p0, p1, p5, VectorNone(), fill_triangles, add_wireframe);

    // Transform if not unit thickline
    Transform transform = transform_init;
    if (start_point != VectorNone())
    {
      assert(end_point != VectorNone());
      Vector center = 0.5 * (start_point + end_point);
      Vector axis = end_point - start_point;
      length = axis.norm();
      Transform rotation = Transforms::rotation_to_align_x_to_axis(axis);
      Transform translation = Transforms::translate(center);
      Transform tx = translation * rotation;
      if (transform.isIdentity())
      {
        transform = tx;
      }
      else
      {
        transform *= tx;
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_cone(
    const Color& color,
    const Transform& transform,
    float truncation_height,
    std::uint32_t lat_count,
    std::uint32_t long_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    const float radius = 0.5f;
    Vector apex(-0.5f, 0., 0.);
    bool add_apex = std::abs(truncation_height - 1.) < 1e-6;
    const float base_center_x = 0.5f;
    Mesh m = Mesh("").shared_color(m_shared_color);
    for (std::uint32_t lat_index = 0; lat_index < lat_count; ++lat_index)
    {
      float alpha = static_cast<float>(lat_index) / lat_count;
      alpha = alpha * truncation_height;

      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        float phi = long_index * 2.0f * PI / long_count;
        float cosPhi = std::cos(phi);
        float sinPhi = std::sin(phi);

        Vector base_point(base_center_x, cosPhi * radius, sinPhi * radius);
        Vector xyz = interpolate(apex, base_point, alpha);
        Vector normal = (base_point - apex).cross(xyz - Vector(xyz(0), 0, 0));
        normal = normal.cross(base_point - apex);
        normal.normalize();
        m.append_vertex(xyz, normal, color);
      }
    }

    if (add_apex)
    {
      m.append_vertex(apex, Vector(-1, 0, 0), color);
    }

    for (std::uint32_t lat_index = 0; lat_index < lat_count - 1; ++lat_index)
    {
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto a = base_index + long_index;
        auto b = base_index + (long_index + 1) % long_count;
        auto c = base_index + long_index + long_count;
        auto d = base_index + (long_index + 1) % long_count + long_count;
        if (fill_triangles)
        {
          m.append_triangle(b, a, c);
          m.append_triangle(b, c, d);
        }

        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(b, d);
          m.append_line(d, c);
          m.append_line(c, a);
        }
      }
    }

    if (add_apex)
    {
      std::uint32_t lat_index = lat_count - 1;
      auto a = lat_count * long_count;
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto b = base_index + long_index;
        auto c = base_index + (long_index + 1) % long_count;

        if (fill_triangles)
        {
          m.append_triangle(b, a, c);
        }

        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(a, c);
        }
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_coordinate_axes(
    float length, float thickness, const Transform& transform)
  {
    this->check_instances();
    if (this->m_vertices.cols() == 6)
    {
      // shared color mesh
      std::cerr << "Converting shared color mesh to use vertex color "
                << "to accommodate coordinate axes.";
      VertexBuffer vertices = VertexBuffer::Zero(this->count_vertices(), 9);
      if (this->count_vertices())
      {
        vertices.leftCols(6) = this->m_vertices;
        vertices.col(6).fill(this->m_shared_color.r());
        vertices.col(7).fill(this->m_shared_color.g());
        vertices.col(8).fill(this->m_shared_color.b());
        this->m_shared_color = Color::None();
      }

      this->m_vertices = vertices;
    }
    else if (this->m_vertices.cols() == 8)
    {
      // image/texture mesh
      throw std::invalid_argument("Cannot add coordinate axes to a UV mesh");
    }

    Mesh m = Mesh("");
    m.add_thickline(
      {1, 0, 0}, {0, 0, 0}, {length, 0, 0}, thickness, 0.5f * thickness);
    m.add_thickline(
      {0, 1, 0}, {0, 0, 0}, {0, length, 0}, thickness, 0.5f * thickness);
    m.add_thickline(
      {0, 0, 1}, {0, 0, 0}, {0, 0, length}, thickness, 0.5f * thickness);
    m.add_sphere({1, 1, 1}, Transforms::scale(thickness * 1.1f));
    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_frustum(
    const Color& color,
    float fov_y_degrees,
    float aspect_ratio,
    float depth,
    float thickness,
    const Transform& transform)
  {
    this->check_instances();
    this->check_color(color);
    Mesh m = Mesh("").shared_color(m_shared_color);
    const float fov_y_half_radians = fov_y_degrees / 2.0f * PI / 180.0f;
    const float height = depth * std::sin(fov_y_half_radians);
    const float width = height * aspect_ratio;

    m.add_thickline(
      color, {0, 0, 0}, {+width, +height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color, {0, 0, 0}, {+width, -height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color, {0, 0, 0}, {-width, -height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color,
      {0.0, 0.0, 0.0},
      {-width, +height, depth},
      0.4f * thickness,
      thickness);
    m.add_thickline(
      color,
      {+width, +height, depth},
      {-width, +height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {-width, +height, depth},
      {-width, -height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {-width, -height, depth},
      {+width, -height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {+width, -height, depth},
      {+width, +height, depth},
      thickness,
      thickness);
    m.add_coordinate_axes(depth * 0.075f, thickness);

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_frustum(
    const Camera& camera, const Color& color, float thickness, float depth)
  {
    this->check_instances();
    this->check_color(color);
    Mesh m = Mesh("").shared_color(m_shared_color);
    Vector eye(0, 0, 0);
    Eigen::Vector3f top_left(-1, -1, 1);
    Eigen::Vector3f top_right(1, -1, 1);
    Eigen::Vector3f bottom_left(-1, 1, 1);
    Eigen::Vector3f bottom_right(1, 1, 1);

    Transform unprojection = camera.projection().inverse();
    top_left =
      (unprojection * top_left.homogeneous()).hnormalized().normalized() *
      depth;
    top_right =
      (unprojection * top_right.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_left =
      (unprojection * bottom_left.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_right =
      (unprojection * bottom_right.homogeneous()).hnormalized().normalized() *
      depth;

    m.add_thickline(color, eye, bottom_right, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, top_right, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, top_left, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, bottom_left, 0.4f * thickness, thickness);
    m.add_thickline(color, bottom_right, bottom_left, thickness, thickness);
    m.add_thickline(color, bottom_left, top_left, thickness, thickness);
    m.add_thickline(color, top_left, top_right, thickness, thickness);
    m.add_thickline(color, top_right, bottom_right, thickness, thickness);
    m.add_coordinate_axes(-0.075f, thickness);

    if (!camera.camera_to_world().isIdentity())
    {
      m.apply_transform(camera.camera_to_world());
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_image(const Camera& camera, float depth)
  {
    this->check_instances();
    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    Vector eye(0, 0, 0);
    Eigen::Vector3f top_left(-1, -1, 1);
    Eigen::Vector3f top_right(1, -1, 1);
    Eigen::Vector3f bottom_left(-1, 1, 1);
    Eigen::Vector3f bottom_right(1, 1, 1);

    Transform unprojection = camera.projection().inverse();
    top_left =
      (unprojection * top_left.homogeneous()).hnormalized().normalized() *
      depth;
    top_right =
      (unprojection * top_right.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_left =
      (unprojection * bottom_left.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_right =
      (unprojection * bottom_right.homogeneous()).hnormalized().normalized() *
      depth;

    m.add_quad(Color::None(), top_left, top_right, bottom_right, bottom_left);
    if (!camera.camera_to_world().isIdentity())
    {
      m.apply_transform(camera.camera_to_world());
    }

    this->append_mesh(m);
  }

  void Mesh::add_disc(
    const Color& color,
    const Transform& transform,
    std::uint32_t segment_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    float radius = 0.5f;

    VectorBuffer vertices =
      VectorBuffer::Zero(static_cast<Eigen::Index>(segment_count) + 1, 3);
    auto thetas = Eigen::ArrayXf::LinSpaced(segment_count, 0, 2.0f * PI);
    auto ys = radius * thetas.cos();
    auto zs = radius * thetas.sin();
    vertices.block(1, 1, segment_count, 1) = ys;
    vertices.bottomRightCorner(segment_count, 1) = zs;

    TriangleBuffer triangles(segment_count, 3);
    triangles.col(0).fill(0);
    triangles.col(2) = arange(0, segment_count) + 1;
    triangles.col(1) = roll(triangles.col(2).array(), 1);

    ColorBuffer colors = ColorBufferNone();
    if (!color.is_none())
    {
      colors = ColorBuffer(segment_count + 1, 3);
      colors.col(0).fill(color(0));
      colors.col(1).fill(color(1));
      colors.col(2).fill(color(2));
    }

    VectorBuffer normals =
      VectorBuffer::Zero(static_cast<Eigen::Index>(segment_count) + 1, 3);
    normals.col(0).fill(1.0f);

    this->add_mesh_with_normals(
      vertices,
      normals,
      triangles,
      colors,
      UVBufferNone(),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_cylinder(
    const Color& color,
    const Transform& transform,
    std::uint32_t segment_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Unit diameter for consistency with other primitives
    float radius = 0.5f;

    auto N = segment_count;

    // Add discs at each end of the cylinder
    // Note, we do not re-use disc vertices for the barrel, as this gives bad
    // normals
    std::array<float, 2> x_vals = {-0.5f, +0.5f};
    for (auto& x : x_vals)
    {
      auto disc_transform = Transforms::rotation_about_y(x == 0.5f ? 0 : PI);
      disc_transform = Transforms::translate({x, 0, 0}) * disc_transform;
      if (!transform.isIdentity())
      {
        disc_transform = transform * disc_transform;
      }

      this->add_disc(color, disc_transform, N, fill_triangles, add_wireframe);
    }

    auto thetas = Eigen::ArrayXf::LinSpaced(N, 0, 2.0f * PI);
    auto ys = radius * thetas.cos();
    auto zs = radius * thetas.sin();

    VectorBuffer vertices(2 * N, 3);
    vertices.topLeftCorner(N, 1).fill(-0.5f);
    vertices.bottomLeftCorner(N, 1).fill(+0.5f);
    vertices.block(0, 1, N, 1) = ys;
    vertices.block(N, 1, N, 1) = ys;
    vertices.topRightCorner(N, 1) = zs;
    vertices.bottomRightCorner(N, 1) = zs;

    VectorBuffer normals = vertices;
    normals.col(0).fill(0.0f);

    TriangleBuffer triangles(2 * N, 3);
    auto range = arange(0, N);
    triangles.topLeftCorner(N, 1) = roll(range, 1);
    triangles.block(0, 1, N, 1) = range;
    triangles.topRightCorner(N, 1) = range + N;
    triangles.bottomLeftCorner(N, 1) = roll(range, 1);
    triangles.block(N, 1, N, 1) = range + N;
    triangles.bottomRightCorner(N, 1) = roll(range, 1) + N;

    ColorBuffer colors = ColorBufferNone();
    if (!color.is_none())
    {
      colors = ColorBuffer(2 * N, 3);
      colors.col(0).fill(color(0));
      colors.col(1).fill(color(1));
      colors.col(2).fill(color(2));
    }

    this->add_mesh_with_normals(
      vertices,
      normals,
      triangles,
      colors,
      UVBufferNone(),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_sphere(
    const Color& color,
    const Transform& transform,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->add_icosphere(color, transform, 2, fill_triangles, add_wireframe);
  }

  void Mesh::add_icosphere(
    const Color& color,
    const Transform& transform,
    std::uint32_t steps,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Create a basic icosohedron primitive

    // Leads to unit diameter for consistency with other primitives
    const float radius = 0.5f;
    const float t = 0.5f * (1.0f + std::sqrt(5.0f));
    VectorBuffer vertex_positions(12, 3);
    vertex_positions << -1.0, +t, 0.0, +1.0, +t, 0.0, -1.0, -t, 0.0, +1.0, -t,
      0.0, 0.0, -1.0, +t, 0.0, +1.0, +t, 0.0, -1.0, -t, 0.0, +1.0, -t, +t, 0.0,
      -1.0, +t, 0.0, +1.0, -t, 0.0, -1.0, -t, 0.0, +1.0;
    vertex_positions.rowwise().normalize();
    vertex_positions *= radius;

    TriangleBuffer triangles(20, 3);
    triangles << 0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11, 1, 5, 9, 5,
      11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8, 3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8,
      3, 8, 9, 4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1;

    typedef std::pair<std::uint32_t, std::uint32_t> Edge;
    // Apply subdivision
    for (std::uint32_t iter = 0; iter < steps; ++iter)
    {
      std::map<Edge, std::uint32_t> new_e_vs;
      TriangleBuffer new_triangles;

      // For each triangle we will make up to 3 new e-verts
      // (some will have less when shared with neighbouring triangles)
      for (auto triangle = 0; triangle < triangles.rows(); ++triangle)
      {
        auto a = triangles(triangle, 0);
        auto b = triangles(triangle, 1);
        auto c = triangles(triangle, 2);
        std::map<Edge, std::uint32_t> v_idx;

        // Loop over triangle edges
        std::array<Edge, 3> edges = {Edge(a, b), Edge(a, c), Edge(b, c)};
        for (const auto& edge : edges)
        {
          Edge back_edge(edge.second, edge.first);
          // Check this edge does not already have a new edge-vertex
          if (new_e_vs.count(edge))
          {
            v_idx[edge] = new_e_vs[edge];
          }
          else if (new_e_vs.count(back_edge))
          {
            v_idx[edge] = new_e_vs[back_edge];
          }
          else
          {
            Vector v = vertex_positions.row(edge.first) +
              vertex_positions.row(edge.second);
            v.normalize();
            v *= radius;
            v_idx[edge] = new_e_vs[edge] =
              static_cast<std::uint32_t>(vertex_positions.rows());
            append_row(vertex_positions, v);
          }
        }

        // Append new triangles
        append_row(
          new_triangles, Triangle(a, v_idx[Edge(a, b)], v_idx[Edge(a, c)]));
        append_row(
          new_triangles,
          Triangle(v_idx[Edge(a, b)], v_idx[Edge(b, c)], v_idx[Edge(a, c)]));
        append_row(
          new_triangles, Triangle(v_idx[Edge(a, c)], v_idx[Edge(b, c)], c));
        append_row(
          new_triangles, Triangle(v_idx[Edge(a, b)], b, v_idx[Edge(b, c)]));
      }

      // Replace existing triangles with new triangles
      triangles = new_triangles;
    }

    // Set up per-vertex colors if required
    ColorBuffer colors;
    if (!color.is_none())
    {
      colors = ColorBuffer(vertex_positions.rows(), 3);
      colors.col(0).fill(color.r());
      colors.col(1).fill(color.g());
      colors.col(2).fill(color.b());
    }

    // Set up per-vertex uvs if required
    UVBuffer uvs;
    if (!m_texture_id.empty())
    {
      // Compute azimuth and inclination values into the uv buffer
      uvs.resize(vertex_positions.rows(), Eigen::NoChange);
      for (auto v_idx = 0; v_idx < vertex_positions.rows(); ++v_idx)
      {
        uvs(v_idx, 0) = static_cast<float>(
          0.5f -
          0.5f *
            std::atan2(vertex_positions(v_idx, 2), vertex_positions(v_idx, 0)) /
            PI); // Azimuth
        uvs(v_idx, 1) = static_cast<float>(
          1.0f -
          std::acos(vertex_positions(v_idx, 1) * 2.0f) / PI); // Inclination
      }

      // Duplicate vertices across the longitude seam - not particularly
      // efficient and bit of a hack but works ok.  Results in non watertight
      // mesh. Loop over triangles
      TriangleBuffer new_triangles;
      for (auto triangle = 0; triangle < triangles.rows(); ++triangle)
      {
        auto a = triangles(triangle, 0);
        auto b = triangles(triangle, 1);
        auto c = triangles(triangle, 2);

        // Rotate triangle indices to ensure a is east most vertex, preserving
        // triangle winding order
        for (std::size_t attempt = 0; attempt < 2; ++attempt)
        {
          if (uvs(a, 0) < uvs(b, 0) || uvs(a, 0) < uvs(c, 0))
          {
            std::tie(a, b, c) = std::make_tuple(b, c, a);
          }
        }

        // Identify hemisphere for a, b, c
        // Hack - doesn't look great at poles for small number of subdiv steps
        bool a_isEast = uvs(a, 0) > 0.66f;
        bool b_isWest = uvs(b, 0) < 0.33f;
        bool c_isWest = uvs(c, 0) < 0.33f;

        // Duplicate vertices if necessary
        if (a_isEast && c_isWest) // Duplicate c vertex
        {
          Vertex new_vertex = vertex_positions.row(c);
          append_row(vertex_positions, new_vertex); // Concatenate a copy of c
          append_row(uvs, Eigen::Vector2f(1.0f + uvs(c, 0), uvs(c, 1)));
          c = static_cast<uint32_t>(
            vertex_positions.rows() - 1); // Will overwrite c in triangle
        }
        if (a_isEast && b_isWest) // Duplicate b vertex
        {
          Vertex new_vertex = vertex_positions.row(b);
          append_row(vertex_positions, new_vertex); // Concatenate a copy of b
          append_row(uvs, Eigen::Vector2f(1.0f + uvs(b, 0), uvs(b, 1)));
          b = static_cast<uint32_t>(
            vertex_positions.rows() - 1); // Will overwrite b in triangle
        }

        // Save the new triangle
        append_row(new_triangles, Triangle(a, b, c));
      }

      // Replace existing triangles with new triangles
      triangles = new_triangles;
    }

    // Populate mesh

    // vertices double as normals given they lie on unit sphere
    this->add_mesh_with_normals(
      std::move(vertex_positions),
      std::move(vertex_positions),
      std::move(triangles),
      std::move(colors),
      std::move(uvs),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_uv_sphere(
    const Color& color,
    const Transform& transform,
    std::uint32_t lat_count,
    std::uint32_t long_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Add vertices

    // Leads to unit diameter for consistency with other primitives
    const double radius = 0.5;
    Mesh m = Mesh("").shared_color(m_shared_color);
    for (std::uint32_t lat_index = 0; lat_index < lat_count + 1; ++lat_index)
    {
      double theta = lat_index * M_PI / lat_count;
      double cosTheta = std::cos(theta);
      double sinTheta = std::sin(theta);
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        double phi = long_index * 2.0 * M_PI / long_count;
        double cosPhi = std::cos(phi);
        double sinPhi = std::sin(phi);

        double dx = radius * cosPhi * sinTheta;
        double dy = radius * cosTheta;
        double dz = radius * sinPhi * sinTheta;
        Vector pos = Eigen::Vector3d(dx, dy, dz).cast<float>();
        m.append_vertex(pos, pos, color);
      }
    }

    // Add triangles
    for (std::uint32_t lat_index = 0; lat_index < lat_count; ++lat_index)
    {
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto a = base_index + long_index;
        auto b = base_index + (long_index + 1) % long_count;
        auto c = base_index + long_index + long_count;
        auto d = base_index + (long_index + 1) % long_count + long_count;
        if (fill_triangles)
        {
          if (lat_index > 0)
          {
            m.append_triangle(a, b, c);
          }
          m.append_triangle(c, b, d);
        }
        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(b, d);
          m.append_line(d, c);
          m.append_line(c, a);
        }
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

} // namespace scenepic