/***************************************************************************
 *   Copyright (C) 2016 - 2017 by                                          *
 *      Tarek Taha, KURI  <tataha@tarektaha.com>                           *
 *      Husameldin Mukhtar, VSAP  <husameldin.mukhtar@kustar.ac.ae>        *
 *                                                                         *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Steet, Fifth Floor, Boston, MA  02111-1307, USA.          *
 ***************************************************************************/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <cmath>

ros::Publisher planePub;
ros::Publisher wrenchPub;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2 cloud_filtered;
  // Convert to PCL data type
  pcl_conversions::toPCL(*input, *cloud);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 planeOutput,wrenchOutput;
  pcl_conversions::fromPCL(cloud_filtered, planeOutput);
  //planeOutput = *input;
  //wrenchOutput = *input;

  ROS_INFO("Received Data");

  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*input,pcl_pc2);

  pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(pcl_pc2,*inputCloud);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
   // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud(inputCloud);
  seg.segment (*inliers, *coefficients);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  PointCloud::Ptr planePoints(new PointCloud);
  PointCloud::Ptr wrenchPoints(new PointCloud);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  else
  {
    std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " "<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;
    std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
    // Extract the inliers
    extract.setInputCloud (inputCloud);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*planePoints);
    std::cerr << "PointCloud representing the planar component: " << planePoints->width * planePoints->height << " data points." << std::endl;

    pcl::toROSMsg(*planePoints, planeOutput);
    planeOutput.header.frame_id = "camera_rgb_optical_frame";
    planeOutput.header.stamp = ros::Time::now();

    //std::cerr << "depth: "<< planeOutput->fields << std::endl;
    //ROS_INFO(planeOutput.z())

    //printf ("Cloud: width = %d, height = %d\n", planePoints->width, planePoints->height);
    BOOST_FOREACH (const pcl::PointXYZ& pt, planePoints->points)
    //printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);

    planePub.publish(planeOutput);

    //=============================================================================================
    // Plane rotation angle
    //=============================================================================================
    double normalLength = sqrt((coefficients->values[0] * coefficients->values[0]) + (coefficients->values[1] * coefficients->values[1]) + (coefficients->values[2] * coefficients->values[2]));

    pcl::PointXYZ normalXY;
    pcl::PointXYZ normalGround;
    pcl::PointXYZ result;
    normalXY.x = 0.0;
    normalXY.y = 0.0;
    normalXY.z = 1.0;

    normalGround.x = coefficients->values[0]/normalLength;
    normalGround.y = coefficients->values[1]/normalLength;
    normalGround.z = coefficients->values[2]/normalLength;

    result.x = normalGround.y * normalXY.z - normalXY.y*normalGround.z;
    result.y = normalXY.x*normalGround.z - normalGround.x*normalXY.z;
    result.z = normalGround.x*normalXY.y - normalGround.y*normalXY.x;
    double resultLenght = sqrt((result.x * result.x) + (result.y * result.y) +(result.z * result.z));
    result.x /=resultLenght;
    result.y /=resultLenght;
    result.z /=resultLenght;
    double theta = std::acos(normalGround.z/sqrt((normalGround.x* normalGround.x)+(normalGround.y*normalGround.y)+(normalGround.z*normalGround.z)));
    //std::cout << "The crossproduct is " << result.x << " "<< result.y<< " "<< result.z <<std::endl;
    //std::cout << "norm " << std::sqrt((result.x * result.x  )+(result.y * result.y)+ (result.z * result.z))<< std::endl;

    printf ("theta = %f\n", theta*180/3.14159);

    //=============================================================================================
    // Normals of plane points (For testing. Can be removed later!)
    //=============================================================================================

    // Create the normal estimation class, and pass the input dataset to it
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud (planePoints);

    // Create an empty kdtree representation, and pass it to the normal estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    ne.setSearchMethod (tree);

    // Output datasets
    pcl::PointCloud<pcl::Normal>::Ptr plane_normals (new pcl::PointCloud<pcl::Normal>);

    // Use all neighbors in a sphere of radius 3cm
    ne.setRadiusSearch (0.03);

    // Compute the features
    ne.compute (*plane_normals);

    //printf ("n_x = %f, n_y = %f, n_z = %f\n", plane_normals->at(100).normal[0],plane_normals->at(100).normal[1],plane_normals->at(100).normal[2]);

    // visualize normals
    //pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    //viewer.setBackgroundColor (0.0, 0.0, 0.5);
    //viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(planePoints, plane_normals);

    //while (!viewer.wasStopped ())
    //{
    //  viewer.spinOnce ();
    //}
    //return 0;

    //===================================================================================================

    // Extract the outliers
    extract.setInputCloud (inputCloud);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*wrenchPoints);
    std::cerr << "PointCloud representing the wrench component: " << wrenchPoints->width * wrenchPoints->height << " data points." << std::endl;

    pcl::toROSMsg(*wrenchPoints, wrenchOutput);
    wrenchOutput.header.frame_id = "camera_rgb_optical_frame";
    wrenchOutput.header.stamp = ros::Time::now();

    wrenchPub.publish(wrenchOutput);
    /*
    for (size_t i = 0; i < inliers->indices.size (); ++i)
    {
      std::cerr << inliers->indices[i] << "    " << inputCloud->points[inliers->indices[i]].x << " "<< inputCloud->points[inliers->indices[i]].y << " "<< inputCloud->points[inliers->indices[i]].z << std::endl;
    }
    */
  }

}

