/*
Example to show conversions to/from Transformation Matrix

Zivid primarily operate with a (4x4) Transformation Matrix (Rotation Matrix + Translation Vector).
This example shows how to use Eigen to convert to and from:
  AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion

 It provides convenience functions that can be reused in applicable applications.
*/

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <iomanip>
#include <iostream>

enum class RotationConvention
{
    ZYX_Intrinsic,
    XYZ_Extrinsic,
    XYZ_Intrinsic,
    ZYX_Extrinsic,
    NOF_ROT
};
constexpr size_t nofRotationConventions = static_cast<size_t>(RotationConvention::NOF_ROT);

struct RollPitchYaw
{
    RotationConvention convention;
    Eigen::Array3d rollPitchYaw;
};

struct Representations
{
    Eigen::AngleAxisd axisAngle;
    Eigen::Vector3d rotationVector;
    Eigen::Quaterniond quaternion;
    std::array<RollPitchYaw, nofRotationConventions> rotations;
};

Eigen::Affine3d getTransformationMatrixFromYAML(const std::string &path);
void saveTransformationMatrixToYAML(const Eigen::Affine3d &, const std::string &path);
cv::Mat eigenToCv(const Eigen::MatrixXd &);
Eigen::MatrixXd cvToEigen(const cv::Mat &);
Eigen::Array3d rotationMatrixToRollPitchYaw(const Eigen::Matrix3d &rotationMatrix, const RotationConvention &rotation);
Eigen::Matrix3d rollPitchYawToRotationMatrix(const Eigen::Array3d &rollPitchYaw, const RotationConvention &rotation);
std::string toString(RotationConvention convention);
Representations zividToRobot(const Eigen::Affine3d &);
Eigen::Affine3d robotToZivid(const Representations &, const Eigen::Vector3d &);

int main()
{
    try
    {
        std::cout << std::setprecision(4);
        std::cout << "This example shows conversions to/from Transformation Matrix" << std::endl;

        const auto transformationMatrix = getTransformationMatrixFromYAML("robotTransform.yaml");
        std::cout << transformationMatrix.matrix() << std::endl;

        // Extract Rotation Matrix and Translation Vector from Transformation Matrix
        std::cout << "RotationMatrix:\n" << transformationMatrix.linear() << std::endl;
        std::cout << "TranslationVector:\n" << transformationMatrix.translation() << std::endl;

        // Convert from Zivid to Robot (Transformation Matrix --> any format)
        const auto robotRotationRepresentations = zividToRobot(transformationMatrix);

        // Convert from Robot to Zivid (any format --> Rotation Matrix)
        const auto transformationMatrix2 =
            robotToZivid(robotRotationRepresentations, transformationMatrix.translation());

        // Combine Rotation Matrix with Translation Vector to form Transformation Matrix
        saveTransformationMatrixToYAML(transformationMatrix2, "robotTransformOut.yaml");
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}

Eigen::Affine3d getTransformationMatrixFromYAML(const std::string &path)
{
    std::cout << "Opening .YAML file which contains transformation matrix (PoseState node)" << std::endl;
    cv::FileStorage fileStorageIn;
    if(!fileStorageIn.open(path, cv::FileStorage::Mode::READ))
    {
        throw std::runtime_error("Could not open " + path + ". Please run this sample from the build directory");
    }
    const auto poseStateNode = fileStorageIn["PoseState"];
    std::cout << "Getting PoseState:" << std::endl;
    if(poseStateNode.empty())
    {
        fileStorageIn.release();
        throw std::runtime_error("PoseState node not found in file");
    }
    auto transformationMatrix = Eigen::Affine3d(static_cast<Eigen::Matrix4d>(cvToEigen(poseStateNode.mat())));
    fileStorageIn.release();

    return transformationMatrix;
}

void saveTransformationMatrixToYAML(const Eigen::Affine3d &transformationMatrix, const std::string &path)
{
    // Save Transformation Matrix to .YAML file
    cv::FileStorage fileStorageOut;
    if(!fileStorageOut.open(path, cv::FileStorage::Mode::WRITE))
    {
        throw std::runtime_error("Could not open robotTransformOut.yaml for writing");
    }
    fileStorageOut.write("TransformationMatrixFromQuaternion", eigenToCv(transformationMatrix.matrix()));
    fileStorageOut.release();
}

cv::Mat eigenToCv(const Eigen::MatrixXd &eigenMat)
{
    cv::Mat cvMat(static_cast<int>(eigenMat.rows()), static_cast<int>(eigenMat.cols()), CV_64FC1, cv::Scalar(0));

    cv::eigen2cv(eigenMat, cvMat);

    return cvMat;
}

Eigen::MatrixXd cvToEigen(const cv::Mat &cvMat)
{
    Eigen::MatrixXd eigenMat(cvMat.rows, cvMat.cols);

    cv::cv2eigen(cvMat, eigenMat);

    return eigenMat;
}

Representations zividToRobot(const Eigen::Affine3d &transformationMatrix)
{
    Representations robotRepresentations;
    std::cout << "\nConverting Rotation Matrix to Axis-Angle" << std::endl;
    const Eigen::AngleAxisd axisAngle(transformationMatrix.linear());
    std::cout << "Axis:\n" << axisAngle.axis() << std::endl;
    std::cout << "Angle:\n" << axisAngle.angle() << std::endl;

    // Axis-angle to Rotation Vector
    std::cout << "\nConverting Axis-Angle to Rotation Vector:" << std::endl;
    robotRepresentations.rotationVector = axisAngle.angle() * axisAngle.axis();
    std::cout << robotRepresentations.rotationVector << std::endl;

    // Rotation Matrix to Quaternion
    std::cout << "\nConverting Rotation Matrix to Quaternion:" << std::endl;
    const Eigen::Quaterniond quaternion(transformationMatrix.linear());
    robotRepresentations.quaternion = quaternion;
    std::cout << robotRepresentations.quaternion.coeffs() << std::endl;

    // Rotation Matrix to Roll-Pitch-Yaw
    for(size_t i = 0; i < nofRotationConventions; i++)
    {
        const RotationConvention convention{ static_cast<RotationConvention>(i) };
        std::cout << "\nConverting Rotation Matrix to Roll-Pitch-Yaw angles (" << toString(convention)
                  << "):" << std::endl;
        robotRepresentations.rotations[i] = { convention,
                                              rotationMatrixToRollPitchYaw(transformationMatrix.linear(), convention) };
        std::cout << robotRepresentations.rotations[i].rollPitchYaw << std::endl;
    }

    return robotRepresentations;
}

Eigen::Affine3d robotToZivid(const Representations &representations, const Eigen::Vector3d &translationVector)
{
    // Roll-Pitch-Yaw to Rotation Matrix
    for(const auto &rotation : representations.rotations)
    {
        std::cout << "\nConverting Roll-Pitch-Yaw angles (" << toString(rotation.convention)
                  << ") to Rotation Matrix:" << std::endl;
        const Eigen::Matrix3d rotationMatrixFromRollPitchYaw =
            rollPitchYawToRotationMatrix(rotation.rollPitchYaw, rotation.convention);
        std::cout << rotationMatrixFromRollPitchYaw << std::endl;
    }

    // Rotation Vector to Axis-angle
    std::cout << "\nConverting Rotation Vector to Axis-Angle" << std::endl;
    const Eigen::AngleAxisd axisAngle(representations.rotationVector.norm(),
                                      representations.rotationVector.normalized());
    std::cout << "Axis:\n" << axisAngle.axis() << std::endl;
    std::cout << "Angle:\n" << axisAngle.angle() << std::endl;

    // Axis-Angle to Quaternion
    std::cout << "\nConverting Axis-Angle to Quaternion:" << std::endl;
    const Eigen::Quaterniond quaternion(axisAngle);
    std::cout << quaternion.coeffs() << std::endl;

    // Quaternion to Rotation Matrix
    std::cout << "\nConverting Quaternion to Rotation Matrix:" << std::endl;
    const auto rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
    std::cout << rotationMatrixFromQuaternion << std::endl;

    Eigen::Affine3d transformationMatrix(rotationMatrixFromQuaternion);
    transformationMatrix.translation() = translationVector;

    return transformationMatrix;
}

std::string toString(RotationConvention convention)
{
    switch(convention)
    {
        case RotationConvention::XYZ_Intrinsic: return "XYZ_Intrinsic";
        case RotationConvention::XYZ_Extrinsic: return "XYZ_Extrinsic";
        case RotationConvention::ZYX_Intrinsic: return "ZYX_Intrinsic";
        case RotationConvention::ZYX_Extrinsic: return "ZYX_Extrinsic";
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid RotationConvention");
}

// The following function converts Rotation Matrix to Roll-Pitch-Yaw angles in radians.
// The rotation convention we use here is that Roll is a rotation about x-axis,
// Pitch is a rotation about y-axis and Yaw is a rotation about z-axis.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The array is ordered by Roll, Pitch and then Yaw.
Eigen::Array3d rotationMatrixToRollPitchYaw(const Eigen::Matrix3d &rotationMatrix, const RotationConvention &rotation)
{
    switch(rotation)
    {
        case RotationConvention::XYZ_Intrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
        case RotationConvention::XYZ_Extrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
        case RotationConvention::ZYX_Intrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
        case RotationConvention::ZYX_Extrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid rotation");
}

// The following function converts Roll-Pitch-Yaw angles in radians to Rotation Matrix.
// This function takes an array of roll, pitch and yaw angles, and a rotation convention, as input parameters.
// For Roll-Pitch-Yaw we define that roll is a rotation about x-axis, pitch is a rotation about y-axis
// and yaw is a rotation about z-axis.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The array is ordered by Roll, Pitch and then Yaw.
Eigen::Matrix3d rollPitchYawToRotationMatrix(const Eigen::Array3d &rollPitchYaw, const RotationConvention &rotation)
{
    switch(rotation)
    {
        case RotationConvention::XYZ_Intrinsic:
        case RotationConvention::ZYX_Extrinsic:
            return (Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX())
                    * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ()))
                .matrix();
        case RotationConvention::ZYX_Intrinsic:
        case RotationConvention::XYZ_Extrinsic:
            return (Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX()))
                .matrix();
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid orientation");
}


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid 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 Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <tsid/math/utils.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_pseudoinverse)
{
  std::cout << "test_pseudoinverse\n";
  using namespace tsid::math;
  const unsigned int m = 3;
  const unsigned int n = 5;

  Matrix A = Matrix::Random(m,n);
  Matrix Apinv = Matrix::Zero(n,m);
  pseudoInverse(A, Apinv, 1e-5);

  BOOST_CHECK(Matrix::Identity(m,m).isApprox(A*Apinv));
}

BOOST_AUTO_TEST_SUITE_END ()


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#include <Eigen/Dense>

#include <boost/numeric/odeint.hpp>
#include <cbr_math/lie/odeint.hpp>
#include <cbr_control/mpc/mpc_tracking.hpp>
#include <matplot/matplot.h>

#include <chrono>
#include <vector>
#include <algorithm>

#include "so3_problem.hpp"

using namespace std::chrono_literals;

int main(int argc, char const * argv[])
{
  using state_t = SO3Problem::state_t;
  using deriv_t = SO3Problem::deriv_t;
  using input_t = SO3Problem::input_t;

  //  -------------------------------------------------------------------------- /
  //                              Simulation Params                              /
  //  -------------------------------------------------------------------------- /

  state_t x0{};
  const auto tf = 10s;
  const auto dt = 10ms;

  auto xd = [](nanoseconds) {
      return state_t(
        Sophus::SO3d::rotZ(0.25) * Sophus::SO3d::rotY(-0.4) * Sophus::SO3d::rotX(0.5),
        Eigen::Vector3d::Zero()
      );
    };

  //  -------------------------------------------------------------------------- /
  //                                      MPC                                    /
  //  -------------------------------------------------------------------------- /

  SO3Problem so3_ocp{};

  cbr::MPCTrackingParams params;
  params.T = 4;
  params.solver_params.osqp_settings.verbose = 1;

  cbr::MPCTracking<SO3Problem, 50> mpc(so3_ocp, params);
  mpc.set_xd(xd);

  //  -------------------------------------------------------------------------- /
  //                                RUN SIMULATION                               /
  //  -------------------------------------------------------------------------- /

  nanoseconds t(0);
  state_t x = x0;

  cbr::lie::odeint::runge_kutta4<state_t, double, deriv_t, double> stepper;

  std::vector<double> sol_t;
  std::vector<input_t, Eigen::aligned_allocator<input_t>> sol_u;
  std::vector<state_t, Eigen::aligned_allocator<state_t>> sol_x;

  while (t < tf) {
    mpc.update_sync(t, x);
    const auto u = mpc.get_u(t);

    sol_t.push_back(duration_cast<duration<double>>(t).count());
    sol_x.push_back(x);
    sol_u.push_back(u);

    stepper.do_step(
      [&so3_ocp, &u](const state_t & x, deriv_t & dr_x, const double) {
        dr_x = so3_ocp.get_f(x, u);
      },
      x,
      duration_cast<duration<double>>(t).count(),
      duration_cast<duration<double>>(dt).count()
    );

    t += dt;
  }

  //  -------------------------------------------------------------------------- /
  //                                PLOT RESULTS                                 /
  //  -------------------------------------------------------------------------- /

  // helper function to extract stuff from solutions
  auto ex_fn = [](const auto & item, auto ex_fn) {
      std::vector<double> ret;
      std::transform(item.cbegin(), item.cend(), std::back_inserter(ret), ex_fn);
      return ret;
    };

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleX();}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleY();}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleZ();}))->line_width(2);
  matplot::title("angles");
  matplot::legend({"roll", "pitch", "yaw"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(0);}))->line_width(2);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(1);}))->line_width(2);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(2);}))->line_width(2);
  matplot::title("velocities");
  matplot::legend({"vx", "vy", "vz"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(0);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(1);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(2);}))->line_width(2);
  matplot::title("inputs");
  matplot::legend({"ux", "uy", "uz"});
  matplot::show();

  return EXIT_SUCCESS;
}