int main (int argc, char** argv)
{

  // Initialize ROS
  ros::init (argc, argv, "panel_orientation");
  ros::NodeHandle nh;
  std::string topic = nh.resolveName("point_cloud");;
  // Create a ROS subscriber for the input point cloud
  // ros::Subscriber sub = nh.subscribe ("/camera/depth_registered/points", 1, cloud_cb);
  //ros::Subscriber sub = nh.subscribe ("/velodyne_points", 1, cloud_cb);
  ros::Subscriber sub = nh.subscribe ("/camera/depth/points", 1, cloud_cb);
  planePub      = nh.advertise<sensor_msgs::PointCloud2>("/plane_points", 1);
  wrenchPub     = nh.advertise<sensor_msgs::PointCloud2>("/wrench_points", 1);

  // Spin
  ros::spin ();
}


#define BOOST_TEST_TOOLS_UNDER_DEBUGGER
#define BOOST_TEST_MODULE kite_model_test
#include <boost/test/included/unit_test.hpp>

#include "integrator.h"
#include "kite.h"

using namespace casadi;

BOOST_AUTO_TEST_SUITE( kite_model_suite_test )

BOOST_AUTO_TEST_CASE( ode_solver_test )
{
    std::string kite_config_file = "umx_radian.yaml";
    KiteProperties kite_props = kite_utils::LoadProperties(kite_config_file);
    AlgorithmProperties algo_props;
    algo_props.Integrator = RK4;

    /** SCALING */
    //kite_props.Tether.length = kite_props.Tether.length / 3;

    KiteDynamics kite(kite_props, algo_props);
    Function ode = kite.getNumericDynamics();

    /** compare three ode solvers */
    Dict opts;
    opts["tf"]         = 5.0;
    opts["poly_order"] = 41;
    opts["tol"]        = 1e-4;
    opts["method"]     = IntType::RK4;
    ODESolver rk4_solver(ode, opts);

    opts["method"] = IntType::CVODES;
    ODESolver cvodes_solver(ode, opts);

    opts["method"] = IntType::CHEBYCHEV;
    ODESolver chebychev_solver(ode, opts);

    //kite_props.Tether.length = kite_props.Tether.length / 3;
    //std::cout << "New tether length : " << kite_props.Tether.length << "\n";

    KiteDynamics kite2(kite_props, algo_props);
    Function ode2 = kite.getNumericDynamics();
    double tf = 7.0;
    casadi::DMDict props;
    props["scale"] = 0;
    props["P"] = casadi::DM::diag(casadi::DM({0.1, 1/3.0, 1/3.0, 1/2.0, 1/5.0, 1/2.0, 1/3.0, 1/3.0, 1/3.0, 1.0, 1.0, 1.0, 1.0}));
    props["R"] = casadi::DM::diag(casadi::DM({1/0.15, 1/0.2618, 1/0.2618}));
    PSODESolver<10,40,13,3>ps_solver(ode, tf, props);

    /** solve a problem */
    DM rk4_sol, cheb_sol, cv_sol, ps_sol;
    //DM init_state = DM::vertcat({0.318732, 0.182552, 0.254833, 1.85435, -0.142882, -0.168359,
    //                          -0.229383, -0.0500282, -0.746832, 0.189409, -0.836349, -0.48178, 0.180367});
    //DM control = DM::vertcat({0.3, kmath::deg2rad(5), -kmath::deg2rad(2)});

    DM init_state = DM::vertcat({6.1977743e+00,  -2.8407148e-02,   9.1815942e-01,   2.9763089e-01,  -2.2052198e+00,  -1.4827499e-01,
                                 -4.1624807e-01, -2.2601052e+00,   1.2903439e+00,   3.5646195e-02,  -6.9986094e-02,   8.2660637e-01,   5.5727089e-01});
    DM control = DM::vertcat({0.1, 0.0, 0.0});

    std::chrono::time_point<std::chrono::system_clock> start = kite_utils::get_time();

    rk4_sol  = rk4_solver.solve(init_state, control, tf);
    std::chrono::time_point<std::chrono::system_clock> rk4_stop = kite_utils::get_time();

    //cv_sol   = cvodes_solver.solve(init_state, control, tf);
    std::chrono::time_point<std::chrono::system_clock> cv_stop = kite_utils::get_time();

    cheb_sol = chebychev_solver.solve(init_state, control, tf);
    std::chrono::time_point<std::chrono::system_clock> cheb_stop = kite_utils::get_time();

    bool FULL = true;
    ps_sol = ps_solver.solve(init_state, control, FULL);
    std::chrono::time_point<std::chrono::system_clock> ps_stop = kite_utils::get_time();

    auto rk4_duration = std::chrono::duration_cast<std::chrono::microseconds>(rk4_stop - start);
    auto cv_duration = std::chrono::duration_cast<std::chrono::microseconds>(cv_stop - rk4_stop);
    auto cheb_duration = std::chrono::duration_cast<std::chrono::microseconds>(cheb_stop - cv_stop);
    auto ps_duration = std::chrono::duration_cast<std::chrono::microseconds>(ps_stop - cheb_stop);

    std::cout << "RK4 solve time: " << std::setprecision(6)
              << static_cast<double>(rk4_duration.count()) * 1e-6 << " [seconds]" << "\n";
    //std::cout << "CVODES solve time: " << std::setprecision(6)
    //          << static_cast<double>(cv_duration.count()) * 1e-6 << " [seconds]" << "\n";
    std::cout << "CHEB solve time: " << std::setprecision(6)
              << static_cast<double>(cheb_duration.count()) * 1e-6 << " [seconds]" << "\n";
    std::cout << "PS solve time: " << std::setprecision(6)
              << static_cast<double>(ps_duration.count()) * 1e-6 << " [seconds]" << "\n";

    std::cout << "RK4: " <<  rk4_sol << "\n";
    std::cout << "CVODES: " <<  cv_sol << "\n";
    std::cout << "CHEB: " <<  cheb_sol << "\n";
    std::cout << "PS:" << ps_sol(Slice(0, 13)) << "\n";

    std::ofstream trajectory_file("integrated_trajectory.txt", std::ios::out);

    if(!trajectory_file.fail())
    {
        for (int i = 0; i < ps_sol.size1(); i = i + 13)
        {
            std::vector<double> tmp = ps_sol(Slice(i, i + 13),0).nonzeros();
            for (uint j = 0; j < tmp.size(); j++)
            {
                trajectory_file << tmp[j] << " ";
            }
            trajectory_file << "\n";
        }
    }
    trajectory_file.close();

    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_SUITE_END()


#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;
}