//
// Phase Shift.cpp
//

#include <iostream>
#include <iomanip>
#include <string>
#include <cerrno>
#include <vector>
#include <fstream>
#include <algorithm>
#include <math.h>
#include <mkl_lapack.h>
#include <complex>
#include <stdlib.h>
#include <stdio.h>
#include <boost/filesystem.hpp>

using namespace std;

typedef complex<double> dcmplx;

int		CalcPowerTableSize(int Omega);
int		ReadMatrixElem(ifstream &FileMatrixElem, int NumShortTerms, vector <double> &ARow, vector <double> &B, double &SLS, int &IsTriplet, int &Ordering, int &LValue, int &Formalism, int &Omega, int &NumSets, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, double &Kappa, double &Mu, string &LString, int &Shielding, string &Lambda, double &Epsilon12, double &Epsilon13, bool &ExtraExponential);
int		ReadShortHeader(ifstream &FileShortRange, int &Omega, int &LValue, int &IsTriplet, int &Formalism, int &Ordering, int &NumShortTerms, int &NumSets, int &Integration, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, bool &ExtraExponential, double &Epsilon12, double &Epsilon13, vector <int> &ExpLen);
void	WriteHeader(ofstream &OutFile, string &LString, int &LValue, char *FileShortName, char *FileMatrixElemName, char *EnergyFileName, bool &Paired, bool &Resorted,
				int &ShortInt, int &NumTerms, double &Kappa, double &Mu, int &Shielding, string &Lambda, double &Alpha, double &Beta, double &Gamma, string &ProgName);
int		CreateSubset(vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub);
int		FindOrderedToddTerm(string EnergyFilename, int TermToFind);
int		LoadToddTerms(int LValue, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub, string EnergyFilename, bool Resorted, bool Paired, int ResortedSize);
int		TestToddFile(string EnergyFilename, int NumShortTerms);
void	uGenKohn(dcmplx (&u)[2][2], double Tau);
void	uGenTKohn(dcmplx (&u)[2][2], double Tau);
void	uGenSKohn(dcmplx (&u)[2][2], double Tau);
double	CombinedKohn(dcmplx (&u)[2][2], int NumShortTerms, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, double SLS, int LValue, int IsTriplet);
string	ShortIntString(int &Integration);
void	FixPhase(double &PhaseShift, int &LValue, int &IsTriplet);
string	GetDateTime(void);

#define NUM_TAUARRAY 35
double TauArray[NUM_TAUARRAY] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.7853981633974483, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.570796326794897, 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.356194490192345, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9, 3.0, 3.141592653589793};
const char *DataHeader = "       n |         Kohn            |       Inverse Kohn      |     Complex Kohn (S)    |     Complex Kohn (T) "
	"   |    Gen Kohn tau = 0.0   |    Gen Kohn tau = 0.1   |    Gen Kohn tau = 0.2   |    Gen Kohn tau = 0.3   |    Gen Kohn tau = 0.4   |    Gen Kohn tau = 0.5"
	"   |    Gen Kohn tau = 0.6   |    Gen Kohn tau = 0.7   |   Gen Kohn tau = pi/4   |    Gen Kohn tau = 0.8   |    Gen Kohn tau = 0.9   |    Gen Kohn tau = 1.0"
	"   |    Gen Kohn tau = 1.1   |    Gen Kohn tau = 1.2   |    Gen Kohn tau = 1.3   |    Gen Kohn tau = 1.4   |    Gen Kohn tau = 1.5   |   Gen Kohn tau = pi/2"
	"   |    Gen Kohn tau = 1.6   |    Gen Kohn tau = 1.7   |    Gen Kohn tau = 1.8   |    Gen Kohn tau = 1.9   |    Gen Kohn tau = 2.0"
	"   |    Gen Kohn tau = 2.1   |    Gen Kohn tau = 2.2   |    Gen Kohn tau = 2.3   |  Gen Kohn tau = 3*pi/4  |    Gen Kohn tau = 2.4   |    Gen Kohn tau = 2.5"
	"   |    Gen Kohn tau = 2.6   |    Gen Kohn tau = 2.7   |    Gen Kohn tau = 2.8   |    Gen Kohn tau = 2.9   |    Gen Kohn tau = 3.0   |    Gen Kohn tau = pi "
	"   |   Gen T Kohn tau = 0.0  |   Gen T Kohn tau = 0.1  |   Gen T Kohn tau = 0.2  |   Gen T Kohn tau = 0.3  |   Gen T Kohn tau = 0.4  |   Gen T Kohn tau = 0.5"
	"  |   Gen T Kohn tau = 0.6  |   Gen T Kohn tau = 0.7  |  Gen T Kohn tau = pi/4  |   Gen T Kohn tau = 0.8  |   Gen T Kohn tau = 0.9  |   Gen T Kohn tau = 1.0"
	"  |   Gen T Kohn tau = 1.1  |   Gen T Kohn tau = 1.2  |   Gen T Kohn tau = 1.3  |   Gen T Kohn tau = 1.4  |   Gen T Kohn tau = 1.5  |  Gen T Kohn tau = pi/2"
	"  |   Gen T Kohn tau = 1.6  |   Gen T Kohn tau = 1.7  |   Gen T Kohn tau = 1.8  |   Gen T Kohn tau = 1.9  |   Gen T Kohn tau = 2.0"
	"  |   Gen T Kohn tau = 2.1  |   Gen T Kohn tau = 2.2  |   Gen T Kohn tau = 2.3  | Gen T Kohn tau = 3*pi/4 |   Gen T Kohn tau = 2.4  |   Gen T Kohn tau = 2.5"
	"  |   Gen T Kohn tau = 2.6  |   Gen T Kohn tau = 2.7  |   Gen T Kohn tau = 2.8  |   Gen T Kohn tau = 2.9  |   Gen T Kohn tau = 3.0  |   Gen T Kohn tau = pi "
	"  |   Gen S Kohn tau = 0.0  |   Gen S Kohn tau = 0.1  |   Gen S Kohn tau = 0.2  |   Gen S Kohn tau = 0.3  |   Gen S Kohn tau = 0.4  |   Gen S Kohn tau = 0.5"
	"  |   Gen S Kohn tau = 0.6  |   Gen S Kohn tau = 0.7  |  Gen S Kohn tau = pi/4  |   Gen S Kohn tau = 0.8  |   Gen S Kohn tau = 0.9  |   Gen S Kohn tau = 1.0"
	"  |   Gen S Kohn tau = 1.1  |   Gen S Kohn tau = 1.2  |   Gen S Kohn tau = 1.3  |   Gen S Kohn tau = 1.4  |   Gen S Kohn tau = 1.5  |  Gen S Kohn tau = pi/2"
	"  |   Gen S Kohn tau = 1.6  |   Gen S Kohn tau = 1.7  |   Gen S Kohn tau = 1.8  |   Gen S Kohn tau = 1.9  |   Gen S Kohn tau = 2.0"
	"  |   Gen S Kohn tau = 2.1  |   Gen S Kohn tau = 2.2  |   Gen S Kohn tau = 2.3  | Gen S Kohn tau = 3*pi/4 |   Gen S Kohn tau = 2.4  |   Gen S Kohn tau = 2.5"
	"  |   Gen S Kohn tau = 2.6  |   Gen S Kohn tau = 2.7  |   Gen S Kohn tau = 2.8  |   Gen S Kohn tau = 2.9  |   Gen S Kohn tau = 3.0  |   Gen S Kohn tau = pi";

dcmplx uKohn[2][2] = {{dcmplx(1,0),dcmplx(0,0)}, {dcmplx(0,0),dcmplx(1,0)}};
dcmplx uInvKohn[2][2] = {{dcmplx(0,0),dcmplx(1,0)}, {dcmplx(-1,0),dcmplx(0,0)}};
dcmplx uCompSKohn[2][2] = {{dcmplx(0,1), dcmplx(-1,0)}, {dcmplx(0,1), dcmplx(1,0)}};
dcmplx uCompTKohn[2][2] = {{dcmplx(1,0), dcmplx(0,0)}, {dcmplx(0,1), dcmplx(1,0)}};

std::string trim(const std::string& str,
                 const std::string& whitespace = " \t")
{
    const int strBegin = str.find_first_not_of(whitespace);
    if (strBegin == std::string::npos)
        return ""; // no content

    const int strEnd = str.find_last_not_of(whitespace);
    const int strRange = strEnd - strBegin + 1;

    return str.substr(strBegin, strRange);
}

int main(int argc, char *argv[])
{
	ifstream FileMatrixElem, FileShortRange;
	ofstream OutFile;
	string LString, Lambda;
	double ShortAlpha1, ShortBeta1, ShortGamma1, ShortAlpha2, ShortBeta2, ShortGamma2, ShortEpsilon12, ShortEpsilon13, Kappa, Mu;
	double LongAlpha1, LongBeta1, LongGamma1, LongAlpha2, LongBeta2, LongGamma2, LongEpsilon12, LongEpsilon13;
	int ShortOmega, ShortLValue, ShortIsTriplet, ShortOrdering, /*ShortNumSets,*/ ShortFormalism;
	int LongOmega, LongLValue, LongIsTriplet, LongOrdering, LongNumSets, LongFormalism;
	int /*Ordering,*/ NumSets, NumShort, NumShortTotal, NumShortTermsFile, ShortInt, Shielding;
	vector <int> ExpLen;
	bool ExtraExponential;
	int TotalTerms;

	vector <double> ARow, B, ShortTerms;
	double *PhiPhi, *PhiHPhi;
	double SLS;
	vector <double> ARowSub, BSub, ShortTermsSub;
	double GenKohnPhase;
	bool Paired, Resorted = false;
	int TermStep;
	dcmplx u[2][2];

	string ProgName = boost::filesystem::canonical(argv[0]).string();  // Get the absolute path of this program

	// Initialize the second set of nonlinear parameters for the files that don't use them.
	ShortAlpha2 = 0.0; LongAlpha2 = 0.0; ShortBeta2 = 0.0; LongBeta2 = 0.0; ShortGamma2 = 0.0; LongGamma2 = 0.0;

	char *FileMatrixElemName = argv[2];
	char *FileShortName = argv[3];
	char *OutFileName = argv[4];
	char *EnergyFileName = argv[6];

	if (argc < 7) {
		cerr << "Not enough parameters on the command line." << endl;
		cerr << "Usage: Phase pairing matrixelements.txt shortrangefile.bin results.txt #terms (energyfile.txt) (resorted?)" << endl;
		cerr << "Example: Phase 1 matrixelements.txt shortrangefile.bin results.txt 84 energyfile.txt true" << endl << endl;
		cerr << " The pairing parameter is 0 for no pairing of terms for the two symmetries and" << endl;
		cerr << " 1 for pairing." << endl;
		return 1;
	}

	if (atoi(argv[1]) == 0) {
		Paired = false;
		TermStep = 1;
	}
	else if (atoi(argv[1]) == 1) {
		Paired = true;
		TermStep = 2;
	}
	else {
		cout << "The pairing entry must be either 0 or 1." << endl;
		return 2;
	}

	FileMatrixElem.open(FileMatrixElemName);
	if (FileMatrixElem.fail()) {
		cerr << "Unable to open file " << FileMatrixElemName << " for reading." << endl;
		return 2;
	}

	FileShortRange.open(FileShortName, ios::in | ios::binary);
	if (FileShortRange.fail()) {
		cerr << "Unable to open file " << FileShortName << " for reading." << endl;
		return 3;
	}

	OutFile.open(OutFileName);
	if (!OutFile.is_open()) {
		cout << "Could not open output file...exiting." << endl;
		return 4;
	}
	TotalTerms = atoi(argv[5]);

	if (argc > 6) {
		int ToddTermNum = TestToddFile(EnergyFileName, TotalTerms);
		if (ToddTermNum < TotalTerms) {
			cout << "Using less than the requested number of terms: " << ToddTermNum << " instead of " << TotalTerms << endl;
			TotalTerms = ToddTermNum;
		}
	}

	if (argc > 7) {
		//@TODO: Case-insensitive string compare
		if (string(argv[7]) == "true") {
			Resorted = true;
			cout << "Computations will be performed with the terms resorted." << endl;
		}
		// Any other string just sets Resorted to false.
		else {
			cout << "Computations will be performed with the ordering specified in the energy file." << endl;
		}
	}

	// Include trailing zeros so the columns line up in the output file.
	cout.setf(ios::showpoint);
	OutFile.setf(ios::showpoint);
	cout << setprecision(18);
	OutFile << setprecision(18);

	int err = ReadShortHeader(FileShortRange, ShortOmega, ShortLValue, ShortIsTriplet, ShortFormalism, ShortOrdering, NumShortTermsFile, NumSets, ShortInt, ShortAlpha1, ShortBeta1, ShortGamma1, ShortAlpha2, ShortBeta2, ShortGamma2, ExtraExponential, ShortEpsilon12, ShortEpsilon13, ExpLen);
	if (err == -1) {
		return 8;
	}

	// Calculate number of terms for a given omega and generate the r-powers.
	NumShort = CalcPowerTableSize(ShortOmega);
	if (NumShort != NumShortTermsFile) {
		//cout << "Number of terms does not match in files...exiting." << endl;
		//return 2;
	}

	// The P-wave files have double the number of elements.
	//NumShortTerms = NumShortTerms*(ShortLValue+1);

	if (ShortLValue == 0)
		NumShortTotal = NumShort;  // The S-wave only has one symmetry
	else
		NumShortTotal = NumShort * 2;

	// Allocate PhiPhi and PhiHPhi matrices
	PhiPhi = new double[NumShortTotal*NumShortTotal];
	PhiHPhi = new double[NumShortTotal*NumShortTotal];
	if (PhiPhi == NULL || PhiHPhi == NULL) {
		cout << "Memory allocation error" << endl;
		return 5;
	}

	// Read in the <phi|phi> and <phi|H|phi> matrix elements.
	FileShortRange.read((char*)PhiPhi, NumShortTotal*NumShortTotal*sizeof(double));
	FileShortRange.read((char*)PhiHPhi, NumShortTotal*NumShortTotal*sizeof(double));

	ARow.resize(NumShortTotal+1);
	B.resize(NumShortTotal+1);
	ShortTerms.resize(NumShortTotal*NumShortTotal);
	err = ReadMatrixElem(FileMatrixElem, NumShortTotal, ARow, B, SLS, LongIsTriplet, LongOrdering, LongLValue, LongFormalism, LongOmega, LongNumSets, LongAlpha1, LongBeta1, LongGamma1, LongAlpha2, LongBeta2, LongGamma2, Kappa, Mu, LString, Shielding, Lambda, LongEpsilon12, LongEpsilon13, ExtraExponential);
	if (err == -1)
		return 6;

	// Compare the short-range and long-range files to make sure they are describing the same problem.
	if ((LongLValue != ShortLValue && ShortLValue != 0) || LongIsTriplet != ShortIsTriplet || LongOrdering != ShortOrdering || LongOmega != ShortOmega || LongAlpha1 != ShortAlpha1 || LongBeta1 != ShortBeta1 || LongGamma1 != ShortGamma1 || LongAlpha2 != ShortAlpha2 || LongBeta2 != ShortBeta2 || LongGamma2 != ShortGamma2) {
		cout << "Short-range and long-range files describe different problems...exiting." << endl;
		return 8;
	}

	for (int i = 0; i < NumShortTotal*NumShortTotal; i++) {
		ShortTerms[i] = PhiHPhi[i] - 0.5*Kappa*Kappa * PhiPhi[i] + 1.5*PhiPhi[i];
		//ShortTerms[i] = PhiHPhi[i] - Kappa*Kappa * PhiPhi[i] + 1.5*PhiPhi[i];  // For electron or positron scattering
	}

	delete [] PhiPhi;
	delete [] PhiHPhi;

	WriteHeader(OutFile, LString, ShortLValue, FileShortName, FileMatrixElemName, EnergyFileName, Paired, Resorted,
				ShortInt, TotalTerms, Kappa, Mu, Shielding, Lambda, ShortAlpha1, ShortBeta1, ShortGamma1, ProgName);

	int FieldWidth = 25;

	for (int i = 0; i <= TotalTerms; i++) {
		LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
		double KohnPhase = CombinedKohn(uKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double InvKohnPhase = CombinedKohn(uInvKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double CompKohnSPhase = CombinedKohn(uCompSKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double CompKohnTPhase = CombinedKohn(uCompTKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		if ((KohnPhase == 0.0) || (InvKohnPhase == 0.0) || (CompKohnSPhase == 0.0) || (CompKohnTPhase == 0.0)) {
			cout << "Terminating loop early due to LAPACK errors." << endl;
			OutFile << "Terminating loop early due to LAPACK errors." << endl;
			break;
		}
		cout << i << " " << KohnPhase << " " << InvKohnPhase << " " << CompKohnSPhase << " " << CompKohnTPhase << endl;
		OutFile << setw(8) << i << setw(1) << " " << setw(FieldWidth) << KohnPhase << setw(1) << " " << setw(FieldWidth) << InvKohnPhase << setw(1) << " " << setw(FieldWidth)
				<< CompKohnSPhase << setw(1) << " " << setw(FieldWidth) << CompKohnTPhase;

		// Generalized Kohn
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}

		// Generalized T-matrix
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenTKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}

		// Generalized S-matrix
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenSKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}
		if (GenKohnPhase == 0.0) {
			cout << "Terminating loop early due to LAPACK errors." << endl;
			OutFile << "Terminating loop early due to LAPACK errors." << endl;
			break;
		}

		OutFile << setw(1) << " " << endl;
	}

	OutFile << "</data>" << endl << "</psh_data>" << endl;

	FileMatrixElem.close();
	FileShortRange.close();
	OutFile.close();

	return 0;
}

int CreateSubset(vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub)
{
	ARowSub.resize(NSub*2+1);
	BSub.resize(NSub*2+1);
	ShortTermsSub.resize(NSub*NSub*4);

	vector <int> UsedTerms, UsedTermsSub;

	UsedTerms.resize(NumShortTerms*2);
	for (int i = 0; i < NumShortTerms*2; i++) {
		UsedTerms[i] = i+1;
	}
	UsedTermsSub.resize(NSub*2);
	for (int i = 0; i < NSub; i++) {
		UsedTermsSub[i*2] = i+1;
		UsedTermsSub[i*2+1] = NumShortTerms+i+1;
	}

	ARowSub[0] = ARow[0];
	BSub[0] = B[0];
	for (int i = 0; i < NSub*2; i++) {
		for (int j = 0; j < NSub*2; j++) {
			// The Fortran output counts from 1, hence the -1 on the RHS.
			ShortTermsSub[i*NSub*2 + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTerms*2 + (UsedTermsSub[j]-1)];
		}
		// Skips the 0 entry in ARow, so they line up.
		ARowSub[i+1] = ARow[UsedTermsSub[i]];
	}
	for (int i = 0; i < NSub*2; i++) {
		BSub[i+1] = B[UsedTermsSub[i]];
	}

	return 0;
}

// Sort algorithm example code from http://www.cplusplus.com/reference/algorithm/sort/
struct myclass {
	bool operator() (int i,int j) { return (i<j);}
} myobject;

int FindOrderedToddTerm(string EnergyFilename, int TermToFind, int NumTerms)
{
	ifstream EnergyFile;
	vector <int> UsedTerms, UsedTermsSub;
	string Line;
	int Term, Index;
	double Energy;

	if (TermToFind < 1) {
		cout << "TermToFind must be 1 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	UsedTerms.resize(NumTerms);
	for (int i = 0; i < NumTerms; i++) {
		EnergyFile >> Term >> Index >> Energy;
		UsedTerms[i] = Term;
	}

	sort(UsedTerms.begin(), UsedTerms.end(), myobject);

	EnergyFile.close();

	// Now search for the term (or find where it would go).
	for (int i = 0; i < NumTerms; i++) {
		if (UsedTerms[i] == TermToFind)
			return i+1;
		if (UsedTerms[i] > TermToFind) {
			if (i == 0)
				return 1;  // Don't want to return 0.
			return i;
		}
	}

	return NumTerms+1;  // Term not found (larger than last used term).
}

// We could just open the energy file in the main program instead of reopening it many times, but this is just easier.
int TestToddFile(string EnergyFilename, int NumShortTerms)
{
	ifstream EnergyFile;
	string Line;
	int Term, Index;
	double Energy;

	if (NumShortTerms < 1) {
		cout << "NumShortTerms must be 1 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	if (EnergyFile.fail())
		return 0;
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	for (int i = 0; i < NumShortTerms; i++) {
		EnergyFile >> Term >> Index >> Energy;
		if (EnergyFile.fail())  // End of terms to use
			return i;
	}

	EnergyFile.close();

	return NumShortTerms;
}

// We could just open the energy file in the main program instead of reopening it many times, but this is just easier.
int LoadToddTerms(int ShortLValue, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub,
					vector <double> &ShortTermsSub, int NumShortTotal, int NSub, string EnergyFilename, bool Resorted, bool Paired, int ResortedSize)
{
	ifstream EnergyFile;
	vector <int> UsedTerms, UsedTermsSub;
	string Line;
	int Term, Index, Size;
	double Energy;

	if (NSub < 0) {
		cout << "NSub must be 0 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	if (EnergyFile.fail())
		return 0;
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	if (Resorted == false)
		Size = NSub;
	else
		Size = ResortedSize;

	UsedTerms.resize(Size);
	for (int i = 0; i < Size; i++) {
		EnergyFile >> Term >> Index >> Energy;
		if (EnergyFile.fail())  // End of terms to use
			return 0;
		UsedTerms[i] = Term;
	}

	if (Paired == true) {
		UsedTermsSub.resize(NSub*2);

		if (Resorted) {
			cout << "Reordering terms" << endl;
			sort(UsedTerms.begin(), UsedTerms.end(), myobject);
		}

		for (int i = 0; i < NSub; i++) {
			UsedTermsSub[i*2] = UsedTerms[i];
			UsedTermsSub[i*2+1] = NumShortTotal/2+UsedTerms[i];
		}

		EnergyFile.close();

		ARowSub.resize(NSub*2+1);
		BSub.resize(NSub*2+1);
		ShortTermsSub.resize(NSub*NSub*4);

		ARowSub[0] = ARow[0];
		BSub[0] = B[0];
		for (int i = 0; i < NSub*2; i++) {
			for (int j = 0; j < NSub*2; j++) {
				// The Fortran output counts from 1, hence the -1 on the RHS.
				ShortTermsSub[i*NSub*2 + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTotal + (UsedTermsSub[j]-1)];
			}
			// Skips the 0 entry in ARow, so they line up.
			ARowSub[i+1] = ARow[UsedTermsSub[i]];
		}
		for (int i = 0; i < NSub*2; i++) {
			BSub[i+1] = B[UsedTermsSub[i]];
		}
	}
	else {
		UsedTermsSub.resize(NSub);

		if (Resorted) {
			cout << "Reordering terms" << endl;
			sort(UsedTerms.begin(), UsedTerms.end(), myobject);
		}

		for (int i = 0; i < NSub; i++) {
			UsedTermsSub[i] = UsedTerms[i];
		}

		EnergyFile.close();

		ARowSub.resize(NSub+1);
		BSub.resize(NSub+1);
		ShortTermsSub.resize(NSub*NSub);

		ARowSub[0] = ARow[0];
		BSub[0] = B[0];
		for (int i = 0; i < NSub; i++) {
			for (int j = 0; j < NSub; j++) {
				// The Fortran output counts from 1, hence the -1 on the RHS.
				ShortTermsSub[i*NSub + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTotal + (UsedTermsSub[j]-1)];
			}
			// Skips the 0 entry in ARow, so they line up.
			ARowSub[i+1] = ARow[UsedTermsSub[i]];
		}
		for (int i = 0; i < NSub; i++) {
			BSub[i+1] = B[UsedTermsSub[i]];
		}
	}

	return NSub;
}

// Returns the number of terms for a given omega.  This could use the formula for combination with repetition,
//  except then it would be unable to use a restricted set of terms if we needed.
int CalcPowerTableSize(int Omega)
{
	int NumTerms = 0;  // The total number of terms
	int om, ki, li, mi, ni, pi, qi;  // These are the exponents we are determining.

	for (om = 0; om <= Omega; om++) {
		for (ki = 0; ki <= Omega; ki++) {
			for (li = 0; li <= Omega; li++) {
				for (mi = 0; mi <= Omega; mi++) {
					for (ni = 0; ni <= Omega; ni++) {
						for (pi = 0; pi <= Omega; pi++) {
							for (qi = 0; qi <= Omega; qi++) {
								if (ki + li + mi + ni + pi + qi == om)
									NumTerms = NumTerms + 1;
							}
						}
					}
				}
			}
		}
	}

	return NumTerms;
}

// Reads in the output from the scattering program (short-range - long-range and long-range - long-range terms)
int ReadMatrixElem(ifstream &FileMatrixElem, int NumShortTerms, vector <double> &ARow, vector <double> &B, double &SLS, int &IsTriplet, int &Ordering, int &LValue, int &Formalism, int &Omega, int &NumSets, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, double &Kappa, double &Mu, string &LString, int &Shielding, string &Lambda, double &Epsilon12, double &Epsilon13, bool &ExtraExponential)
{
	string Line, Line1, Line2, Line3, Line4, OrderString;
	int NumTerms, Offset;

	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, LString);
	getline(FileMatrixElem, OrderString);

	FileMatrixElem >> Line >> Omega;
	FileMatrixElem >> Line1 >> Line2 >> Line3 >> NumTerms;

	cout << LString << endl;
	if (LString == "S-Wave Singlet Ps-H") {
		LValue = 0;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "S-Wave Triplet Ps-H") {
		LValue = 0;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "S-Wave Singlet Ps-H - Laplacian Formalism - Exponential") {
		LValue = 0;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "S-Wave Triplet Ps-H - Laplacian Formalism - Exponential") {
		LValue = 0;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 1st formalism" || LString == "P-Wave Singlet Ps-H") {  // Second is the older type
		LValue = 1;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "P-Wave Triplet Ps-H: 1st formalism" || LString == "P-Wave Triplet Ps-H") {  // Second is the older type
		LValue = 1;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 2nd formalism") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "P-Wave Triplet Ps-H: 2nd formalism") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 1st formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 2;
	}
	else if (LString == "P-Wave Triplet Ps-H: 1st formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 2;
	}
	else if (LString == "P-Wave Singlet Ps-H: 2nd formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 2;
	}
	else if (LString == "P-Wave Triplet Ps-H: 2nd formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 2;
	}
	else if (LString == "D-Wave Singlet Ps-H: 1st formalism") {
		LValue = 2;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "D-Wave Triplet Ps-H: 1st formalism") {
		LValue = 2;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "F-Wave Singlet Ps-H") {
		LValue = 3;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "F-Wave Triplet Ps-H") {
		LValue = 3;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "G-Wave Singlet Ps-H") {
		LValue = 4;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "G-Wave Triplet Ps-H") {
		LValue = 4;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "H-Wave Singlet Ps-H") {
		LValue = 5;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "H-Wave Triplet Ps-H") {
		LValue = 5;
		IsTriplet = 1;
		NumSets = 1;
	}
	else {
		cout << "Problem string in matrix element file has an unknown value...exiting." << endl;
		return -1;
	}

	if (ExtraExponential) {
		FileMatrixElem >> Line >> Alpha1 >> Line1 >> Beta1 >> Line2 >> Gamma1 >> Line3 >> Epsilon12 >> Line4 >> Epsilon13;
		cout << Alpha1 << " " << Beta1 << " " << Gamma1 << endl;
	}
	else {
		if (NumSets == 1) {
			FileMatrixElem >> Line >> Alpha1 >> Line1 >> Beta1 >> Line2 >> Gamma1;
			cout << Alpha1 << " " << Beta1 << " " << Gamma1 << endl;
		}
		else if (NumSets == 2) {
			FileMatrixElem >> Line >> Alpha2 >> Line1 >> Beta2 >> Line2 >> Gamma2;
		}
	}

	//getline(FileMatrixElem, Line);
	//getline(FileMatrixElem, Line);
	FileMatrixElem >> Line >> Mu;
	FileMatrixElem >> Line;
	Shielding = -1;
	if (Line.find("Shielding") != string::npos) {  // Skip this line - not yet to kappa line
		FileMatrixElem >> Line;
		FileMatrixElem >> Shielding;
		//getline(FileMatrixElem, Line);
		FileMatrixElem >> Line;
	}
	FileMatrixElem >> Kappa;
	getline(FileMatrixElem, Line);

	if (OrderString == "Using Denton's ordering") {
		Ordering = 0;
	}
	else if (OrderString == "Using Peter Van Reeth's ordering") {
		Ordering = 1;
	}
	else {
		cout << "Ordering string in matrix element file has an unknown value...exiting." << endl;
		return -1;
	}

	//getline(FileMatrixElem, Line);
	FileMatrixElem >> Line;
	if (Line.find("Lambda") != string::npos) {  // Has the extra lambda line here
		getline(FileMatrixElem, Lambda);
	}
	getline(FileMatrixElem, Line);

	for (int i = 0; i < 11; i++) {
		getline(FileMatrixElem, Line);
		if (Line == "A matrix row")  // Some files have one less extra line
			break;
	}

	// Reads in first row (and column) of A
	for (int i = 0; i < NumShortTerms+1; i++) {
		getline(FileMatrixElem, Line);
		istringstream iss(Line);
		iss >> Offset >> ARow[i];
	}

	// Skips extra lines
	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);

	// Reads in B vector
	for (int i = 0; i < NumShortTerms+1; i++) {
		getline(FileMatrixElem, Line);
		istringstream iss(Line);
		iss >> Offset >> B[i];
	}

	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);

	// SLS is not in A or B, so read it in.
	FileMatrixElem >> SLS;

	return 0;
}

// Reads in the short-range file header
int ReadShortHeader(ifstream &FileShortRange, int &Omega, int &LValue, int &IsTriplet, int &Formalism, int &Ordering, int &NumShortTerms, int &NumSets, int &Integration, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, bool &ExtraExponential, double &Epsilon12, double &Epsilon13, vector <int> &ExpLen)
{
	int MagicNum, Version, HeaderLen, DataFormat, NumShortTerms1, NumShortTerms2;
	int VarLen;

	FileShortRange.read((char*)&MagicNum, 4);
	FileShortRange.read((char*)&Version, 4);
	FileShortRange.read((char*)&HeaderLen, 4);
	FileShortRange.read((char*)&DataFormat, 4);
	FileShortRange.read((char*)&Omega, 4);
	FileShortRange.read((char*)&NumShortTerms1, 4);
	FileShortRange.read((char*)&NumShortTerms2, 4);
	FileShortRange.read((char*)&LValue, 4);
	FileShortRange.read((char*)&Formalism, 4);
	FileShortRange.read((char*)&IsTriplet, 4);
	FileShortRange.read((char*)&Ordering, 4);
	FileShortRange.read((char*)&Integration, 4);
	FileShortRange.read((char*)&NumSets, 4);

	//@TODO: More descriptive errors for each

	if (MagicNum != 0x31487350) {  // "PsH1" in hexadecimal (with reverse due to endianness)
		cout << "This is not a valid Ps-H file (MagicNum)...exiting." << endl;
		return -1;
	}

	if (Version < 1 || Version > 9) {
		cout << "This is not a valid Ps-H file (Version)...exiting." << endl;
		cout << Version << endl;
		return -1;
	}

	if (HeaderLen != 80 && HeaderLen != 104) {
		cout << "This is not a valid Ps-H file (HeaderLen)...exiting." << endl;
		return -1;
	}

	if (DataFormat != 8) {
		cout << "This is not a valid Ps-H file (Dataformat)...exiting." << endl;
		return -1;
	}

	if ((Formalism != 1 && Formalism != 2) || (IsTriplet != 0 && IsTriplet != 1) || (Ordering != 0 && Ordering != 1)) {
		cout << "This is not a valid Ps-H file (Formalism/IsTriplet/Ordering)...exiting." << endl;
		return -1;
	}

	if (NumSets != 1 && NumSets != 2) {
		cout << "This is not a valid Ps-H file (NumSets)...exiting." << endl;
		return -1;
	}

	/*if (NumShortTerms1 != NumShortTerms2) {
		cout << "This is not a valid Ps-H file...exiting." << endl;  // Cannot handle two different values for this yet.
		return -1;
	}*/
	NumShortTerms = NumShortTerms1;

	// @TODO: Set up to work properly with sectors
	FileShortRange.read((char*)&Alpha1, 8);
	FileShortRange.read((char*)&Beta1, 8);
	FileShortRange.read((char*)&Gamma1, 8);
	ExtraExponential = false;
	if (Version == 9) {  // Extra exponentials
		double BlankDouble;
		int BlankInt;
		ExtraExponential = true;
		FileShortRange.read((char*)&Epsilon12, 8);
		FileShortRange.read((char*)&Epsilon13, 8);
		FileShortRange.read((char*)&BlankDouble, 8);  // To be reserved for Epsilon23 at some point in the future

		ExpLen.resize(2);  // No r23 exponential right now
		FileShortRange.read((char*)&ExpLen[0], 4);
		FileShortRange.read((char*)&ExpLen[1], 4);
		FileShortRange.read((char*)&BlankInt, 4);
	}
	if (NumSets == 2) {
		//read (FileShortRange) Alpha2, Beta2, Gamma2
		FileShortRange.read((char*)&Alpha2, 8);
		FileShortRange.read((char*)&Beta2, 8);
		FileShortRange.read((char*)&Gamma2, 8);
	}

	FileShortRange.read((char*)&VarLen, 4);

	return 0;
}

void WriteHeader(ofstream &OutFile, string &LString, int &LValue, char *FileShortName, char *FileMatrixElemName, char *EnergyFileName, bool &Paired, bool &Resorted,
				int &ShortInt, int &NumTerms, double &Kappa, double &Mu, int &Shielding, string &Lambda, double &Alpha, double &Beta, double &Gamma, string &ProgName)
{
	OutFile << "<?xml version=\"1.0\" encoding=\"UTF-8\"?> " << endl;
	OutFile << "<psh_data>" << endl << "<header>" << endl;
	OutFile << "	<problem>" << LString << "</problem>" << endl;
	OutFile << "	<lvalue>" << LValue << "</lvalue>" << endl;
	OutFile << "	<shortfile>" << FileShortName << "</shortfile>" << endl;
	OutFile << "	<longfile>" << FileMatrixElemName << "</longfile>" << endl;
	//if (argc >= 6)  // Energy file present  //@TODO: Are we even accepting runs without now?
		OutFile << "	<energyfile>" << EnergyFileName << "</energyfile>" << endl;
	if (Paired == true)
		OutFile << "	<paired>" << "true" << "</paired>" << endl;
	else
		OutFile << "	<paired>" << "false" << "</paired>" << endl;
	OutFile << "	<ordering>" << "Peter" << "</ordering>" << endl;
	if (Resorted == true)
		OutFile << "	<reorder>" << "true" << "</reorder>" << endl;
	else
		OutFile << "	<reorder>" << "false" << "</reorder>" << endl;
	OutFile << "	<shortint>" << ShortIntString(ShortInt) << "</shortint>" << endl;
	OutFile << "	<numterms>" << NumTerms << "</numterms>" << endl;
	OutFile << "	<numsets>" << 1 << "</numsets>" << endl;
	OutFile << "	<kappa>" << Kappa << "</kappa>" << endl;
	OutFile << "	<mu>" << Mu << "</mu>" << endl;
	if (Shielding == -1)  // No shielding value specified in file - assume default
		Shielding = 2*LValue + 1;
	OutFile << "	<shielding>" << Shielding << "</shielding>" << endl;
	if (trim(Lambda) != "" || Lambda.size() > 0)
		OutFile << "	<lambda>" << trim(Lambda) << "</lambda>" << endl;
	OutFile << "	<nonlinear>" << endl;
	OutFile << "		<alpha>" << Alpha << "</alpha>" << endl;
	OutFile << "		<beta>" << Beta << "</beta>" << endl;
	OutFile << "		<gamma>" << Gamma << "</gamma>" << endl;
	OutFile << "	</nonlinear>" << endl;
	OutFile << "	<program>" << ProgName << "</program>" << endl;
	OutFile << "	<datetime>" << GetDateTime() << "</datetime>" << endl;

	cout << "n          Kohn              Inverse Kohn          Complex Kohn (S)       Complex Kohn (T)" << endl;
	OutFile << "</header>" << endl << "<dataheader>" << endl << DataHeader << endl << "</dataheader>" << endl << "<data>" << endl;
	OutFile << setprecision(16);
	OutFile << scientific;

	return;
}

// Generalized real Kohn
void uGenKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(cos(Tau),0);
	u[0][1] = dcmplx(sin(Tau),0);
	u[1][0] = dcmplx(-sin(Tau),0);
	u[1][1] = dcmplx(cos(Tau),0);
	return;
}