/**
 * \addtogroup cpp_tools
 * @{
 * \addtogroup cpp_tools_cdx_convert_continuous_to_discrete cdx-convert-continuous-to-discrete
 * @{
 *
 * \file cdx-convert-continuous-to-discrete.cpp
 *
 * \date Nov 24, 2011
 * \author Frank M. Schubert
 *
 * \brief This file contains the implementation of a tool which converts continuous-
 * delay CDX files to discrete-delay CDX files.
 *
 */

#ifndef __APPLE__
#include <omp.h>
#endif

#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/program_options.hpp> // for reading command line parameters
namespace po = boost::program_options;

#include <armadillo>

#include "cdx/WriteDiscreteDelayFile.h"
#include "cdx/ReadContinuousDelayFile.h"

/**
 * \brief Computes the Hamming window function
 *
 * \param[in] n number of coefficients
 * \return vector with window coefficients
 */
arma::vec hamming(const int n) {
	arma::vec t(n);

	if (n == 1)
		t(0) = 0.08;
	else
		for (int i = 0; i < n; i++)
			t[i] = (0.54 - 0.46 * std::cos(2.0 * M_PI * i / (n - 1)));

	return t;
}

/**
 * \brief Mirrors a signal after fft.
 *
 * \param[in] v incoming signal vector
 * \return mirrored vector
 */
arma::cx_vec fftshift(const arma::cx_vec &v) {
	const int nr = v.size() / 2;
	return arma::join_vert(v.rows(nr, v.size() - 1), v.rows(0, nr - 1));
}

using namespace std;