// Generalized T-matrix Kohn
void uGenTKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(cos(Tau),0);
	u[0][1] = dcmplx(sin(Tau),0);
	u[1][0] = dcmplx(-sin(Tau),cos(Tau));
	u[1][1] = dcmplx(cos(Tau),sin(Tau));
	return;
}

// Generalized S-matrix Kohn
void uGenSKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(-sin(Tau),-cos(Tau));
	u[0][1] = dcmplx(cos(Tau),-sin(Tau));
	u[1][0] = dcmplx(-sin(Tau),cos(Tau));
	u[1][1] = dcmplx(cos(Tau),sin(Tau));
	return;
}

// ARow and BVec are sent from the main program as the A and B from the Kohn method.  This function rearranges everything into
//  the matrix equation (7) of their paper and solves.
double CombinedKohn(dcmplx (&u)[2][2], int NumShortTerms, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, double SLS, int LValue, int IsTriplet)
{
	MKL_INT n, nrhs, lda, ldb, info;
	//double *A = new double[(NumShortTerms+1)*(NumShortTerms+1)];
	vector <dcmplx> A((NumShortTerms+1)*(NumShortTerms+1));
	vector <dcmplx> X(NumShortTerms+1);
	double CLC = ARow[0], CLS = B[0];
	double SLC = CLS + 1.0;  // Use (S,LC) = (C,LS) + 1
	dcmplx SLSt, SLCt, CLSt, CLCt;

	dcmplx detu = u[0][0]*u[1][1] - u[0][1]*u[1][0];  // Determinant

	SLSt = u[0][0]*u[0][0]*SLS + u[0][0]*u[0][1]*SLC + u[0][1]*u[0][0]*CLS + u[0][1]*u[0][1]*CLC;
	SLCt = u[0][0]*u[1][0]*SLS + u[0][0]*u[1][1]*SLC + u[0][1]*u[1][0]*CLS + u[0][1]*u[1][1]*CLC;
	CLSt = u[1][0]*u[0][0]*SLS + u[1][0]*u[0][1]*SLC + u[1][1]*u[0][0]*CLS + u[1][1]*u[0][1]*CLC;
	CLCt = u[1][0]*u[1][0]*SLS + u[1][0]*u[1][1]*SLC + u[1][1]*u[1][0]*CLS + u[1][1]*u[1][1]*CLC;

	// Copy short-range terms to bottom-right NumShortTerms x NumShortTerms submatrix of A.
	for (int i = 0; i < NumShortTerms; i++) {
		for (int j = 0; j < NumShortTerms; j++) {
			A[(i+1)*(NumShortTerms+1) + (j+1)] = ShortTerms[i*NumShortTerms + j];
		}
	}

	// Fill in the rest of A
	A[0] = CLCt;
	for (int i = 1; i < NumShortTerms+1; i++) {
		//A[i] = dcmplx(ARow[i], B[i]);
		A[i] = u[1][0]*B[i] + u[1][1]*ARow[i];
		A[i*(NumShortTerms+1)] = A[i];
	}

	// Fill in B (or X)
	X[0] = -CLSt;
	for (int i = 1; i < NumShortTerms+1; i++) {
		X[i] = - u[0][0]*B[i] - u[0][1]*ARow[i];
	}

	// LAPACK requires calls by reference, so we have to define all these variables.
	vector <int> ipiv(NumShortTerms+1);
	n = lda = ldb = NumShortTerms+1;
	nrhs = 1;
	zgesv(&n, &nrhs, (MKL_Complex16*)&A[0], &lda, &ipiv[0], (MKL_Complex16*)&X[0], &ldb, &info);
	if (info != 0) {
		cout << "LAPACK Error: " << info << endl;
		return 0.0;
	}

	// Equation () of notes
	dcmplx PsiLS = X[0] * CLSt;
	for (int i = 1; i < (NumShortTerms+1); i++) {
		PsiLS += X[i] * (u[0][0]*B[i] + u[0][1]*ARow[i]);
	}
	dcmplx L = -(PsiLS + SLSt) / detu;
	// Go from general L matrix element to K.
	dcmplx K = (u[0][1] + u[1][1]*L) / (u[0][0] + u[1][0]*L);

	//@TODO: Check K for imaginary part.
	double PhaseShift = atan(K.real());
	FixPhase(PhaseShift, LValue, IsTriplet);
	return PhaseShift;
}

string ShortIntString(int &Integration)
{
	switch (Integration)
	{
		case 1:
			return string("Direct summation");
		case 2:
			return string("Asymptotic expansion");
		case 3:
			return string("Recursion relations");
	}
	return string("Unknown integration");
}

// Since we are finding atan(delta) instead of delta directly, some of the results
//  are in the wrong range.
void FixPhase(double &PhaseShift, int &LValue, int &IsTriplet)
{
	double Pi = 4.0 * atan(1.0);

	if (LValue == 0 && IsTriplet == 0) {  // ^1S
		if (PhaseShift > 0.0) {
			PhaseShift = PhaseShift - Pi;
		}
	}
	else if (LValue == 0 && IsTriplet == 1) {  // ^3S
		if (PhaseShift > 0.0) {
			PhaseShift = PhaseShift - Pi;
		}
	}
	else if (LValue >= 3) {  // ^1F and higher
		if (PhaseShift > Pi) {
			PhaseShift = PhaseShift - Pi;
		}
	}
}

// Modified from http://www.dreamincode.net/code/snippet1102.htm
string GetDateTime(void)
{
	//Find the current time
	time_t curtime = time(0);

	//convert it to tm
	tm now=*localtime(&curtime);

	//BUFSIZ is standard macro that expands to a integer constant expression
	//that is greater then or equal to 256. It is the size of the stream buffer
	//used by setbuf()
	char dest[BUFSIZ]={0};

	//Format string determines the conversion specification's behaviour
	const char format[]="%x %X";

	//strftime - converts date and time to a string
	if (strftime(dest, sizeof(dest)-1, format, &now)>0) {
		return string(dest);
	}
	else
		cerr << "strftime failed. Errno code: " << errno << endl;
	return string("");
}

General Code/Phase Shift/Phase Shift.cpp


#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <iostream>

#include <bot_core/trans.h>

std::string print_Isometry3d(Eigen::Isometry3d pose){
  Eigen::Vector3d t(pose.translation());
  Eigen::Quaterniond r(pose.rotation());

  std::stringstream ss;
  ss <<t[0]<<", "<<t[1]<<", "<<t[2]<<", "
       <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
  return ss.str();
}

std::string print_BotTrans(BotTrans *bt){
  std::stringstream ss;
  ss << bt->trans_vec[0] <<", "<<bt->trans_vec[1]<<", "<<bt->trans_vec[2]<<", "
       <<bt->rot_quat[0]<<", "<<bt->rot_quat[1]<<", "<<bt->rot_quat[2]<<", "<<bt->rot_quat[3] ;
  return ss.str();
}

void setBotTrans(BotTrans *bt, double x, double y, double z, double qw, double qx, double qy, double qz){
  bt->trans_vec[0] = x;  bt->trans_vec[1] = y;  bt->trans_vec[2] = z;
  bt->rot_quat[0] = qw;  bt->rot_quat[1] = qx;
  bt->rot_quat[2] = qy;  bt->rot_quat[3] = qz;
}

Eigen::Isometry3d setIsometry3dFromBotTrans(BotTrans *bt){
  Eigen::Isometry3d tf;
  tf.setIdentity();
  tf.translation()  << bt->trans_vec[0], bt->trans_vec[1],bt->trans_vec[2];
  Eigen::Quaterniond q = Eigen::Quaterniond(bt->rot_quat[0], bt->rot_quat[1], bt->rot_quat[2], bt->rot_quat[3]);
  tf.rotate(q);

  return tf;
}

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

   double trans_vec[] = {-2.7363, 0.5958, -1.1588};
   double rot_quat[] = {0.8977, 0.0011, -9.0933e-04, -0.4407};

  BotTrans init_vicon, init_est, current_est;
  setBotTrans(&init_vicon, 1.4696,  0.5235, 0.8753, 0.9749, -0.0108, 0.0076, 0.2221);
  setBotTrans(&init_est, 2.1455, 1.8038, 1.1523, 0.8977, -0.0011, 9.0933e-04, 0.4407);
  setBotTrans(&current_est, 2.1456, 1.8038, 1.1523, 0.8976, -0.0011, 9.2603e-04, 0.4407);

  Eigen::Isometry3d init_vicon_e = setIsometry3dFromBotTrans(&init_vicon);
  Eigen::Isometry3d init_est_e = setIsometry3dFromBotTrans(&init_est);
  Eigen::Isometry3d current_est_e = setIsometry3dFromBotTrans(&current_est);

  std::cout << print_Isometry3d(current_est_e) << " current_est_e\n";
  std::cout << print_Isometry3d(init_est_e) << " init_est_e\n";
  Eigen::Isometry3d init_est_e_inv = init_est_e.inverse();
  std::cout << print_Isometry3d(init_est_e_inv) << " init_est_e.inverse\n";

  Eigen::Isometry3d delta_e =  init_est_e_inv * current_est_e;
  std::cout << print_Isometry3d(delta_e) << " delta_e\n";

  BotTrans init_est_inv;
  BotTrans* init_est_ptr = &init_est;
  bot_trans_copy(&init_est_inv, init_est_ptr);

  bot_trans_invert ( &init_est_inv );

  std::cout << print_BotTrans(&current_est) << " current_est\n";

  std::cout << print_BotTrans(&init_est_inv) << " init_est_inv\n";

  BotTrans delta;
  BotTrans* delta_ptr = &current_est;
  bot_trans_apply_trans( delta_ptr  ,  &init_est_inv );
  std::cout << print_BotTrans(&current_est) << " delta\n";

  return 0;
}


// #include "NeoHookeanFEMConstraint.h"
// #include <Eigen/LU>
// #include <math.h>
// #include <iostream>
// bool
// NeoHookeanFEMConstraint::
// ComputeDeformationGradient(const Eigen::VectorXd& x)
// {

// 	if(FEMConstraint::ComputeDeformationGradient(x))
// 	{
// 		mCacheFTF = (mCacheF.transpose())*mCacheF;
// 		mCacheInvF = (mCacheF.inverse());
// 		mCacheInvFT = mCacheInvF.transpose();
// 		// if(fabs(mCacheF.determinant())<1E-6)
// 			// std::cout<<mCacheF<<std::endl;
// 		return true;
// 	}
// 	return false;
// }
// void
// NeoHookeanFEMConstraint::
// ComputedP(const Eigen::Matrix2d& dF,Eigen::Matrix2d& dP)
// {
// 	double I3 = mCacheFTF.determinant();

// 	dP =
// 		mMu*dF+
// 		(mMu - mLambda*log(I3)*0.5)*mCacheInvFT*(dF.transpose())*mCacheInvFT+
// 		(mLambda*((mCacheInvF*dF).trace()))*mCacheInvFT;
// }
// NeoHookeanFEMConstraint::
// NeoHookeanFEMConstraint(const double& stiffness,const double& poisson_ratio,int i0,int i1,int i2,double vol,const Eigen::Matrix2d& invDm)
// 	:FEMConstraint(stiffness,poisson_ratio,i0,i1,i2,vol,invDm),
// 	mCacheFTF(Eigen::Matrix2d::Zero()),
// 	mCacheInvFT(Eigen::Matrix2d::Zero())
// {
// }
// double
// NeoHookeanFEMConstraint::
// EvalPotentialEnergy(const Eigen::VectorXd& x)
// {
// 	ComputeDeformationGradient(x);

// 	double I1 = mCacheFTF.trace();
// 	double I3 = mCacheFTF.determinant();

// 	return mVol*(0.25*mMu*(I1-log(I3)-3) + 0.125*mLambda*(log(I3)*log(I3)));
// }
// void
// NeoHookeanFEMConstraint::
// EvalGradient(const Eigen::VectorXd& x, Eigen::VectorXd& gradient)
// {
// 	ComputeDeformationGradient(x);

// 	double I3 = mCacheFTF.determinant();
// 	// std::cout<<I3<<std::endl;
// 	Eigen::Matrix2d P = (0.5*mMu)*(mCacheF-mCacheInvFT) + (0.5*mLambda*log(I3))*mCacheInvFT;

// 	P = mVol*P*mInvDm;

// 	gradient.block<2,1>(mi0*2,0) += -(P.block<2,1>(0,0) + P.block<2,1>(0,1));
// 	gradient.block<2,1>(mi1*2,0) += P.block<2,1>(0,0);
// 	gradient.block<2,1>(mi2*2,0) += P.block<2,1>(0,1);
// }

// void
// NeoHookeanFEMConstraint::
// EvalHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& dx, Eigen::VectorXd& dg)
// {
// 	ComputeDeformationGradient(x);
// 	Eigen::Matrix2d dDs,dF,dP;
// 	Eigen::Vector2d dx0(dx.block<2,1>(mi0*2,0));
// 	dDs.block<2,1>(0,0) = dx.block<2,1>(mi1*2,0)-dx0;
// 	dDs.block<2,1>(0,1) = dx.block<2,1>(mi2*2,0)-dx0;

// 	dF = dDs*(mInvDm);
// 	ComputedP(dF,dP);

// 	dP = mVol * dP * (mInvDm.transpose());

// 	dg.block<2,1>(mi0*2,0) += -(dP.block<2,1>(0,0) + dP.block<2,1>(0,1));
// 	dg.block<2,1>(mi1*2,0) += dP.block<2,1>(0,0);
// 	dg.block<2,1>(mi2*2,0) += dP.block<2,1>(0,1);
// }