int main(int argc, char **argv) {
	cout
			<< "\n==== Convert continuous-delay to discrete-delay CDX file ====\n\n";

	cout << "Copyright (C) 2011-2016  Frank M. Schubert\n"
			<< "This program comes with ABSOLUTELY NO WARRANTY.\n"
			<< "This is free software, and you are welcome to redistribute it\n"
			<< "under certain conditions. See the licenses/gpl-3.0.txt file for more information.\n";

	// Declare the supported command line options:
	// from http://www.boost.org/doc/libs/1_55_0b1/doc/html/program_options/tutorial.html
	po::options_description desc("Allowed options");
	desc.add_options()("help", "produce help message")("input-file,i",
			po::value<string>(), "input continuous-delay CDX file")(
			"output-file,o", po::value<string>(),
			"output discrete-delay CDX file")("sampling-frequency,s",
			po::value<double>(),
			"sampling frequency of the delay axis in Hz of the resulting CDX file")(
			"delay-before-min,b", po::value<double>(),
			"delay to add before minimum delay")("delay-after-max,a",
			po::value<double>(), "delay to add after maximum delay")(
			"enable-filtering,e", po::bool_switch(), "enable filtering")(
			"subtract-reference-delay,r", po::bool_switch(),
			"subtract the reference for each CIR")("type,t",
			po::value<unsigned>(),
			"only process components with types equal to this number.");

	// parse command line options:
	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << endl;
		exit(0);
	}

	if (vm.count("input-file") != 1)
		throw std::runtime_error("no input file name given.");

	const string input_file = vm["input-file"].as<string>();

	if (vm.count("output-file") != 1)
		throw std::runtime_error("no input file name given.");

	const string output_file = vm["output-file"].as<string>();

	const double smpl_freq = vm["sampling-frequency"].as<double>();

	const double delay_before_min = vm["delay-before-min"].as<double>();
	const double delay_after_max = vm["delay-after-max"].as<double>();
	const bool filter_enabled = vm["enable-filtering"].as<bool>();
	const bool subtract_ref_delay_from_components =
			vm["subtract-reference-delay"].as<bool>();

	const bool filter_by_types = vm.count("type") == 0 ? false : true;
	unsigned type_to_process = 0;
	if (filter_by_types == true) {
		cout << "info: filtering by types, only components with type equal to "
				<< type_to_process << "will be processed.\n";
		type_to_process = vm["type"].as<unsigned>();
	}

	cout << "info: open input file: " << input_file << "...\n";

	CDX::ReadContinuousDelayFile cdx_in(input_file);

	const vector<string> link_names = cdx_in.get_link_names();

	// open CDX output file for interpolation results:
	const double param_c0_m_s = cdx_in.get_c0_m_s();
	cout << "info: c0 = " << param_c0_m_s << " m/s\n";

	const double cir_rate_Hz = cdx_in.get_cir_rate_Hz();
	cout << "info: cir_rate_Hz = " << cir_rate_Hz << " Hz\n";

	const unsigned int nof_cirs = cdx_in.get_nof_cirs();
	cout << "info: number of CIRs in file: " << nof_cirs << "\n";

	const double transmitter_frequency_Hz =
			cdx_in.get_transmitter_frequency_Hz();
	cout << "info: transmitter_frequency: " << transmitter_frequency_Hz
			<< " Hz\n";

	cout << "info: opening output_file: " << output_file << "...\n";
	CDX::WriteDiscreteDelayFile cdx_out = CDX::WriteDiscreteDelayFile(
			output_file, param_c0_m_s, cir_rate_Hz, transmitter_frequency_Hz,
			link_names, smpl_freq);

	for (size_t link = 0; link < link_names.size(); link++) {
		cout << "process: reading CIRs of link " << link_names.at(link)
				<< " ... ";
		cout.flush();

		// read in whole input file
		vector<CDX::cir_t> cirs(nof_cirs);
		vector<double> reference_delays(nof_cirs);

		// find minimum and maximum delay in file:
		double delay_min = std::numeric_limits<double>::max();
		double delay_max = std::numeric_limits<double>::min();

		for (unsigned int k = 0; k < nof_cirs; k++) {
			cirs.at(k) = cdx_in.get_cir(link_names.at(link), k);
			reference_delays.at(k) = cirs.at(k).ref_delay;

			// read components
			for (size_t n = 0; n < cirs.at(k).components.size(); n++) {
				if (subtract_ref_delay_from_components == true)
					cirs.at(k).components.at(n).delay -= reference_delays.at(k);

				if (cirs.at(k).components.at(n).delay < delay_min) {
					delay_min = cirs.at(k).components.at(n).delay;
				}
				if (cirs.at(k).components.at(n).delay > delay_max) {
					delay_max = cirs.at(k).components.at(n).delay;
				}
			}
		}

		// correct reference delay if necessary:
		if (subtract_ref_delay_from_components == true)
			for (size_t k = 0; k < reference_delays.size(); k++)
				reference_delays.at(k) = 0.0;

		cout << "done.\n";
		cout << "process: minimum delay in file: " << delay_min << "\n";
		cout << "process: maximum delay in file: " << delay_max << "\n";
		cout << "info: delay before min delay: " << delay_before_min << "\n";
		cout << "info: delay after max delay: " << delay_after_max << "\n";

		// compute delay length of interpolation interval:
		const double interval_length = (delay_max + delay_after_max)
				- (delay_min - delay_before_min);
		cout << "process: interpolation interval: " << interval_length << "\n";

		// calculate interpolated CIR length:
		const size_t nof_coeffs = round(interval_length * smpl_freq);
		cout << "process: number of coefficients: " << nof_coeffs << "\n";

		cout << "info: smpl_freq = " << smpl_freq << "\n";

		cout << "info: allocating "
				<< nof_cirs * nof_coeffs * 16 / 1024.0 / 1024.0 << " MB... ";
		cout.flush();

		arma::cx_mat interp_cirs(nof_cirs, nof_coeffs);
		interp_cirs.zeros();
		cout << "done.\n";

		cout << "process: interpolating CIRs... " << endl;
		cout.flush();
		boost::progress_display show_progress(nof_cirs);

		const double interpolation_bandwidth = smpl_freq / 2.0;
		const double Om = 2.0 * M_PI * (interpolation_bandwidth);

		const arma::cx_rowvec window = fftshift(
				arma::conv_to<arma::cx_vec>::from(hamming(nof_coeffs))).st();

		if (window.size() != nof_coeffs)
			throw std::runtime_error("window.size() != nof_coeffs");

		size_t k, c;
		size_t n;
#pragma omp parallel for private (k, c, n)
		// for all CIRs
		for (k = 0; k < nof_cirs; k++) {
			for (c = 0; c < cirs.at(k).components.size(); c++) { // for all components
				if (filter_by_types == true
						and type_to_process != cirs.at(k).components.at(c).type)
					continue;

				for (n = 0; n < nof_coeffs; n++) { // for all coefficients
					const double fac = Om
							* (static_cast<double>(n) / smpl_freq
									- (cirs.at(k).components.at(c).delay
											- delay_min + delay_before_min));
					const double sin_term = boost::math::sinc_pi(fac);

					interp_cirs(k, n) += cirs.at(k).components.at(c).amplitude
							* sin_term;

					/*
					 const double fac = Om
					 * (static_cast<double>(n) / smpl_freq
					 - (cirs[k].components[c].delay
					 + delay_before_min));
					 const double sin_term = sin(fac) / fac;

					 interp_cirs(k, n) += cirs[k].components[c].weight
					 * sin_term;
					 */
				}
			}

			if (filter_enabled == true) {
				// filter the CIR:

				// fft is not thread-safe, so we must do:
#pragma omp critical
				{
					const arma::cx_rowvec cir_fft = arma::fft(
							interp_cirs.row(k));
					const arma::cx_rowvec cir_fft_window = cir_fft % window;
					const arma::cx_rowvec filtered_cir = arma::ifft(
							cir_fft_window);
					interp_cirs.row(k) = filtered_cir;
				}
			}

			++show_progress;
		}
		cout << endl << "process: writing CIRs... ";
		cout.flush();

		// setup link in CDX file:
		cdx_out.setup_link(link_names.at(link), nof_coeffs, delay_min);

		for (size_t n = 0; n < nof_cirs; n++) { // for all CIRs

			// convert armadillo vec to stdlib vec:
			vector<complex<double> > std_cir = arma::conv_to<
					vector<complex<double> > >::from(interp_cirs.row(n));

			cdx_out.append_cir_snapshot(link_names.at(link), std_cir,
					reference_delays.at(n) + delay_before_min);
		}
		cout << "done.\n";
	}
	cout << "all done. exit.\n";
	return 0;
}