// void
// NeoHookeanFEMConstraint::
// EvaluateDVector(int index, const Eigen::VectorXd& x,Eigen::VectorXd& d)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// void
// NeoHookeanFEMConstraint::
// EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// void
// NeoHookeanFEMConstraint::
// EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// int
// NeoHookeanFEMConstraint::
// GetNumHessianTriplets()
// {
// 	return 36;
// }


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsTernary.h"
#include "InterpolationType.h"

#include "catch.hpp"

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

#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD ternary kks in a loop", "[ternary kks loop]")
{
#ifdef _OPENMP
    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;
#endif

    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    const double Tmin     = 2990;
    const double Tmax     = 3020.;
    const int nTintervals = 10;
    const double deltaT   = (Tmax - Tmin) / (double)nTintervals;

    std::cout << "Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    // initial guesses
    const double init_guess[4] = { 0.33, 0.38, 0.32, 0.33 };
    double nominalc[2]         = { 0.33, 0.33 };

    std::vector<double> cl(2 * (nTintervals + 1));
    std::vector<double> cs(2 * (nTintervals + 1));

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary* cafe
        = new Thermo4PFM::CALPHADFreeEnergyFunctionsTernary(calphad_db,
            newton_db, energy_interp_func_type, conc_interp_func_type);

    {
        // serial loop
        for (int i = 0; i < nTintervals + 1; i++)
        {
            const double temperature = Tmin + i * deltaT;

            double phi = 0.5;

            double conc[4] = { init_guess[0], init_guess[1], // liquid
                init_guess[2], init_guess[3] }; // solid

            // compute equilibrium concentrations in each phase
            cafe->computePhaseConcentrations(temperature, nominalc, &phi, conc);

            std::cout << "Temperature = " << temperature << std::endl;
            std::cout << "Concentrations: cl = (" << conc[0] << "." << conc[1]
                      << ")"
                      << " and cs = (" << conc[2] << "," << conc[3] << ")"
                      << std::endl;

            cl[2 * i]     = conc[0];
            cl[2 * i + 1] = conc[1];
            cs[2 * i]     = conc[2];
            cs[2 * i + 1] = conc[3];
        }
    }

// parallel loop
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;
        double phi               = 0.5;

        double conc[4] = { init_guess[0], init_guess[1], // liquid
            init_guess[2], init_guess[3] }; // solid

        // compute equilibrium concentrations in each phase
        cafe->computePhaseConcentrations(temperature, nominalc, &phi, &conc[0]);

        CHECK(conc[0] == Approx(cl[2 * i]).margin(1.e-6));
        CHECK(conc[1] == Approx(cl[2 * i + 1]).margin(1.e-6));
        CHECK(conc[2] == Approx(cs[2 * i]).margin(1.e-6));
        CHECK(conc[3] == Approx(cs[2 * i + 1]).margin(1.e-6));
    }

#ifdef HAVE_OPENMP_OFFLOAD
    double* xdev = new double[4 * (nTintervals + 1)];
// clang-format off
#pragma omp target map(to : cafe [0:1],     \
                            init_guess[:4]) \
                   map(from : xdev[:4 * (nTintervals + 1)])
// clang-format on
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;

        double phi = 0.5;

        xdev[4 * i + 0] = init_guess[0];
        xdev[4 * i + 1] = init_guess[1]; // liquid
        xdev[4 * i + 2] = init_guess[2];
        xdev[4 * i + 3] = init_guess[3]; // solid
        // compute equilibrium concentrations in each phase
        cafe->computePhaseConcentrations(
            temperature, nominalc, &phi, &xdev[4 * i]);
    }

    for (int i = 0; i < nTintervals + 1; i++)
    {
        std::cout << "Device: x=" << xdev[4 * i] << "," << xdev[4 * i + 1]
                  << "," << xdev[4 * i + 2] << "," << xdev[4 * i + 3]
                  << std::endl;
        CHECK(xdev[4 * i + 0] == Approx(cl[2 * i]).margin(1.e-6));
        CHECK(xdev[4 * i + 1] == Approx(cl[2 * i + 1]).margin(1.e-6));
        CHECK(xdev[4 * i + 2] == Approx(cs[2 * i]).margin(1.e-6));
        CHECK(xdev[4 * i + 3] == Approx(cs[2 * i + 1]).margin(1.e-6));
    }

    delete[] xdev;
#endif

    delete cafe;
}


#pragma once
#include <Eigen/Dense>
#include "SdfObject.hpp"
#include "../accelerate/Bound3.hpp"

class SdfRoundCone : public SdfObject
{
public:
    SdfRoundCone(Eigen::Vector3f a, Eigen::Vector3f b, float ra, float rb) : SdfObject(a), a(a), b(b), ra(ra), rb(rb){};

    float sdf(const Eigen::Vector3f &position) const override
    {
        constexpr auto sign = [](auto a)
        { return a >= 0 ? 1 : -1; };

        constexpr auto dot = [](Eigen::Vector3f a)
        { return a.dot(a); };

        // sampling independent computations (only depend on shape)
        auto ba = b - a;
        auto l2 = dot(ba);
        auto rr = ra - rb;
        auto a2 = l2 - rr * rr;
        auto il2 = 1.0f / l2;

        // sampling dependant computations
        auto pa = position - a;
        auto y = pa.dot(ba);
        auto z = y - l2;
        auto x2 = dot(pa * l2 - ba * y);
        auto y2 = y * y * l2;
        auto z2 = z * z * l2;

        // single square root!
        auto k = sign(rr) * rr * rr * x2;
        if (sign(z) * a2 * z2 > k)
            return sqrt(x2 + z2) * il2 - rb;
        if (sign(y) * a2 * y2 < k)
            return sqrt(x2 + y2) * il2 - ra;
        return (sqrt(x2 * a2 * il2) + y * rr) * il2 - ra;
    };

    std::unique_ptr<Bound3> build_bound3() const override
    {
        auto aa = ra * Eigen::Vector3f::Ones();
        auto bb = rb * Eigen::Vector3f::Ones();
        auto min = (a - aa).cwiseMin(b - bb);
        auto max = (a + aa).cwiseMax(b + bb);
        return std::make_unique<Bound3>(min, max);
    };

private:
    Eigen::Vector3f a;

    Eigen::Vector3f b;

    float ra;

    float rb;
};


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Optimization/NelderMead.h"

using namespace Scine::Molassembler;

struct NelderMeadHimmelblau {
  double operator() (const Eigen::VectorXd& parameters) {
    assert(parameters.size() == 2);

    const double x = parameters(0);
    const double y = parameters(1);

    const double firstBracket = (x * x + y - 11);
    const double secondBracket = (x + y * y - 7);

    return firstBracket * firstBracket + secondBracket * secondBracket;
  }
};

BOOST_AUTO_TEST_CASE(NelderMead, *boost::unit_test::label("Temple")) {
  Eigen::Matrix<double, 2, 3> simplexVertices;
  simplexVertices << -3.0,  0.0,  0.0,
                     -1.5,  0.0, -3.0;

  struct NelderMeadChecker {
    bool shouldContinue(unsigned iteration, double /* lowestValue */, double stddev) {
      return iteration < 1000 && stddev > 0.01;
    }
  };

  auto optimizationResult = Temple::NelderMead<>::minimize(
    simplexVertices,
    NelderMeadHimmelblau {},
    NelderMeadChecker {}
  );

  BOOST_CHECK_MESSAGE(
    std::fabs(optimizationResult.value) <= 0.1,
    "Nelder-Mead does not find minimization of Himmelblau function, value is "
    << optimizationResult.value << " after " << optimizationResult.iterations
    << " iterations. Simplex vertices are: " << simplexVertices
  );
}


#ifndef __INS_HH__
#define __INS_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the INS Module On Board, Error equations based on Zipfel,
Figure 10.27, space stabilized INS with GPS updates) LIBRARY DEPENDENCY:
      ((../src/Ins.cpp))
*******************************************************************************/
#include <armadillo>
#include <cassert>
#include <fstream>
#include <functional>
#include <tuple>
#include "Time_management.hh"
#include "aux.hh"
#include "cad_utility.hh"
#include "dm_delta_ut.hh"
#include "integrate.hh"
#include "math_utility.hh"
#include "matrix/utility.hh"

class time_management;

class INS {
  TRICK_INTERFACE(INS);

 public:
  INS();

  INS(const INS& other);

  INS& operator=(const INS& other);

  void initialize();

  /* Internal Getter */
  arma::mat build_WEII();

  void load_location(double lonx, double latx, double alt);
  void load_angle(double yaw, double roll, double pitch);
  void load_geodetic_velocity(double alpha0x, double beta0x, double dvbe);
  void calculate_INS_derived_TEI();

  void update(double int_step);

  void set_ideal();
  void set_non_ideal();
  void set_gps_correction(unsigned int index);
  void set_liftoff(unsigned int index);

  /* Input File */

  std::function<arma::vec3()> grab_computed_WBIB;
  std::function<arma::vec3()> grab_error_of_computed_WBIB;

  std::function<arma::vec3()> grab_computed_FSPB;
  std::function<arma::vec3()> grab_error_of_computed_FSPB;

  // std::function<arma::vec3()>   grab_SBII;
  // std::function<arma::vec3()>   grab_VBII;
  // std::function<double()>       grab_dbi;
  // std::function<arma::mat33()>  grab_TBI;
  std::function<int()> grab_gps_update;
  std::function<arma::vec3()> grab_SXH;
  std::function<arma::vec3()> grab_VXH;
  std::function<void()> clear_gps_flag;
  // std::function<arma::mat33()>  grab_TEI;  // Only for testing
  // std::function<arma::vec3()>   grab_SBEE;
  // std::function<arma::vec3()>   grab_VBEE;
  // std::function<double()>   grab_phibdx;
  // std::function<double()>   grab_thtbdx;
  // std::function<double()>   grab_psibdx;
  std::function<arma::vec3()> grab_PHI;
  std::function<arma::vec3()> grab_DELTA_VEL;
  std::function<arma::vec3()> grab_PHI_HIGH;
  std::function<arma::vec3()> grab_PHI_LOW;

  double get_loncx();
  double get_latcx();
  double get_altc();
  double get_dvbec();
  double get_alphacx();
  double get_betacx();
  double get_phibdcx();
  double get_thtbdcx();
  double get_psibdcx();
  double get_thtvdcx();
  double get_phipcx();
  double get_alppcx();

  arma::vec3 get_SBIIC();
  arma::vec3 get_VBIIC();
  arma::vec3 get_WBICI();
  arma::vec3 get_EGRAVI();
  arma::mat33 get_TBIC();
  arma::vec3 get_SBEEC();
  arma::vec3 get_VBEEC();
  arma::mat33 get_TEIC();
  arma::vec4 get_TBDQ();
  arma::mat33 get_TBD();
  arma::mat33 get_TBICI();
  arma::mat33 get_TDCI();
  arma::vec3 get_WBECB();
  arma::vec3 get_ABICB();

  /* Internal Initializers */
  void default_data();

 private:
  arma::vec build_VBEB(double _alpha0x, double _beta0x, double _dvbe);

  time_management* time;

  /* Internal Propagator / Calculators */
  bool GPS_update();

  arma::vec3 calculate_INS_derived_postion(arma::vec3 SBII);
  arma::vec3 calculate_INS_derived_velocity(arma::vec3 VBII);
  arma::vec3 calculate_INS_derived_bodyrate(arma::mat33 TBIC, arma::vec3 WBICB);

  arma::mat33 calculate_INS_derived_TBI(arma::mat33 TBI);

  arma::vec3 calculate_gravity_error(double dbi);
  double calculate_INS_derived_dvbe();

  double calculate_INS_derived_alpha(arma::vec3 VBECB);
  double calculate_INS_derived_beta(arma::vec3 VBECB);

  double calculate_INS_derived_alpp(arma::vec3 VBECB);
  double calculate_INS_derived_phip(arma::vec3 VBECB);

  double calculate_INS_derived_psivd(arma::vec3 VBECD);
  double calculate_INS_derived_thtvd(arma::vec3 VBECD);

  void calculate_INS_derived_euler_angles(arma::mat33 TBD);

  void load_angle();
  void propagate_TBI_Q(double int_step, arma::vec3 WBICB);

  // void error_diagnostics();
  // For test
  arma::vec3 euler_angle(arma::mat33 TBD);
  arma::mat33 build_321_rotation_matrix(arma::vec3 angle);

  arma::vec AccelHarmonic(arma::vec3 SBII, double CS[21][21], int n_max,
                          int m_max, arma::mat33 TEIC);

  /* Internal Calculators */

  /* Sensors */

  /* Constants */
  arma::mat WEII;     /* *o  (r/s)    Earth's angular velocity (skew-sym) */
  double _WEII[3][3]; /* *o  (r/s)    Earth's angular velocity (skew-sym) */

  /* Propagative Stats */
  arma::vec EVBI;  /* *o  (m/s)   INS vel error */
  double _EVBI[3]; /* *o  (m/s)   INS vel error */

  arma::vec EVBID;  /* *o  (m/s)   INS vel error derivative */
  double _EVBID[3]; /* *o  (m/s)   INS vel error derivative */

  arma::vec ESBI;  /* *o  (m)     INS pos error */
  double _ESBI[3]; /* *o  (m)     INS pos error */

  arma::vec ESBID;  /* *o  (m)     INS pos error derivative */
  double _ESBID[3]; /* *o  (m)     INS pos error derivative */

  arma::vec RICI;  /* *o  (r)     INS tilt error derivative */
  double _RICI[3]; /* *o  (r)     INS tilt error */

  arma::vec RICID;  /* *o  (r)     INS tilt error derivative */
  double _RICID[3]; /* *o  (r)     INS tilt error derivative */

  /* Generating Outputs */
  arma::mat TBIC; /* *o  (--)    Computed T.M. of body wrt inertia coordinate */
  double _TBIC[3][3]; /* *o  (--)    Computed T.M. of body wrt inertia
                         coordinate */

  arma::vec TBIC_Q;  /* *o  (--)    Computed T.M of body wrt inertia coordinate
                        quaterion */
  double _TBIC_Q[4]; /* *o  (--)    Computed T.M of body wrt inertia coordinate
                        quaterion */

  arma::vec TBIDC_Q;  /* *io (--)    Transformation Matrix of body coord wrt
                         inertia coord derivative (Quaternion) */
  double _TBIDC_Q[4]; /* *io (--)    Transformation Matrix of body coord wrt
                         inertia coord derivative (Quaternion) */

  arma::vec
      SBIIC; /* *o  (m)     Computed pos of body wrt inertia reference point*/
  double _SBIIC[3]; /* *o  (m)     Computed pos of body wrt inertia reference
                       point*/

  arma::vec VBIIC;  /* *o  (m/s)   Computed body vel in inertia coor */
  double _VBIIC[3]; /* *o  (m/s)   Computed body vel in inertia coor */

  arma::vec SBEEC;  /* *o   (m)    Computed body position in ECEF */
  double _SBEEC[3]; /* *o   (m)    Computed body position in ECEF */

  arma::vec VBEEC;  /* *o   (m)    Computed body velocity in ECEF */
  double _VBEEC[3]; /* *o   (m)    Computed body velocity in ECEF */

  arma::vec
      WBICI; /* *o  (r/s)   Computed inertial body rate in inert coordinate */
  double _WBICI[3]; /* *o  (r/s)   Computed inertial body rate in inert
                       coordinate */

  arma::vec EGRAVI;  /* *o  (--)    error by gravity */
  double _EGRAVI[3]; /* *o  (--)    error by gravity */

  arma::vec VBECD;  /* *o  (m/s)   Geodetic velocity */
  double _VBECD[3]; /* *o  (m/s)   Geodetic velocity */

  arma::mat TDCI;     /* *o  (--)    Comp T.M. of geodetic wrt inertial */
  double _TDCI[3][3]; /* *o  (--)    Comp T.M. of geodetic wrt inertial */

  arma::mat TEIC;     /* *o  (--)   T.M. of inertia to ECEF */
  double _TEIC[3][3]; /* *o  (--)   T.M. of inertia to ECEF */

  arma::vec INS_I_ATT_ERR;
  double _INS_I_ATT_ERR[3];

  arma::vec TESTV;
  double _TESTV[3];

  arma::vec TMP_old;
  double _TMP_old[3];

  arma::vec VBIIC_old;
  double _VBIIC_old[3];

  arma::vec POS_ERR;
  double _POS_ERR[3];

  arma::vec GRAVGI;  /* *o (m/s2)  Gravity acceleration wrt inertial frame */
  double _GRAVGI[3]; /* *o (m/s2)  Gravity acceleration wrt inertial frame */

  arma::vec TBDQ;
  double _TBDQ[4];

  arma::mat TBD;
  double _TBD[3][3];

  arma::mat TBICI;
  double _TBICI[3][3];

  arma::mat TLI;
  double _TLI[3][3];

  arma::vec VBIIC_old_old;
  double _VBIIC_old_old[3];

  VECTOR(WBECB, 3); /* ** (r/s)  Body angular rate in ECEF */

  VECTOR(ABICB, 3); /* **  (m/s2) Computed body net acceleration */

  double
      dbic; /* *io  (m)     INS computed vehicle distance from Earth center */
  double dvbec; /* *io  (m/s)   Computed body speed wrt earth */

  double alphacx; /* *io  (d)     INS computed angle of attack */
  double betacx;  /* *io  (d)     INS computed sideslip angle */

  double thtvdcx; /* *io  (d)     INS computed vertical flight path angle */
  double psivdcx; /* *io  (d)     INS computed heading angle */

  double alppcx; /* *io  (d)     INS computed total angle of attack */
  double phipcx; /* *io  (d)     INS computed aero roll angle */

  double loncx; /* *io  (d)     INS derived longitude */
  double latcx; /* *io  (d)     INS derived latitude */
  double altc;  /* *io  (m)     INS derived altitude */

  double phibdcx; /* *io  (d)     INS computed geodetic Euler roll angle */
  double thtbdcx; /* *io  (d)     INS computed geodetic Euler pitch angle */
  double psibdcx; /* *io  (d)     INS computed geodetic Euler yaw angle */

  /* Non-propagating Diagnostic Variables */
  /* These can be deleted, but keep to remain trackable in trick simulator */

  double ins_pos_err;     /* *io  (m)     INS absolute postion error */
  double ins_vel_err;     /* *io  (m/s)   INS absolute velocity error */
  double ins_tilt_err;    /* *o  (r)     INS absolute tilt error */
  double ins_pose_err;    /* *io  (m)     INS absolute postion error */
  double ins_vele_err;    /* *io  (m/s)   INS absolute velocity error */
  double ins_phi_err;     /* *o (d)      INS absolute phi angle error */
  double ins_tht_err;     /* *o (d)     INS absolute tht angle error */
  double ins_psi_err;     /* *o  (d)     INS absolute psi angle error */
  unsigned int gpsupdate; /* *o (--)   Set wether use gps correction or not */
  unsigned int liftoff;   /* *o (--)  set LV liftoff */
  unsigned int ideal; /* *o (--)  choose which extropolate algorithm to use */

  unsigned int testindex;
};

#endif  // __INS_HH__


#include <cmath>
#include <exception>
#include <iostream>
#include <string>
#include <boost/algorithm/string.hpp>

class NucPepXform
{
    std::string baseDNA = "ACGT";
    std::string baseRNA = "ACGU";
    std::string basePEP = "KNKNTTTTRSRSIIMIQHQHPPPPRRRRLLLLEDEDAAAAGGGGVVVV.Y.YSSSS.CWCLFLF";

    int strToInt(std::string inStr, std::string inBase)
    {
        int retNum = 0;
        for (auto i: inStr)
        {
            retNum += pow(inBase.length(), inStr.length()-i-1) * (inBase.at(inStr.at(i)));
        }

        return retNum;
     };

    std::string intToStr(int inInt, std::string outBase, int maxDigits)
    {
        int remainder = inInt;
        std::string retStr = "";

        for (int i = maxDigits; i >= 0; --i)
        {
            retStr += outBase.at(trunc(remainder / (int)pow(outBase.length(), i)));
            remainder = trunc(remainder % (int)pow(outBase.length(), i));
        }

        return retStr;
    };

    std::string verify(std::string inStr, std::string strBase)
    {
        boost::to_upper(inStr);
        std::string outStr = "";

        for (auto i: inStr)
        {
            if (strBase.find(inStr.at(i)) >=0)
            {
                outStr += inStr.at(i);
            } else {
                std::cout << inStr.at(i) << ": invalid character, removing from value." << std::endl;
            };
        };

        return outStr;
    };

    std::string transcribe(std::string DNAseq)
    {
        std::string DNAstr = verify(DNAseq, baseDNA);
        std::string RNAstr = "";

        for (auto i: DNAstr)
        {
            RNAstr += intToStr(strToInt(&DNAstr.at(i), baseDNA), baseRNA, 0);
        };

        return RNAstr;
    };

    std::string revTranscribe(std::string RNAseq)
    {
        std::string RNAstr = verify(RNAseq, baseRNA);
        std::string DNAstr = "";

        for (auto i: RNAstr)
        {
            DNAstr += intToStr(strToInt(&RNAstr.at(i), baseRNA), baseDNA, 0);
        };

        return DNAstr;
    };

    std::string translate(std::string RNAseq)
    {
        std::string RNAstr = verify(RNAseq, baseRNA);
        std::string peptide = "";
        std::string codon = "123";

        for (int i=0; i<(int)RNAseq.length(); ++i)
        {
            codon = RNAstr.substr(i, i+codon.length());
            peptide += intToStr(strToInt(codon, baseRNA), basePEP, 0);

        };

        return peptide;
    };
};


#include <iostream>
#include <memory>
#include <random>
#include <string>

#include <Eigen/Sparse>

#include "GeometricMultigridOperators.h"
#include "InitialMultigridTestDomains.h"
#include "Renderer.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "UniformGrid.h"
#include "Utilities.h"

using namespace FluidSim2D::RenderTools;
using namespace FluidSim2D::SimTools;

std::unique_ptr<Renderer> renderer;

static constexpr int gridSize = 256;
static constexpr bool useComplexDomain = true;
static constexpr bool useSolidSphere = true;