/** @} */
/** @} */


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename K, typename L>
BOOST_FORCEINLINE void rgb2yuv_work(const K& r, const K& g, const K& b, L *y, L *u, L *v)
{
  *y = 0.299f*r + 0.587f*g + 0.114f*b;
  *u = 0.492f*(b - *y);
  *v = 0.877f*(r - *y);
}

template<typename T> struct rgb2yuv_scalar
{
  template<typename Setup>
  rgb2yuv_scalar(Setup const& s)
                    :  height(boost::fusion::at_c<0>(s))
                    ,  width(boost::fusion::at_c<1>(s))
                    ,  size_(height*width)
  {
    y.resize(size_);
    u.resize(size_);
    v.resize(size_);
    r.resize(size_);
    g.resize(size_);
    b.resize(size_);
    for(std::size_t i=0; i<size_; i++)
      r[i] = g[i] = b[i] = y[i] = u[i] = v[i] = T(i);
  }

  void operator()()
  {
    for(std::size_t it = 0; it < size_; it++)
      rgb2yuv_work(r[it], g[it], b[it], &y[it], &u[it], &v[it]);
  }

  friend std::ostream& operator<<(std::ostream& os, rgb2yuv_scalar<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

private:
  std::size_t height;
  std::size_t width;
  std::size_t size_;
  std::vector<T> r, g, b, y, u, v;
};

NT2_REGISTER_BENCHMARK_TPL( rgb2yuv_scalar, (float) )
{
  std::size_t hmin  = args("hmin",  32);
  std::size_t hmax  = args("hmax", 128);
  std::size_t hstep = args("hstep",  2);
  std::size_t wmin  = args("wmin",  32);
  std::size_t wmax  = args("wmax", 128);
  std::size_t wstep = args("wstep",  2);

  run_during_with< rgb2yuv_scalar<float> > ( 1.
                                          , and_( geometric(hmin,hmax,hstep)
                                                , geometric(wmin,wmax,wstep)
                                                )
                                          , cycles_per_element<stats::median_>()
                                          );
}


#include "gtest/gtest.h"
#include <Eigen/Core>
#include "MeshDivide.h"
#include "MeshFactory.h"

namespace {
  using namespace Geotree;

  class MeshDivideTest : public ::testing::Test {
  };

  TEST_F(MeshDivideTest, Cube0)
  {
    MeshFactory factory;
    Mesh cube0 = factory.cube(10,10,10);
    Mesh cube1 = factory.cube(10,1,1);
    cube1.translate(Vector3d(1, 1, 1));

    IntersectionPoints points(cube0, cube1);
    Paths paths(points);
    MeshDivide divide(paths);

    EXPECT_EQ(divide.facesMesh0A.size(), 10);
    EXPECT_EQ(divide.facesMesh0B.size(), 0);
    EXPECT_EQ(divide.facesMesh1A.size(), 2);
    EXPECT_EQ(divide.facesMesh1B.size(), 2);

    EXPECT_FALSE(divide.inside0A);
    EXPECT_FALSE(divide.inside0B);
    EXPECT_TRUE(divide.inside1A);
    EXPECT_FALSE(divide.inside1B);
  }

  TEST_F(MeshDivideTest, Cube1)
  {
    MeshFactory factory;
    Mesh cube0 = factory.cube(10,10,10);
    Mesh cube1 = factory.cube(10,10,10);
    cube1.translate(Vector3d(1, 1, 1));

    IntersectionPoints points(cube0, cube1);
    Paths paths(points);
    MeshDivide divide(paths);

    EXPECT_EQ(divide.facesMesh0A.size(), 6);
    EXPECT_EQ(divide.facesMesh0B.size(), 0);
    EXPECT_EQ(divide.facesMesh1A.size(), 6);
    EXPECT_EQ(divide.facesMesh1B.size(), 0);

    EXPECT_FALSE(divide.inside0A);
    EXPECT_FALSE(divide.inside0B);
    EXPECT_FALSE(divide.inside1A);
    EXPECT_FALSE(divide.inside1B);
  }

  TEST_F(MeshDivideTest, Cube2)
  {
    MeshFactory factory;
    Mesh cube0 = factory.cube(10,10,10);
    Mesh cube1 = factory.cube(12,8,8);
    cube1.translate(Vector3d(-1, 1, 1));

    IntersectionPoints points(cube0, cube1);
    Paths paths(points);
    MeshDivide divide(paths);

    EXPECT_EQ(divide.facesMesh0A.size(), 8);
    EXPECT_EQ(divide.facesMesh0B.size(), 0);
    EXPECT_EQ(divide.facesMesh1A.size(), 2);
    EXPECT_EQ(divide.facesMesh1B.size(), 2);

    EXPECT_FALSE(divide.inside0A);
    EXPECT_FALSE(divide.inside0B);
    EXPECT_FALSE(divide.inside1A);
    EXPECT_FALSE(divide.inside1B);
  }

}


/*
 * SharedLocalisationUpdateBundle.hpp
 *
 *  Created on: 26/04/2014
 *      Author: osushkov
 */

#pragma once

#include "LocalisationUtils.hpp"
#include "LocalisationDefs.hpp"
#include "types/boostSerializationEigenTypes.hpp"
#include <Eigen/Eigen>

struct SharedLocalisationUpdateBundle {
   SharedLocalisationUpdateBundle() :
      ballSeenFraction(0.0),
      isUpdateValid(false),
      sharedUpdateMean(),
      sharedUpdateCovariance(),
      haveVisionUpdates(false),
      haveBallUpdates(false),
      sharedDx(0.0f),
      sharedDy(0.0f),
      sharedDh(0.0f),
      sharedCovarianceDx(0.0f),
      sharedCovarianceDy(0.0f),
      sharedCovarianceDh(0.0f) {}

   SharedLocalisationUpdateBundle(
         const float ballSeenFraction,
         const Eigen::MatrixXd &sharedUpdateMeanIn,
         const Eigen::MatrixXd &sharedUpdateCovarianceIn,
         const bool haveVisionUpdates,
         const bool haveBallUpdates,
         const double sharedDx,
         const double sharedDy,
         const double sharedDh,
         const double sharedCovarianceDx,
         const double sharedCovarianceDy,
         const double sharedCovarianceDh) :
            ballSeenFraction(ballSeenFraction),
            isUpdateValid(true),
            haveVisionUpdates(haveVisionUpdates),
            haveBallUpdates(haveBallUpdates),
            sharedDx(sharedDx),
            sharedDy(sharedDy),
            sharedDh(sharedDh),
            sharedCovarianceDx(sharedCovarianceDx),
            sharedCovarianceDy(sharedCovarianceDy),
            sharedCovarianceDh(sharedCovarianceDh) {

      for (int i = 0; i < SHARED_DIM; i++) {
         this->sharedUpdateMean(i, 0) = sharedUpdateMeanIn(i, 0);
      }

      for (int row = 0; row < SHARED_DIM; row++) {
         for (int col = 0; col < SHARED_DIM; col++) {
            this->sharedUpdateCovariance(row, col) = sharedUpdateCovarianceIn(row, col);
         }
      }
   }

   float ballSeenFraction;
   bool isUpdateValid;

   Eigen::Matrix<float, SHARED_DIM, 1> sharedUpdateMean;
   Eigen::Matrix<float, SHARED_DIM, SHARED_DIM> sharedUpdateCovariance;
   bool haveVisionUpdates;
   bool haveBallUpdates;

   float sharedDx;
   float sharedDy;
   float sharedDh;

   float sharedCovarianceDx;
   float sharedCovarianceDy;
   float sharedCovarianceDh;

   void output(void) const {
      std::cout << "dx,dy,dz: " << sharedDx << " " << sharedDy << " " << sharedDh << std::endl;
      std::cout << "cdx,cdy,cdz: " << sharedCovarianceDx << " " << sharedCovarianceDy << " "
            << sharedCovarianceDh << std::endl;

      prettyOutputMatrix("sharedUpdateMean", copyStaticToDynamicMatrix<SHARED_DIM, 1>(sharedUpdateMean));
      prettyOutputMatrix("sharedUpdateCovariance", copyStaticToDynamicMatrix<SHARED_DIM, SHARED_DIM>(sharedUpdateCovariance));
   }

   template<class Archive>
   void serialize(Archive &ar, const unsigned int file_version) {
      ar & ballSeenFraction;
      ar & isUpdateValid;
      ar & sharedUpdateMean;
      ar & sharedUpdateCovariance;
      ar & haveVisionUpdates;
      ar & haveBallUpdates;
      ar & sharedDx;
      ar & sharedDy;
      ar & sharedDh;
      ar & sharedCovarianceDx;
      ar & sharedCovarianceDy;
      ar & sharedCovarianceDh;
   }
};

inline std::ostream& operator<<(std::ostream& os, const SharedLocalisationUpdateBundle& bundle) {
   os << (int) bundle.isUpdateValid << std::endl;

   for (int i = 0; i < SHARED_DIM; i++) {
      os << bundle.sharedUpdateMean(i, 0) << std::endl;
   }

   for (int i = 0; i < SHARED_DIM; i++) {
      for (int j = 0; j < SHARED_DIM; j++) {
         os << bundle.sharedUpdateCovariance(i, j) << std::endl;
      }
   }

   os << bundle.sharedDx << std::endl;
   os << bundle.sharedDy << std::endl;
   os << bundle.sharedDh << std::endl;

   os << bundle.sharedCovarianceDx << std::endl;
   os << bundle.sharedCovarianceDy << std::endl;
   os << bundle.sharedCovarianceDh << std::endl;

   return os;
}

inline std::istream& operator>>(std::istream& is, SharedLocalisationUpdateBundle& bundle) {
   int isUpdateValid;
   is >> isUpdateValid;
   bundle.isUpdateValid = (bool) isUpdateValid;

   for (int i = 0; i < SHARED_DIM; i++) {
      is >> bundle.sharedUpdateMean(i, 0);
   }

   for (int i = 0; i < SHARED_DIM; i++) {
      for (int j = 0; j < SHARED_DIM; j++) {
         is >> bundle.sharedUpdateCovariance(i, j);
      }
   }

   is >> bundle.sharedDx;
   is >> bundle.sharedDy;
   is >> bundle.sharedDh;

   is >> bundle.sharedCovarianceDx;
   is >> bundle.sharedCovarianceDy;
   is >> bundle.sharedCovarianceDh;

   return is;
}


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_maxexponent Maxexponent
 *
 * \par Description
 * Constant Maxexponent, this is the maximum exposant
 * for floating point numbers, i.e. 127 for float , 1023 for double
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/maxexponent.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::maxexponent_(A0)>::type
 *     Maxexponent();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Maxexponent
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Maxexponent of functor Maxexponent
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Maxexponent : ext::pure_constant_<Maxexponent>
    {
      template<class Target, class Dummy=void>
      struct  apply : meta::int_c<typename Target::type,0> {};
    };

    template<class T, class Dummy>
    struct  Maxexponent::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::int_c<boost::simd::int32_t,127> {};

    template<class T, class Dummy>
    struct  Maxexponent::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::int_c<boost::simd::int64_t,1023> {};

  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Maxexponent, Maxexponent)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


//
//  cal_slip_sliprate.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/8/18.
//
//

#ifndef cal_slip_sliprate_hpp
#define cal_slip_sliprate_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "maplocal.hpp"

using namespace Eigen;
void cal_slip_slip_rate(VectorXd &u_n, VectorXd &v_n, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, int Ndofn, int nx, ArrayXd &delt_u_n, ArrayXd &delt_v_n);

#endif /* cal_slip_sliprate_hpp */


#define BOOST_TEST_MODULE Gpufit

#include "Gpufit/gpufit.h"

#include <boost/test/included/unit_test.hpp>

#include <array>

BOOST_AUTO_TEST_CASE( Linear_Fit_1D )
{
	/*
		Performs a single fit using the Linear Fit (LINEAR_1D) model.
		- Uses user info
		- Uses trivial weights.
		- No noise is added.
		- Checks fitted parameters equalling the true parameters.
	*/

    std::size_t const n_fits{ 1 } ;
    std::size_t const n_points{ 2 } ;

	std::array< float, 2 > const true_parameters{ { 1, 1 } };

    std::array< float, n_points > data{ { 1, 2 } } ;

	std::array< float, n_points > weights{ { 1, 1 } } ;

    std::array< float, 2 > initial_parameters{ { 1, 0 } } ;

    float tolerance{ 0.001f } ;

	int max_n_iterations{ 10 } ;

	std::array< int, 2 > parameters_to_fit{ { 1, 1 } } ;

	std::array< float, n_points > user_info{ { 0.f, 1.f } } ;

	std::array< float, 2 > output_parameters ;
    int output_states ;
    float output_chi_squares ;
    int output_n_iterations ;

	// test with LSE
    int status = gpufit
        (
            n_fits,
            n_points,
            data.data(),
            weights.data(),
            LINEAR_1D,
            initial_parameters.data(),
            tolerance,
            max_n_iterations,
            parameters_to_fit.data(),
            LSE,
            n_points * sizeof( float ),
            reinterpret_cast< char * >( user_info.data() ),
            output_parameters.data(),
            & output_states,
            & output_chi_squares,
            & output_n_iterations
        ) ;

    BOOST_CHECK( status == 0 ) ;
	BOOST_CHECK( output_states == 0 );
	BOOST_CHECK( output_n_iterations <= max_n_iterations );
	BOOST_CHECK( output_chi_squares < 1e-6f );

	BOOST_CHECK(std::fabsf(output_parameters[0] - true_parameters[0]) < 1e-6f);
	BOOST_CHECK(std::fabsf(output_parameters[1] - true_parameters[1]) < 1e-6f);

	// test with MLE
	status = gpufit
		(
			n_fits,
			n_points,
			data.data(),
			weights.data(),
			LINEAR_1D,
			initial_parameters.data(),
			tolerance,
			max_n_iterations,
			parameters_to_fit.data(),
			MLE,
			n_points * sizeof(float),
			reinterpret_cast< char * >(user_info.data()),
			output_parameters.data(),
			&output_states,
			&output_chi_squares,
			&output_n_iterations
		);

	BOOST_CHECK(status == 0);
	BOOST_CHECK(output_states == 0);
	BOOST_CHECK(output_n_iterations <= max_n_iterations);
	BOOST_CHECK(output_chi_squares < 1e-6f);

	BOOST_CHECK(std::fabsf(output_parameters[0] - true_parameters[0]) < 1e-6f);
	BOOST_CHECK(std::fabsf(output_parameters[1] - true_parameters[1]) < 1e-4f);

}