int main(int argc, char** argv)
{
	using namespace GeometricMultigridOperators;

	using StoreReal = double;
	using SolveReal = double;

	using Vector = std::conditional<std::is_same<SolveReal, float>::value, Eigen::VectorXf, Eigen::VectorXd>::type;

	UniformGrid<CellLabels> domainCellLabels;
	VectorGrid<StoreReal> boundaryWeights;

	int mgLevels;
	{
		UniformGrid<CellLabels> baseDomainCellLabels;
		VectorGrid<StoreReal> baseBoundaryWeights;

		// Complex domain set up
		if (useComplexDomain)
			buildComplexDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								useSolidSphere);

		// Simple domain set up
		else
			buildSimpleDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								1 /*dirichlet band*/);

		// Build expanded domain
		std::pair<Vec2i, int> mgSettings = buildExpandedDomain(domainCellLabels, boundaryWeights, baseDomainCellLabels, baseBoundaryWeights);

		mgLevels = mgSettings.second;
	}

	SolveReal dx = boundaryWeights.dx();

	UniformGrid<StoreReal> solutionGrid(domainCellLabels.size(), 0);

	tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(),tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = domainCellLabels.unflatten(cellIndex);

			if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
				domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
			{
				Vec2f point(dx * Vec2f(cell));
				solutionGrid(cell) = 4. * (std::sin(2 * PI * point[0]) * std::sin(2 * PI * point[1]) +
											std::sin(4 * PI * point[0]) * std::sin(4 * PI * point[1]));
			}
		}
	});

	// Print initial guess
	solutionGrid.printAsOBJ("initialGuess");

	UniformGrid<CellLabels> coarseCellLabels = buildCoarseCellLabels(domainCellLabels);

	assert(unitTestBoundaryCells<StoreReal>(domainCellLabels, &boundaryWeights) && unitTestBoundaryCells<StoreReal>(coarseCellLabels));
	assert(unitTestExteriorCells(domainCellLabels) && unitTestExteriorCells(coarseCellLabels));
	assert(unitTestCoarsening(coarseCellLabels, domainCellLabels));

	SolveReal coarseDx = 2 * dx;

	{
		//
		// Debug test for simple transfers
		//

		// Test a simple tansfer to the coarse grid and a transfer back
		UniformGrid<StoreReal> coarseInitialGuess(coarseCellLabels.size(), 0);

		downsample<SolveReal>(coarseInitialGuess, solutionGrid, coarseCellLabels, domainCellLabels);

		coarseInitialGuess.printAsOBJ("downsampledGrid");

		// Transfer back
		UniformGrid<StoreReal> transferGrid(domainCellLabels.size(), 0);
		upsampleAndAdd<SolveReal>(transferGrid, coarseInitialGuess, domainCellLabels, coarseCellLabels);

		transferGrid.printAsOBJ("upsampledGrid");
	}

	//
	// Debug test by downsampling residual, solving for correction error and upsampling correction back
	//
	{
		//
		// Compute residual
		//

		UniformGrid<StoreReal> residualGrid(domainCellLabels.size(), 0);
		UniformGrid<StoreReal> rhsGrid(domainCellLabels.size(), 0);

		computePoissonResidual<SolveReal>(residualGrid, solutionGrid, rhsGrid, domainCellLabels, dx, &boundaryWeights);

		residualGrid.printAsOBJ("residualGrid");

		//
		// Restrict residual to coarse RHS
		//

		UniformGrid<StoreReal> coarseRHSGrid(coarseCellLabels.size(), 0);
		downsample<SolveReal>(coarseRHSGrid, residualGrid, coarseCellLabels, domainCellLabels);

		coarseRHSGrid.printAsOBJ("downsampledResidual");

		//
		// Apply direct solver
		//

		UniformGrid<StoreReal> coarseSolution(coarseCellLabels.size(), 0);

		{

			UniformGrid<StoreReal> coarseResidualGrid(coarseCellLabels.size(), 0);

			//
			// Solver with direct solver
			//

			// Build indices
			int interiorCellCount = 0;

			UniformGrid<int> interiorCellIndices(coarseCellLabels.size(), -1);

			forEachVoxelRange(Vec2i(0), coarseCellLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
					coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					interiorCellIndices(cell) = interiorCellCount++;
			});

			Vector rhsVector = Vector::Zero(interiorCellCount);

			tbb::parallel_for(tbb::blocked_range<int>(0, coarseCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = interiorCellIndices.unflatten(cellIndex);

					if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = interiorCellIndices(cell);
						assert(index >= 0);

						rhsVector(index) = coarseRHSGrid(cell);
					}
					else
					{
						assert(interiorCellIndices(cell) == -1);
					}
				}
			});

			// Build rows
			std::vector<Eigen::Triplet<SolveReal>> sparseElements;

			SolveReal gridScalar = 1. / sqr(coarseDx);
			forEachVoxelRange(Vec2i(0), coarseCellLabels.size(), [&](const Vec2i &cell)
			{
				if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL)
				{
					int diagonal = 0;
					int index = interiorCellIndices(cell);
					assert(index >= 0);

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);
							assert(coarseCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL ||
									coarseCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL);

							int adjacentIndex = interiorCellIndices(adjacentCell);
							assert(adjacentIndex >= 0);

							sparseElements.emplace_back(index, adjacentIndex, -gridScalar);
						}

					sparseElements.emplace_back(index, index, 4. * gridScalar);
				}
				else if (coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					int diagonal = 0;
					int index = interiorCellIndices(cell);
					assert(index >= 0);

					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(cell, axis, direction);

							if (coarseCellLabels(adjacentCell) == CellLabels::INTERIOR_CELL ||
								coarseCellLabels(adjacentCell) == CellLabels::BOUNDARY_CELL)
							{
								int adjacentIndex = interiorCellIndices(adjacentCell);
								assert(adjacentIndex >= 0);

								sparseElements.emplace_back(index, adjacentIndex, -gridScalar);
								++diagonal;
							}
							else
							{
								assert(interiorCellIndices(adjacentCell) == -1);
								if (coarseCellLabels(adjacentCell) == CellLabels::DIRICHLET_CELL)
									++diagonal;
							}
						}

					sparseElements.emplace_back(index, index, diagonal * gridScalar);
				}
			});

			// Solve system
			Eigen::SparseMatrix<SolveReal> sparseMatrix(interiorCellCount, interiorCellCount);
			sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());
			sparseMatrix.makeCompressed();

			Eigen::SimplicialCholesky<Eigen::SparseMatrix<SolveReal>> solver;
			solver.compute(sparseMatrix);

			if (solver.info() != Eigen::Success)
			{
			    std::cout << "Solver failed to pre-compute system" << std::endl;
			    return 0;
			}

			Vector solutionVector = solver.solve(rhsVector);
			if (solver.info() != Eigen::Success)
			{
			    std::cout << "Solver failed" << std::endl;
			    return 0;
			}

			tbb::parallel_for(tbb::blocked_range<int>(0, coarseCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
				{
					Vec2i cell = interiorCellIndices.unflatten(cellIndex);

					if (coarseCellLabels(cell) == CellLabels::INTERIOR_CELL ||
						coarseCellLabels(cell) == CellLabels::BOUNDARY_CELL)
					{
						int index = interiorCellIndices(cell);
						assert(index >= 0);

						coarseSolution(cell) = solutionVector(index);
					}
					else
					{
						assert(interiorCellIndices(cell) == -1);
					}
				}
			});

			coarseSolution.printAsOBJ("coarseSolutionGrid");
		}

		//
		// Prolongate solution
		//

		UniformGrid<StoreReal> correctionGrid(domainCellLabels.size(), 0);

		upsampleAndAdd<SolveReal>(correctionGrid, coarseSolution, domainCellLabels, coarseCellLabels);

		correctionGrid.printAsOBJ("prolongatedCorrectio");

		//
		// Apply correction
		//

		upsampleAndAdd<SolveReal>(solutionGrid, coarseSolution, domainCellLabels, coarseCellLabels);
		solutionGrid.printAsOBJ("solutionGrid");

		//
		// Print out grids
		//

		// Print domain labels to make sure they are set up correctly
		int pixelHeight = 1080;
		int pixelWidth = pixelHeight;
		renderer = std::make_unique<Renderer>("MG Error Correction and Transfer Test", Vec2i(pixelWidth, pixelHeight), Vec2f(0), 1, &argc, argv);

		ScalarGrid<float> tempGrid(Transform(dx, Vec2f(0)), domainCellLabels.size());

		tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = domainCellLabels.unflatten(cellIndex);

				tempGrid(cell) = float(domainCellLabels(cell));
			}
		});

		tempGrid.drawVolumetric(*renderer, Vec3f(0), Vec3f(1), float(CellLabels::INTERIOR_CELL), float(CellLabels::BOUNDARY_CELL));

		renderer->run();
	}
}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <ostream>
#include <vector>
#include <array>
#include <random>
#include <cassert>
#include <mpi.h>
#include <Eigen/Dense>
#include <fstream>
#include "StrategyN2M3.hpp"

// calculate the distribution of fixation probability rho
// against randomly selected N2M3 deterministic strategies

std::array<double,2> CalcPayoffs(const std::array<double,64>& stationary_state, double benefit) {
  const double cost = 1.0;
  std::array<double,2> ans = {0.0, 0.0};
  for (size_t i = 0; i < 64; i++) {
    StateN2M3 s(i);
    double pa = 0.0, pb = 0.0;
    if (s.a_1 == C) { pa -= cost; pb += benefit; }
    if (s.b_1 == C) { pb -= cost; pa += benefit; }
    ans[0] += stationary_state[i] * pa;
    ans[1] += stationary_state[i] * pb;
  }
  return std::move(ans);
}

double FixationProb(size_t N, double sigma, double e, double benefit, const StrategyN2M3 &res, const StrategyN2M3 &mut, double s_yy) {
  auto a_xx = mut.StationaryState(e);
  auto a_xy = mut.StationaryState(e, &res);

  double s_xx = CalcPayoffs(a_xx, benefit)[0];
  auto _xy = CalcPayoffs(a_xy, benefit);
  double s_xy = _xy[0];
  double s_yx = _xy[1];

  // \frac{1}{\rho} = \sum_{i=0}^{N-1} \exp\left( \sigma \sum_{j=1}^{i} \left[(N-j-1)s_{yy} + js_{yx} - (N-j)s_{xy} - (j-1)s_{xx} \right] \right) \\
  //                = \sum_{i=0}^{N-1} \exp\left( \frac{\sigma i}{2} \left[(-i+2N-3)s_{yy} + (i+1)s_{yx} - (-i+2N-1)s_{xy} - (i-1)s_{xx} \right] \right)

  double num_games = (N-1);
  s_xx /= num_games;
  s_yy /= num_games;
  s_xy /= num_games;
  s_yx /= num_games;
  double rho_inv = 0.0;
  for (int i=0; i < N; i++) {
    double x = sigma * i * 0.5 * (
        (2*N-3-i) * s_yy
            + (i+1) * s_yx
            - (2*N-1-i) * s_xy
            - (i-1) * s_xx
    );
    rho_inv += std::exp(x);
  }
  return 1.0 / rho_inv;
}

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  Eigen::initParallel();
  if( argc != 8 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <N> <sigma> <e> <benefit> <resident 0:caprin, +:num_resident_samples> <num_mutants> <seed>" << std::endl;
    MPI_Finalize();
    return 1;
  }

  size_t N = std::strtoul(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  double benefit = std::strtod(argv[4], nullptr);

  long n_resident = std::strtol(argv[5], nullptr, 0);
  long n_mutants = std::strtol(argv[6], nullptr, 0);
  int seed = std::strtol(argv[7], nullptr, 0);

  std::uniform_int_distribution<uint64_t > dist(0, std::numeric_limits<uint64_t>::max() );

  const size_t NUM_BINS = 1000;
  std::vector<size_t> counts(NUM_BINS, 0ul);
  size_t robust_count = 0ul;

  int my_rank = 0;
  MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);
  int num_procs = 0;
  MPI_Comm_size(MPI_COMM_WORLD, &num_procs);

  if (n_resident > 0) {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);
      size_t my_n_resident = n_resident / num_procs;
      if (my_rank < n_resident % num_procs) { my_n_resident++; }
      // std::cerr << "my_n_resident: " << my_n_resident << " " << my_rank << ' ' << th << std::endl;

      for (size_t i = 0; i < my_n_resident; i++) {
        uint64_t r = dist(rnd);
        StrategyN2M3 res(r);
        auto a_yy = res.StationaryState(e);
        double s_yy = CalcPayoffs(a_yy, benefit)[0];
        for (size_t j = 0; j < n_mutants; j++) {
          uint64_t r2 = dist(rnd);
          StrategyN2M3 mut(r2);
          double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yy);
          size_t b = static_cast<size_t>(rho * NUM_BINS);
          counts[b]++;
          if (rho <= 1.0 / N) { robust_count++; }
        }
      }
    }
  }
  else {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd_tl(seq);

      StrategyN2M3 res = StrategyN2M3::CAPRI2();
      auto a_yy = res.StationaryState(e);
      double s_yy = CalcPayoffs(a_yy, benefit)[0];

      size_t my_n_mutants = n_mutants / num_procs;
      if (my_rank < n_mutants % num_procs) { my_n_mutants++; }
      //std::cerr << "my_n_mutants: " << my_n_mutants << ' ' << my_rank << ' ' << th << std::endl;

      for (size_t j = 0; j < my_n_mutants; j++) {
        uint64_t r2 = dist(rnd_tl);
        StrategyN2M3 mut(r2);
        double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yy);
        size_t b = static_cast<size_t>(rho * NUM_BINS);
        counts[b] += 1;
        if (rho <= 1.0 / N) { robust_count++; }
      }
    }
  }

  // reduce counts
  std::vector<size_t> all_counts(NUM_BINS, 0ul);
  size_t all_robust_count = 0ul;
  MPI_Reduce(counts.data(), all_counts.data(), all_counts.size(), MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&robust_count, &all_robust_count, 1, MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);

  // print counts
  if (my_rank == 0) {
    std::ofstream fout("dist.dat");
    double dx = 1.0 / NUM_BINS;
    double total = (n_resident == 0) ? n_mutants : n_resident * n_mutants;
    for (size_t i = 0; i < NUM_BINS; i++) {
      fout << i * dx << ' ' << (double)all_counts[i]/total << std::endl;
    }

    std::cerr << "robust_count/total: " << all_robust_count << " / " << total << " : " << (double)all_robust_count/total << std::endl;
  }

  MPI_Finalize();
  return 0;
}


#include "CalibrationTool.h"

#include <Eigen/Dense>
#include <fstream>
#include <iostream>

namespace echobot {

void CalibrationTool::calibrate() {

}

Eigen::Affine3d CalibrationTool::loadCalFile(std::string path) {
    auto calMat = Eigen::Affine3d::Identity();
    std::ifstream inFile;
    inFile.open(path);

    if(inFile.is_open())
    {
        for(int row = 0; row < calMat.rows(); row++){
            for(int col = 0; col < calMat.cols(); col++)
            {
                double item = 0;
                inFile >> item;
                calMat(row, col) = item;
            }
        }
        inFile.close();
    }
    return calMat;
}

void CalibrationTool::saveCalFile(std::string path, Eigen::Affine3d calMat) {
    std::ofstream outFile;
    outFile.open(path);

    if(outFile.is_open()){
        outFile << calMat.matrix();
        outFile.close();
    }
}

CalibrationTool::CalibrationTool() {
    mCalibrationFilePath = Config::getConfigPath() + "calibration/";
    m_rMb = loadCalFile(mCalibrationFilePath + "camMbase.cal");
    m_eeMt = loadCalFile(mCalibrationFilePath + "eeMtool.cal");
    m_tMus = loadCalFile(mCalibrationFilePath + "toolMus.cal");
    m_registration_pcMdata = loadCalFile(mCalibrationFilePath + "registration_pcMdata.cal");
    m_registration_pcMt = loadCalFile(mCalibrationFilePath + "registration_pcMt.cal");
    std::cout << m_registration_pcMdata.matrix() << std::endl;
}

void CalibrationTool::set_rMb(Eigen::Affine3d mat) {
    m_rMb = mat;
}

void CalibrationTool::set_eeMt(Eigen::Affine3d mat) {
    m_eeMt = mat;
}

void CalibrationTool::set_tMus(Eigen::Affine3d mat) {
    m_tMus = mat;
}

void CalibrationTool::set_registration_pcMdata(Eigen::Affine3d mat) {
    m_registration_pcMdata = mat;
}

void CalibrationTool::set_registration_pcMt(Eigen::Affine3d mat) {
    m_registration_pcMt = mat;
}

Eigen::Affine3d CalibrationTool::get_registration_pcMdata(){
    std::cout << m_registration_pcMdata.matrix() << std::endl;
    return m_registration_pcMdata;
}

}