#include "Arduino.h"
#include "math.h"
#include "navduino.h"
#include "wgs84.h"
#include "eigen.h"
#include <Eigen/LU>
#include <Eigen/Geometry>

using namespace Eigen;

/*
  Description:
  ------------
  This function converts a vector of euler angles into a Direction Cosine
  Matrix (DCM) given a rotation and frame sequence. In the case of this
  function, the returned DCM maps vectors from one coordinate frame (either
  North-East-Down (NED) or body frame) into the other. This is done by
  multiplying the DCM with the vector to be rotated: v_body = DCM * v_NED.

  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles to describe the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Matrix3f dcm - Direction cosine matrix (rotation matix)
*/
Matrix3f angle2dcm(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    float roll;
    float pitch;
    float yaw;

    if (angle_unit == DEGREES)
    {
        roll  = deg2rad(angles(0));
        pitch = deg2rad(angles(1));
        yaw   = deg2rad(angles(2));
    }
    else
    {
        roll  = angles(0);
        pitch = angles(1);
        yaw   = angles(2);
    }

    Matrix3f R1(3, 3);
    Matrix3f R2(3, 3);
    Matrix3f R3(3, 3);

    R1 << 1,          0,         0,
          0,  cos(roll), sin(roll),
          0, -sin(roll), cos(roll);

    R2 << cos(pitch), 0, -sin(pitch),
                   0, 1,           0,
          sin(pitch), 0,  cos(pitch);

    R3 << cos(yaw), sin(yaw), 0,
         -sin(yaw), cos(yaw), 0,
                 0,        0, 1;

    Matrix3f dcm(3, 3);

    // Note that multiplication of the matrices are opposite to the
    // rotation sequence due to how matrix multiplication works
    if (rotation_sequence == 321)
        dcm = R1 * R2 * R3;
    else if (rotation_sequence == 312)
        dcm = R2 * R1 * R3;
    else if (rotation_sequence == 231)
        dcm = R1 * R3 * R2;
    else if (rotation_sequence == 213)
        dcm = R3 * R1 * R2;
    else if (rotation_sequence == 132)
        dcm = R2 * R3 * R1;
    else if (rotation_sequence == 123)
        dcm = R3 * R2 * R1;
    else
        dcm = R1 * R2 * R3;

    if (!NED_to_body)
        return dcm.transpose();

    return dcm;
}

/*
  Description:
  ------------
  This function converts a Direction Cosine Matrix (DCM) into the
  corresponding euler angles.

  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Matrix3f& dcm          - Direction cosine matrix (rotation matix)
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f angles - Vector of euler angles to describe the rotation
*/
Vector3f dcm2angle(const Matrix3f& dcm, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    Vector3f angles;

    if (rotation_sequence == 321)
    {
        if (NED_to_body)
        {
            angles << atan2(dcm(1, 2), dcm(2, 2)), // Roll
                     -asin(dcm(0, 2)),             // Pitch
                      atan2(dcm(0, 1), dcm(0, 0)); // Yaw
        }
        else
        {
            angles << atan2(dcm(2, 1), dcm(2, 2)), // Roll
                     -asin(dcm(2, 0)),             // Pitch
                      atan2(dcm(1, 0), dcm(0, 0)); // Yaw
        }
    }

    if (angle_unit == DEGREES)
    {
        angles(0) = rad2deg(angles(0));
        angles(1) = rad2deg(angles(1));
        angles(2) = rad2deg(angles(2));
    }

    return angles;
}

/*
  Description:
  ------------
  Convert a sequence of euler angles to an equivalent unit quaternion.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles to describe the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf angle2quat(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    Quaternionf quat(angle2dcm(angles, angle_unit, NED_to_body, rotation_sequence));
    return quat;
}

/*
  Description:
  ------------
  Convert a unit quaternion to the equivalent sequence of euler angles
  about the rotation_sequence axes.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Quaternionf quat       - Quaternion that describes the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f& angles - Vector of euler angles to describe the rotation
*/
Vector3f quat2angle(const Quaternionf& quat, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2angle(quat.toRotationMatrix(), angle_unit, NED_to_body, rotation_sequence);
}

/*
  Description:
  ------------
  Convert a single unit quaternion to one DCM.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions

  Arguments:
  ----------
  * const Quaternionf quat       - Quaternion that describes the rotation
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Matrix3f& dcm - Rotates vectors from one coordinate frame to the other
*/
Matrix3f quat2dcm(const Quaternionf& quat)
{
    return quat.toRotationMatrix();
}

/*
  Description:
  ------------
  Convert a DCM to a unit quaternion.

  https://eater.net/quaternions
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Axes_conventions

  Arguments:
  ----------
  * const Matrix3f& dcm          - Direction cosine matrix (rotation matix)
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf dcm2quat(const Matrix3f& dcm)
{
    Quaternionf quat(dcm);
    return quat;
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a DCM.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Vector3f& vec - Rodrigues rotation vector

  Returns:
  --------
  * Matrix3f dcm - Direction cosine matrix (rotation matix)
*/
Matrix3f vec2dcm(const Vector3f& vec)
{
    float theta   = vec.norm();
    Vector3f axis = vec / theta;

    AngleAxisf angAxis(theta, axis);
    return angAxis.toRotationMatrix();
}

/*
  Description:
  ------------
  Convert a DCM to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix (rotation matix)

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f dcm2vec(const Matrix3f& dcm)
{
    AngleAxisf angAxis(dcm);
    return angAxis.angle() * angAxis.axis();
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a quaternion.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://eater.net/quaternions

  Arguments:
  ----------
  * const Vector3f& vec - Rodrigues rotation vector

  Returns:
  --------
  * Quaternionf quat - Quaternion that describes the rotation
*/
Quaternionf vec2quat(const Vector3f& vec)
{
    return dcm2quat(vec2dcm(vec));
}

/*
  Description:
  ------------
  Convert a quaternion to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://eater.net/quaternions

  Arguments:
  ----------
  * const Quaternionf& quat - Quaternion that describes the rotation

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f quat2vec(const Quaternionf& quat)
{
    AngleAxisf angAxis(quat);
    return angAxis.angle() * angAxis.axis();
}

/*
  Description:
  ------------
  Convert a Rodrigues rotation vector to a vector of euler angles.

  **NOTE: The Rodrigues vector's rotation angle must be in RADIANS.**

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& vec          - Rodrigues rotation vector
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f angles - Vector of euler angles
*/
Vector3f vec2angle(const Vector3f& vec, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2angle(vec2dcm(vec), angle_unit, NED_to_body, rotation_sequence);
}

/*
  Description:
  ------------
  Convert a vector of euler angles to a Rodrigues rotation vector.

  https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf
  https://en.wikipedia.org/wiki/Axes_conventions
  https://en.wikipedia.org/wiki/Euler_angles

  Arguments:
  ----------
  * const Vector3f& angles       - Vector of euler angles
  * const bool& angle_unit       - Unit of the euler angles (rad or degrees)
  * const bool& NED_to_body      - Rotate either to or from the NED frame
  * const int& rotation_sequence - The order in which the euler angles are applied
                                   to the rotation. 321 is the standard rotation
                                   sequence for aerial navigation

  Returns:
  --------
  * Vector3f vec - Rodrigues rotation vector (rotation angle is in RADIANS)
*/
Vector3f angle2vec(const Vector3f& angles, const bool& angle_unit, const bool& NED_to_body, const int& rotation_sequence)
{
    return dcm2vec(angle2dcm(angles, angle_unit, NED_to_body, rotation_sequence));
}

/*
  Description:
  ------------
  Calculate Earth's geocentric radius at a given geodetic latitude in
  meters. "The geocentric radius is the distance from the Earth's
  center to a point on the spheroid surface at geodetic latitude".

  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * float radius - Earth's geocentric radius in meters at a given latitude in meters
*/
float earthGeoRad(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    float num = pow(a_sqrd * cos(lat), 2) + pow(b_sqrd * sin(lat), 2);
    float den = pow(a * cos(lat), 2) + pow(b * sin(lat), 2);

    return sqrt(num / den);
}

/*
  Description:
  ------------
  Calculate Earth's radius of curvature in the prime vertical (East -
  West) - denoted as N - and meridian (North - South) - denoted as
  M - at a given latitude.

  https://en.wikipedia.org/wiki/Radius_of_curvature
  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * Vector2f radii - Earth's radii of curvature { N, M } in meters at a given latitude
*/
Vector2f earthRad(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    float R_N = a / sqrt(1 - (ecc_sqrd * pow(sin(lat), 2)));
    float R_M = (a * (1 - ecc_sqrd)) / pow(1 - (ecc_sqrd * pow(sin(lat), 2)), 1.5);

    Vector2f radii;
    radii << R_N, // Earth's prime-vertical radius of curvature
             R_M; // Earth's meridional radius of curvature

    return radii;
}

/*
  Description:
  ------------
  Calculate azimuthal radius at a given latitude. This is the
  Earth's radius of cuvature at a given latitude in the
  direction of a given asimuth.

  https://en.wikipedia.org/wiki/Radius_of_curvature
  https://en.wikipedia.org/wiki/Earth_radius
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * float radius - Earth's azimuthal radius in meters at a given latitude and azimuth
*/
float earthAzimRad(const float& lat, const float& _azimuth, const bool& angle_unit)
{
    float azimuth = _azimuth;

    if (angle_unit == DEGREES)
        azimuth = deg2rad(azimuth);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float N = eradvec(0);
    float M = eradvec(1);

    return 1 / ((pow(cos(azimuth), 2) / M) + (pow(sin(azimuth), 2) / N));
}

/*
  Description:
  ------------
  Calculate Earth's angular velocity in m/s resolved in the NED frame at a given
  latitude.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const float& _lat      - Angle of latitude
  * const bool& angle_unit - Unit of the latitude angle (rad or degrees)

  Returns:
  --------
  * Vector3f e - Earth's angular velocity resolved in the NED frame at a given latitude
*/
Vector3f earthRate(const float& _lat, const bool& angle_unit)
{
    float lat = _lat;

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f e;
    e << omega_E * cos(lat),
         0,
        -omega_E * sin(lat);

    return e;
}

/*
  Description:
  ------------
  Calculate the latitude, longitude, and altitude (LLA) angular rates
  given the locally tangent velocity in the NED frame and latitude.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& vned   - Velocity vector in m/s in the NED frame
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees) (this also affects the output units
                             (rad/s or degrees/s)

  Returns:
  --------
  * Vector3f lla_dot - LLA angular rates given the locally tangent velocity
                       in the NED frame and latitude
*/
Vector3f llaRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit)
{
    float VN = vned(0);
    float VE = vned(1);
    float VD = vned(2);

    float lat = lla(0);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);
    float Rns = eradvec(1);

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f lla_dot;

    if (angle_unit == RADIANS)
    {
        lla_dot << VN / (Rns + alt),
                   VE / (Rew + alt) / cos(lat),
                  -VD;
    }
    else
    {
        lla_dot << rad2deg(VN / (Rns + alt)),
                   rad2deg(VE / (Rew + alt) / cos(lat)),
                  -VD;
    }

    return lla_dot;
}

/*
  Description:
  ------------
  Calculate the navigation/transport angular rates given the locally tangent
  velocity in the NED frame and latitude. The navigation/transport rate is
  the angular velocity of the NED frame relative to the ECEF frame.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& vned   - Velocity vector in m/s in the NED frame
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees) (this also affects the output units
                             (rad/s or degrees/s)

  Returns:
  --------
  * Vector3f rho - Navigation/transport angular rates in the ECEF frame given
                   the locally tangent velocity in the NED frame and latitude
*/
Vector3f navRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit)
{
    float VN = vned(0);
    float VE = vned(1);

    float lat = lla(0);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);
    float Rns = eradvec(1);

    if (angle_unit == DEGREES)
        lat = deg2rad(lat);

    Vector3f rho;

    if (angle_unit == RADIANS)
    {
        rho << VE / (Rew + alt),
              -VN / (Rns + alt),
              -VE * tan(lat) / (Rew + alt);
    }
    else
    {
        rho << rad2deg(VE / (Rew + alt)),
               rad2deg(-VN / (Rns + alt)),
               rad2deg(-VE * tan(lat) / (Rew + alt));
    }

    return rho;
}

/*
  Description:
  ------------
  Convert a LLA coordinate to an ECEF coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Vector3f ecef - ECEF coordinate in meters
*/
Vector3f lla2ecef(const Vector3f& lla, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);
    float alt = lla(2);

    Vector2f eradvec = earthRad(lat, angle_unit);
    float Rew = eradvec(0);

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);
    }

    Vector3f ecef;

    ecef << (Rew + alt) * cos(lat) * cos(lon),
            (Rew + alt)* cos(lat)* sin(lon),
            ((1 - ecc_sqrd) * Rew + alt)* sin(lat);

    return ecef;
}

/*
  Description:
  ------------
  Convert an ECEF coordinate to a LLA coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef   - ECEF coordinate in meters
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Vector3f lla - LLA coordinate (altitude in meters)
*/
Vector3f ecef2lla(const Vector3f& ecef, const bool& angle_unit)
{
    float x = ecef(0);
    float y = ecef(1);
    float z = ecef(2);

    float x_sqrd = pow(x, 2);
    float y_sqrd = pow(y, 2);
    float z_sqrd = pow(z, 2);

    float lon = atan2(y, x);

    float p      = sqrt(x_sqrd + y_sqrd);
    float p_sqrd = pow(p, 2);
    float F      = 54 * b_sqrd * z_sqrd;
    float G      = p_sqrd + ((1 - ecc_sqrd) * z_sqrd) - (ecc_sqrd * (a_sqrd - b_sqrd));
    float G_sqrd = pow(G, 2);
    float c      = (pow(ecc, 4) * F * p_sqrd) / pow(G, 3);
    float c_sqrd = pow(c, 2);
    float s      = pow(1 + c + sqrt(c_sqrd + (2 * c)), 1.5);
    float k      = s + 1 + (1 / s);
    float k_sqrd = pow(k, 2);
    float P      = F / (3 * k_sqrd * G_sqrd);
    float Q      = sqrt(1 + (2 * pow(ecc, 4) * P));
    float r0     = ((-P * ecc_sqrd * p) / (1 + Q)) + sqrt((0.5 * a_sqrd * (1 + (1 / Q))) - ((P * (1 - ecc_sqrd) * z_sqrd) / (Q * (1 + Q))) - (0.5 * P * p_sqrd));
    float U      = sqrt(pow(p - (ecc_sqrd * r0), 2) + z_sqrd);
    float V      = sqrt(pow(p - (ecc_sqrd * r0), 2) + ((1 - ecc_sqrd) * z_sqrd));
    float z0     = (b_sqrd * z) / (a * V);

    float h   = U * (1 - (b_sqrd / (a * V)));
    float lat = atan2(z + (ecc_prime_sqrd * z0), p);

    if (angle_unit == DEGREES)
    {
        lat = rad2deg(lat);
        lon = rad2deg(lon);
    }

    Vector3f lla;
    lla << lat,
           lon,
           h;

    return lla;
}

/*
  Description:
  ------------
  Find the direction cosine matrix that describes the rotation from the ECEF
  coordinate frame to the NED frame given a lat/lon/alt location.

  https://www.mathworks.com/help/aeroblks/directioncosinematrixeceftoned.html
  https://en.wikipedia.org/wiki/Rotation_matrix
  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla    - LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * Matrix3f C -
*/
Matrix3f ecef2ned_dcm(const Vector3f& lla, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);
    }

    Matrix3f C;

    C(0, 0) = -sin(lat) * cos(lon);
    C(0, 1) = -sin(lat) * sin(lon);
    C(0, 2) = cos(lat);

    C(1, 0) = -sin(lon);
    C(1, 1) = cos(lon);
    C(1, 2) = 0;

    C(2, 0) = -cos(lat) * cos(lon);
    C(2, 1) = -cos(lat) * sin(lon);
    C(2, 2) = -sin(lat);

    return C;
}

/*
  Description:
  ------------
  Convert an ECEF coordinate to a NED coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef    - ECEF coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ned - NED coordinate in meters
*/
Vector3f ecef2ned(const Vector3f& ecef, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef_ref = lla2ecef(lla_ref, angle_unit);
    Matrix3f C        = ecef2ned_dcm(lla_ref, angle_unit);

    return C * (ecef - ecef_ref);
}

/*
  Description:
  ------------
  Convert a LLA coordinate to a NED coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla     - LLA coordinate (altitude in meters)
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ned - NED coordinate in meters
*/
Vector3f lla2ned(const Vector3f& lla, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef = lla2ecef(lla, angle_unit);
    Vector3f ned  = ecef2ned(ecef, lla_ref, angle_unit);

    return ned;
}

/*
  Description:
  ------------
  Convert a NED coordinate to an ECEF coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned      - NED coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f ecef - ECEF coordinate in meters
*/
Vector3f ned2ecef(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit)
{
    float lat_ref = lla_ref(0);
    float lon_ref = lla_ref(1);

    if (angle_unit == DEGREES)
    {
        lat_ref = deg2rad(lat_ref);
        lon_ref = deg2rad(lon_ref);
    }

    Matrix3f C(3, 3);

    C(0, 0) = -sin(lat_ref) * cos(lon_ref);
    C(0, 1) = -sin(lat_ref) * sin(lon_ref);
    C(0, 2) =  cos(lat_ref);

    C(1, 0) = -sin(lon_ref);
    C(1, 1) =  cos(lon_ref);
    C(1, 2) =  0;

    C(2, 0) = -cos(lat_ref) * cos(lon_ref);
    C(2, 1) = -cos(lat_ref) * sin(lon_ref);
    C(2, 2) = -sin(lat_ref);

    Vector3f ecef;
    ecef = C.transpose() * ned;

    return ecef;
}

/*
  Description:
  ------------
  Convert a NED coordinate to a LLA coordinate.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned     - NED coordinate in meters
  * const Vector3f& lla_ref - LLA coordinate of the NED frame origin (altitude in meters)
  * const bool& angle_unit  - Unit of the latitude and longitude angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f lla - LLA coordinate (altitude in meters)
*/
Vector3f ned2lla(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit)
{
    Vector3f ecef     = ned2ecef(ned, lla_ref, angle_unit);
    Vector3f ecef_ref = lla2ecef(lla_ref, angle_unit);
    ecef += ecef_ref;

    Vector3f lla = ecef2lla(ecef, angle_unit);

    return lla;
}

/*
  Description:
  ------------
  Create a 4x4 pose matrix that can be used to apply an affine transform of a
  3 dimensional vector/point from one coordinate frame to another. The two
  coordinate frames do not need to be colocated.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix that describes the rotation
                          between the two coordinate frames
  * const Vector3f& t   - Translation vector between the origins of the two
                          coordinate frames (unit of distance is arbitrary -
                          up to the user to decide)

  Returns:
  --------
  * Matrix4f poseMatrix - 4x4 pose matrix for affine coordinate frame transforms
*/
Matrix4f poseMat(const Matrix3f& dcm, const Vector3f& t)
{
    Matrix4f poseMatrix;
    poseMatrix << 0, 0, 0, 0,
                  0, 0, 0, 0,
                  0, 0, 0, 0,
                  0, 0, 0, 0;
    poseMatrix(seq(0, 2), seq(0, 2)) << dcm;
    poseMatrix(seq(0, 2), 3) << t;
    poseMatrix(3, 3) = 1;

    return poseMatrix;
}

/*
  Description:
  ------------
  Extract the DCM from a given pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation
  https://en.wikipedia.org/wiki/Rotation_matrix

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Matrix3f dcm - DCM extracted from pose matrix
*/
Matrix3f pose2dcm(const Matrix4f& poseMatrix)
{
    Matrix3f dcm;
    dcm << poseMatrix(seq(0, 2), seq(0, 2));

    return dcm;
}

/*
  Description:
  ------------
  Extract translation vector from a given pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Vector3f t - Translation vector extracted from pose matrix
*/
Vector3f pose2t(const Matrix4f& poseMatrix)
{
    Vector3f t;
    t << poseMatrix(seq(0, 2), 3);

    return t;
}

/*
  Description:
  ------------
  Create a reversed 4x4 pose matrix.

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms

  Returns:
  --------
  * Matrix4f poseMatrix - Reversed 4x4 pose matrix for affine coordinate frame
                          transforms
*/
Matrix4f reversePoseMat(const Matrix4f& poseMatrix)
{
    Matrix3f dcm = pose2dcm(poseMatrix);
    Vector3f t   = pose2t(poseMatrix);

    return poseMat(dcm.transpose(), dcm.transpose() * -t);
}

/*
  Description:
  ------------
  Compute the matrix derivative of the given pose matrix w.r.t the given variable.
  The possible variables include:
  * dRx - w.r.t. rotation around the x axis
  * dRy - w.r.t. rotation around the y axis
  * dRz - w.r.t. rotation around the z axis
  * dtx - w.r.t. translation around the x axis
  * dty - w.r.t. translation around the y axis
  * dtz - w.r.t. translation around the z axis

  This operation is often used when constructing Jacobian matrices for optimization
  problems (i.e. least squares/GN optimization)

  https://en.wikipedia.org/wiki/Affine_transformation
  https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant
  https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms
  * const int& derivType       - Variable to take the derivative w.r.t.

  Returns:
  --------
  * Matrix4f poseMatrix - Pose matrix derivative
*/
Matrix4f poseMatDeriv(const Matrix4f& poseMatrix, const int& derivType)
{
    Matrix4f derivPose;
    Matrix3f dcm = pose2dcm(poseMatrix);
    Vector3f vec;

    switch(derivType)
    {
        case dRx:
        {
            vec << 1, 0, 0;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dRy:
        {
            vec << 0, 1, 0;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dRz:
        {
            vec << 0, 0, 1;

            derivPose(seq(0, 2), seq(0, 2)) << skew(vec) * dcm;
            break;
        }

        case dtx:
        {
            vec << 1, 0, 0;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }

        case dty:
        {
            vec << 0, 1, 0;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }

        case dtz:
        {
            vec << 0, 0, 1;

            Matrix4f tempPose;
            tempPose(3, seq(0, 2)) << vec;

            derivPose << tempPose * poseMatrix;
            break;
        }
    }

    return derivPose;
}

/*
  Description:
  ------------
  Applies an affine transformation to vector/point x described by the given DCM and
  translation vector. This affine transformation converts the vector/point x from
  it's initial coordinate frame to another. A common use of this function would be
  to convert a 3D point in an airplane's sensor's body frame into the airplane's
  body frame (and vice versa).

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix3f& dcm - Direction cosine matrix that describes the rotation
                          between the two coordinate frames
  * const Vector3f& t   - Translation vector between the origins of the two
                          coordinate frames (unit of distance is arbitrary -
                          up to the user to decide)
  * const Vector3f& x   - The vector/point to be transformed

  Returns:
  --------
  * Vector3f new_x - The vector/point transformed to the new coordinate frame
*/
Vector3f transformPt(const Matrix3f& dcm, const Vector3f& t, const Vector3f& x)
{
    Vector4f x_1;
    x_1 << x;
    x_1(3) = 1;

    Vector4f new_x_1 = poseMat(dcm, t) * x_1;
    Vector3f new_x   = new_x_1(seq(0, 2));

    return new_x;
}

/*
  Description:
  ------------
  Applies an affine transformation to vector/point x described by the given pose
  matrix. This affine transformation converts the vector/point x from it's
  initial coordinate frame to another. A common use of this function would be
  to convert a 3D point in an airplane's sensor's body frame into the airplane's
  body frame (and vice versa).

  https://en.wikipedia.org/wiki/Affine_transformation

  Arguments:
  ----------
  * const Matrix4f& poseMatrix - 4x4 pose matrix for affine coordinate frame
                                 transforms
  * const Vector3f& x          - The vector/point to be transformed

  Returns:
  --------
  * Vector3f new_x - The vector/point transformed to the new coordinate frame
*/
Vector3f transformPt(const Matrix4f& poseMatrix, const Vector3f& x)
{
    Vector4f x_1;
    x_1 << x;
    x_1(3) = 1;

    Vector4f new_x_1 = poseMatrix * x_1;
    Vector3f new_x = new_x_1(seq(0, 2));

    return new_x;
}

/*
  Description:
  ------------
  Make a skew symmetric matrix from a 3 element vector. Skew
  symmetric matrices are often used to easily take cross products.

  https://en.wikipedia.org/wiki/Skew-symmetric_matrix

  Arguments:
  ----------
  * const Vector3f& w - 3 element vector

  Returns:
  --------
  * Matrix3f C - Skew symmetric matrix of vector w
*/
Matrix3f skew(const Vector3f& w)
{
    Matrix3f C;

    C << 0.0, -w(2),  w(1),
        w(2),   0.0, -w(0),
       -w(1),  w(0),   0.0;

    return C;
}

/*
  Description:
  ------------
  Calculate the bearing between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and bearing
                             angles (rad or degrees)

  Returns:
  --------
  * float bearing - The bearing between the two given LLA coordinates
*/
float bearingLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    float deltaLon = lon_2 - lon_1;

    float x = cos(lat_2) * sin(deltaLon);
    float y = cos(lat_1) * sin(lat_2) - sin(lat_1) * cos(lat_2) * cos(deltaLon);

    return fmod(rad2deg(atan2(x, y)) + 360, 360);
}

/*
  Description:
  ------------
  Calculate the bearing between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1  - First NED coordinate in meters
  * const Vector3f& ned_2  - Second NED coordinate in meters
  * const bool& angle_unit - Unit of the latitude, longitude, and bearing
                             angles (rad or degrees)

  Returns:
  --------
  * float bearing - The bearing between the two given LLA coordinates
*/
float bearingNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    float n_1 = ned_1(0);
    float e_1 = ned_1(1);

    float n_2 = ned_2(0);
    float e_2 = ned_2(1);

    float x = e_2 - e_1;
    float y = n_2 - n_1;

    return fmod(rad2deg(atan2(x, y)) + 360, 360);
}

/*
  Description:
  ------------
  Calculate the distance between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude and longitude angles (rad
                             or degrees)

  Returns:
  --------
  * float dist - The distance between the two given LLA coordinates in meters
*/
double distanceLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    double deltaLat = lat_2 - lat_1;
    double deltaLon = lon_2 - lon_1;

    double _a = (sin(deltaLat / 2) * sin(deltaLat / 2)) + cos(lat_1) * cos(lat_2) * (sin(deltaLon / 2)) * (sin(deltaLon / 2));

    float azimuth = bearingLla(lla_1, lla_2, angle_unit);
    float radius  = earthAzimRad(lla_1(0), azimuth, angle_unit);

    return 2 * radius * atan2(sqrt(_a), sqrt(1 - _a));
}

/*
  Description:
  ------------
  Calculate the total distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The total distance between the two given NED coordinates
*/
float distanceNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    Vector3f out = ned_2 - ned_1;
    return out.norm();
}

/*
  Description:
  ------------
  Calculate the horizontal distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The horizontal distance between the two given NED coordinates
*/
float distanceNedHoriz(const Vector3f& ned_1, const Vector3f& ned_2)
{
    Vector2f out;

    out << ned_2(0) - ned_1(0),
           ned_2(1) - ned_1(1);

    return out.norm();
}

/*
  Description:
  ------------
  Calculate the vertical distance between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1 - First NED coordinate in meters
  * const Vector3f& ned_2 - Second NED coordinate in meters

  Returns:
  --------
  * float dist - The vertical distance between the two given NED coordinates
*/
float distanceNedVert(const Vector3f& ned_1, const Vector3f& ned_2)
{
    return ned_2(2) - ned_1(2);
}

/*
  Description:
  ------------
  Calculate the total distance between two ECEF coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system
  https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ecef_1 - First ECEF coordinate in meters
  * const Vector3f& ecef_2 - Second ECEF coordinate in meters

  Returns:
  --------
  * float dist - The total distance between the two given ECEF coordinates
*/
float distanceEcef(const Vector3f& ecef_1, const Vector3f& ecef_2)
{
    Vector3f out = ecef_2 - ecef_1;
    return out.norm();
}

/*
  Description:
  ------------
  Calculate the elevation angle between two LLA coordinates.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla_1  - First LLA coordinate (altitude in meters)
  * const Vector3f& lla_2  - Second LLA coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and elevation
                             angles (rad or degrees)

  Returns:
  --------
  * float elevation - The elevation angle between the two given LLA coordinates
*/
float elevationLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit)
{
    float lat_1 = lla_1(0);
    float lon_1 = lla_1(1);
    float alt_1 = lla_1(2);

    float lat_2 = lla_2(0);
    float lon_2 = lla_2(1);
    float alt_2 = lla_2(2);

    if (angle_unit == DEGREES)
    {
        lat_1 = deg2rad(lat_1);
        lon_1 = deg2rad(lon_1);

        lat_2 = deg2rad(lat_2);
        lon_2 = deg2rad(lon_2);
    }

    float dist   = distanceLla(lla_1, lla_2, angle_unit);
    float height = alt_2 - alt_1;

    return rad2deg(atan2(height, dist));
}

/*
  Description:
  ------------
  Calculate the elevation angle between two NED coordinates.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned_1  - First NED coordinate (altitude in meters)
  * const Vector3f& ned_2  - Second NED coordinate (altitude in meters)
  * const bool& angle_unit - Unit of the latitude, longitude, and elevation
                             angles (rad or degrees)

  Returns:
  --------
  * float elevation - The elevation angle between the two given NED coordinates
*/
float elevationNed(const Vector3f& ned_1, const Vector3f& ned_2)
{
    float d_1 = -ned_1(2);
    float d_2 = -ned_2(2);

    float dist   = distanceNed(ned_1, ned_2);
    float height = d_2 - d_1;

    return rad2deg(atan2(height, dist));
}

/*
  Description:
  ------------
  Calculate a LLA coordinate based on a given LLA coordinate, distance,
  azimuth, and elevation angle.

  http://www.movable-type.co.uk/scripts/latlong.html
  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& lla     - LLA coordinate (altitude in meters)
  * const float& dist       - "As the crow flies" distance between the two
                              LLA coordinates in meters
  * const float& _azimuth   - Azimuth angle between the two LLA coordinates
  * const float& _elevation - Elevation angle between the two LLA coordinates
  * const bool& angle_unit  - Unit of the latitude, longitude, azimuth, and
                              elevation angles (rad or degrees)

  Returns:
  --------
  * Vector3f& out - New LLA coordinate (altitude in meters)
*/
Vector3f LDAE2lla(const Vector3f& lla, const float& dist, const float& _azimuth, const float& _elevation, const bool& angle_unit)
{
    float lat = lla(0);
    float lon = lla(1);
    float alt = lla(2);

    float azimuth   = _azimuth;
    float elevation = _elevation;

    if (angle_unit == DEGREES)
    {
        lat = deg2rad(lat);
        lon = deg2rad(lon);

        azimuth   = deg2rad(azimuth);
        elevation = deg2rad(elevation);
    }

    float radius   = earthAzimRad(lla(0), _azimuth, angle_unit);
    float adj_dist = dist / radius;

    float lat_2 = asin(sin(lat) * cos(adj_dist) + cos(lat) * sin(adj_dist) * cos(azimuth));
    float lon_2 = lon + atan2(sin(azimuth) * sin(adj_dist) * cos(lat), cos(adj_dist) - sin(lat) * sin(lat_2));

    Vector3f out;

    out << rad2deg(lat_2),
           rad2deg(lon_2),
           alt + (dist * tan(elevation));

    return out;
}

/*
  Description:
  ------------
  Calculate a NED coordinate based on a given NED coordinate, distance,
  azimuth, and elevation angle.

  https://en.wikipedia.org/wiki/Geographic_coordinate_system

  Arguments:
  ----------
  * const Vector3f& ned     - NED coordinate in meters
  * const float& dist       - Horizontal distance between the two
                              NED coordinates in meters
  * const float& _azimuth   - Azimuth angle between the two NED coordinates
  * const float& _elevation - Elevation angle between the two NED coordinates
  * const bool& angle_unit  - Unit of the azimuth and elevation angles (rad
                              or degrees)

  Returns:
  --------
  * Vector3f& out - New NED coordinate in meters
*/
Vector3f NDAE2ned(const Vector3f& ned, const float& dist, const float& _azimuth, const float& _elevation, const bool& angle_unit)
{
    float n = ned(0);
    float e = ned(1);
    float d = ned(2);

    float azimuth   = _azimuth;
    float elevation = _elevation;

    if (angle_unit == DEGREES)
    {
        azimuth   = deg2rad(azimuth);
        elevation = deg2rad(elevation);
    }

    Vector3f out;

    out << n + dist * cos(azimuth),
           e + dist * sin(azimuth),
           d + (dist * tan(elevation));

    return out;
}


#include <iostream>

#include <Eigen/Dense>
#include <Eigen/QR>

/* @brief QR decomposition from Cholesky decomposition
 * @param[in] A An $m \times n$ matrix
 * @param[out] R The upper triangular matrix from the QR decomposition of $A$
 * @param[out] Q The orthogonal matrix from the QR decomposition of $A$
 */
void CholeskyQR(const Eigen::MatrixXd &A, Eigen::MatrixXd &R, Eigen::MatrixXd &Q) {
	Eigen::MatrixXd L = (A.transpose() * A).llt().matrixL();
	Q = L.triangularView<Eigen::Lower>().solve(A.transpose()).transpose();
	R = L.transpose();
}

/* @brief Direct QR decomposition
 * @param[in] A An $m \times n$ matrix
 * @param[out] R The upper triangular matrix from the QR decomposition of $A$
 * @param[out] Q The orthogonal matrix from the QR decomposition of $A$
 */
void DirectQR(const Eigen::MatrixXd &A, Eigen::MatrixXd &R, Eigen::MatrixXd &Q) {

	int m = A.rows();
	int n = A.cols();

	Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
	Q = qr.householderQ() * Eigen::MatrixXd::Identity(m, n);
	R = (Q.transpose() * A);
}

int main() {
	int m = 3;
	int n = 2;

	Eigen::MatrixXd A(m,n);
	double epsilon = 1.e-8;
	//A << 3, 5, 1, 9, 7, 1;
	A << 1, 1, epsilon, 0, 0, epsilon;
	std::cout << "A =" << std::endl << A << std::endl;

	Eigen::MatrixXd R, Q;

	CholeskyQR(A, R, Q);
	std::cout << "CholeskyQR: ===========" << std::endl;
	std::cout << "R =" << std::endl << R << std::endl;
	std::cout << "Q =" << std::endl << Q << std::endl;

	DirectQR(A, R, Q);
	std::cout << "DirectQR: =============" << std::endl;
	std::cout << "R =" << std::endl << R << std::endl;
	std::cout << "Q =" << std::endl << Q << std::endl;

	return 0;
}


/**
 * @file slae.hpp:  System of Linear Algebraic Equations utilities.
 *
 * This module contains functions for solving SLAE,
 * particularly for Cube Product Strassen-like algorithm finding.
 *
 * @author Eugene Petkevich
 * @version pre-alpha
 */

#ifndef SLAE_HPP_INCLUDED
#define SLAE_HPP_INCLUDED

#include <vector>
#include <map>

#include <boost/dynamic_bitset.hpp>

#include "utils.hpp"

using namespace std;

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

/// All the following functions solve SLAE using different methods.

template <size_t N>
bool gauss_solve(vector<mm_bitset<N>> a, mm_bitset<N> b);

class Gauss_Presolve_Data;
template <size_t N>
void gauss_presolve(vector<mm_bitset<N>> a, Gauss_Presolve_Data& p);
template <size_t N>
bool gauss_solve(const Gauss_Presolve_Data& p, mm_bitset<N> b);

template <size_t N>
bool binary_solve(const vector<mm_bitset<N>>& a, const mm_bitset<N>& b);

template <size_t N>
bool binary_solve_recursive(const vector<mm_bitset<N>>& a, const mm_bitset<N>& b);

template <size_t N>
bool gauss_solve_randomized(vector<mm_vector_with_properties<N>> a, mm_vector_with_properties<N> b);

template <size_t N>
class Gauss_WP_Presolve_Data;
template <size_t N>
bool gauss_wp_presolve(const vector<mm_vector_with_properties<N>>& a, Gauss_WP_Presolve_Data<N>& p);
template <size_t N>
bool gauss_wp_solve(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b);
template <size_t N>
bool gauss_wp_solve_cached(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b);
template <size_t N>
bool gauss_wp_solve_fast_cached(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b);

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

/// Comparison functions for bitvectors.

template <size_t N>
bool compare_lexical (const mm_bitset<N>& a, const mm_bitset<N>& b);

template <size_t N>
bool compare_count (const mm_bitset<N>& a, const mm_bitset<N>& b);

template <size_t N>
bool compare_combined (const mm_bitset<N>& a, const mm_bitset<N>& b);

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

/**
 * Sovle SLAE by Gaussian elimination.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
gauss_solve(vector<mm_bitset<N>> a, mm_bitset<N> b)
{
    int r = 0;
    for (int i = 0; i < a.size(); ++i) { // for all columns
        // first we exchange rows if needed, so that a[i][i] = 1
        if (!a[i][r]) {
            int k = r+1;
            while ((k < N) && (!a[i][k])) {
                ++k;
            }
            if (k >= N) {
                continue;
            } else {
                bool tmp = b[k];
                b[k] = b[r];
                b[r] = tmp;
                for (int l = i; l < a.size(); ++l) {
                    tmp = a[l][k];
                    a[l][k] = a[l][r];
                    a[l][r] = tmp;
                }
            }
        }
        // second, we make all coefficients under a[i][i] equal to 0
        for (int j = r+1; j < b.size(); ++j) { // for all rows
            if (a[i][j]) {
                for (int k = i; k < a.size(); ++k) {
                    a[k][j] = (a[k][j] != a[k][r]);
                }
                b[j] = (b[j] != b[r]);
            }
        }
        ++r;
    }
    // we check all i from 7 to 15 for zero coefficients
    for (int i = r; i < b.size(); ++i) {
        if (b[i])
            return false;
    }
    return true;
}

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

/**
 * Data for Gaussian elimination presolve.
 */
class Gauss_Presolve_Data {
public:
    size_t n; /// Size of the matrix.
    int r;
    vector<int> rows_s;
    vector<int> rows_d;
    vector<bool> rows_o;
};

/**
 * Presolve SLAE by Gaussian elimination.
 *
 * Collects Gaussian elimination operations that are needed
 * to selve a SLAE with a specified matrix.
 *
 * @param a: the coefficient matrix;
 * @param p: varuable to store presolve data;
 */
template <size_t N>
void
gauss_presolve(vector<mm_bitset<N>> a, Gauss_Presolve_Data & p)
{
    p.n = a.size();
    p.rows_s.clear();
    p.rows_d.clear();
    p.rows_o.clear();
    int r = 0;
    for (int i = 0; i < a.size(); ++i) { // for all columns
        // first we exchange rows if needed, so that a[i][i] = 1
        if (!a[i][r]) {
            int k = r+1;
            while ((k < N) && (!a[i][k])) {
                ++k;
            }
            if (k >= N) {
                continue;
            } else {
                bool tmp;
                p.rows_s.push_back(r);
                p.rows_d.push_back(k);
                p.rows_o.push_back(true);
                for (int l = i; l < a.size(); ++l) {
                    tmp = a[l][k];
                    a[l][k] = a[l][r];
                    a[l][r] = tmp;
                }
            }
        }
        // second, we make all coefficients under a[i][i] equal to 0
        for (int j = r+1; j < N; ++j) { // for all rows
            if (a[i][j]) {
                for (int k = i; k < a.size(); ++k) {
                    a[k][j] = (a[k][j] != a[k][r]);
                }
                p.rows_s.push_back(r);
                p.rows_d.push_back(j);
                p.rows_o.push_back(false);
            }
        }
        ++r;
    }
    p.r = r;
}

/**
 * Sovle SLAE by Gaussian elimination using precomputed data.
 *
 * @param p: the presolve data;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
gauss_solve(const Gauss_Presolve_Data& p, mm_bitset<N> b)
{
    for (int i = 0; i < p.rows_o.size(); ++i) {
        if (p.rows_o[i]) {
            bool tmp = b[p.rows_s[i]];
            b[p.rows_s[i]] = b[p.rows_d[i]];
            b[p.rows_d[i]] = tmp;
        } else {
            b[p.rows_d[i]] = (b[p.rows_d[i]] != b[p.rows_s[i]]);
        }
    }
    for (int i = p.r; i < b.size(); ++i) {
        if (b[i])
            return false;
    }
    return true;
}

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

/**
 * Solve SLAE by trying all solutions.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
binary_solve(const vector<mm_bitset<N>>& a, const mm_bitset<N>& b)
{
    uint_least64_t counter;
    uint_least64_t limit = power(2,a.size());
    for (counter = 1; counter < limit; ++counter) {
        boost::dynamic_bitset<> x(a.size(),counter);
        mm_bitset<N> r(0);
        bool is_good = true;
        // we multiply a by x
        for (int i = 0; i < b.size(); ++i) {
            for (int j = 0; j < a.size(); ++j) {
                if (x[j]) {
                    r[i] = (r[i] != a[j][i]);
                }
            }
            if (r[i] != b[i]) {
                is_good = false;
                break;
            }
        }
        if (is_good)
            return true;
    }
    return false;
}

/**
 * Solve SLAE by trying all solutions.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
binary_solve(const vector<mm_vector_with_properties<N>>& a, const mm_vector_with_properties<N>& b)
{
    vector<mm_bitset<N>> sa;
    for (auto& vp: a) {
        sa.push_back(vp.v);
    }
    return binary_solve(sa, b.v);
}

/**
 * Solve SLAE by trying all solutions and return the result.
 *
 * The SLAE should have a solution.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 *
 * @return solution (if SLAE has no solution, return value could be anything).
 */
template <size_t N>
boost::dynamic_bitset<>
binary_solve_result(const vector<mm_bitset<N>>& a, const mm_bitset<N>& b)
{
    uint_least64_t counter;
    uint_least64_t limit = power(2,a.size());
    for (counter = 1; counter < limit; ++counter) {
        boost::dynamic_bitset<> x(a.size(),counter);
        mm_bitset<N> r(0);
        bool is_good = true;
        // we multiply a by x
        for (int i = 0; i < b.size(); ++i) {
            for (int j = 0; j < a.size(); ++j) {
                if (x[j]) {
                    r[i] = (r[i] != a[j][i]);
                }
            }
            if (r[i] != b[i]) {
                is_good = false;
                break;
            }
        }
        if (is_good)
            return x;
    }
    return boost::dynamic_bitset<>();
}

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

/**
 * Subroutine for solving SLAE via backtracking.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 * @param depth: backtracking tree current depth;
 * @param current: current value of guessed solution.
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
binary_solve_explore(const vector<mm_bitset<N>>* a, const mm_bitset<N>* b, int depth, const mm_bitset<N>* current)
{
    if (depth == -1) {
        for (int k = 0; k < N; ++k) {
            if (current->test(k) != b->test(k))
                return false;
        }
        return true;
    } else {
        bool result = binary_solve_explore(a, b, depth-1, current);
        if (result)
            return true;
        mm_bitset<N>* current_add = new mm_bitset<N>();
        for (int k = 0; k < N; ++k) {
            (*current_add)[k] = current->test(k) != (*a)[depth][k];
        }
        result = binary_solve_explore(a, b, depth-1, current_add);
        delete current_add;
        return result;
    }
}

/**
 * Solve SLAE by trying solutions via backtracking.
 *
 * @param a: the coefficient matrix;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
binary_solve_recursive(const vector<mm_bitset<N>>& a, const mm_bitset<N>& b)
{
    uint_least64_t depth = a.size()-1;
    mm_bitset<N>* current = new mm_bitset<N>();
    current->reset();
    bool result = binary_solve_explore(&a, &b, depth, current);
    delete current;
    return result;
}

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

/**
 * TODO
 */
template <size_t N>
bool
gauss_solve_randomized(vector<mm_vector_with_properties<N>> a, mm_vector_with_properties<N> b)
{
    return true;
}

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

/**
 * Compare function to sort bitsets lexicographically.
 *
 * @param N: bitset size;
 */
template <size_t N>
bool
compare_lexical (const mm_bitset<N>& a, const mm_bitset<N>& b)
{
    for (int i = 0; i < N; ++i) {
        if (a[i] < b[i]) {
            return true;
        } else if (a[i] > b[i]) {
            return false;
        }
    }
    return true;
}

/**
 * Compare function to sort bitsets by number of 1-bits.
 *
 * @param N: bitset size;
 */
template <size_t N>
bool
compare_count (const mm_bitset<N>& a, const mm_bitset<N>& b)
{
    return (a.count() < b.count());
}

/**
 * Compare function to sort bitsets by number of bits,
 * and if it is the same, lexicographically.
 *
 * @param N: bitset size;
 */
template <size_t N>
bool
compare_combined (const mm_bitset<N>& a, const mm_bitset<N>& b)
{
    if (a.count() < b.count()) {
        return true;
    } else if (a.count() > b.count()) {
        return false;
    } else {
        for (int i = 0; i < N; ++i) {
            if (a[i] > b[i]) {
                return true;
            } else if (a[i] < b[i]) {
                return false;
            }
        }
        return true;
    }
}

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

/**
 * Presolve data for Gaussian elimination variant for solving SLAE.
 *
 * @param N: size of result and variable vectors.
 */
template <size_t N>
class Gauss_WP_Presolve_Data {
public:
    vector<mm_vector_with_properties<N>> am; /// Transformed Matrix.
    vector<int> imi;
    map<int, mm_vector_with_properties<N>> cache; /// cache with sums of vectors
    mm_vector_with_properties<N>* memo_array; /// cache with sums of vectors
    bool* memo_status; /// cache status

    Gauss_WP_Presolve_Data(int f_count);
    ~Gauss_WP_Presolve_Data();
};

/**
 * Constructor.
 *
 * @param f_count:  number of vectors in matrix.
 */
template <size_t N>
Gauss_WP_Presolve_Data<N>::
Gauss_WP_Presolve_Data(int f_count)
{
    memo_array = new mm_vector_with_properties<N>[power(2, f_count)];
    memo_status = new bool[power(2, f_count)];
}

/**
 * Destructor.
 */
template <size_t N>
Gauss_WP_Presolve_Data<N>::
~Gauss_WP_Presolve_Data()
{
    delete [] memo_array;
    delete [] memo_status;
}

/**
 * Collect operoationsdata for Gaussian elimination variant for solving SLAE.
 *
 * @param N: size of result and variable vectors.
 *
 * @param a: the coefficient matrix;
 * @param p: variable to store presolve data.
 */
template <size_t N>
bool
gauss_wp_presolve(const vector<mm_vector_with_properties<N>>& a, Gauss_WP_Presolve_Data<N>& p)
{
    p.am = a;
    p.imi.clear();
    p.cache.clear();
    for (typename vector<mm_vector_with_properties<N>>::iterator i = p.am.begin(); i != p.am.end();) { // for all columns
        int k = 0;
        while (((k < N) && (!(*i).v[k])) || (find(p.imi.begin(), p.imi.end(), k) != p.imi.end())) {
            ++k;
        }
        if (k == N) { // linearly dependent
            i = p.am.erase(i);
            return false;
        } else { // make 0's all items in a row k except for i'th column
            for (typename vector<mm_vector_with_properties<N>>::iterator j = p.am.begin(); j != i; ++j) {
                if ((*j).v[k]) {
                    (*j).v ^= (*i).v;
                    (*j).r ^= (*i).r;
                }
            }
            for (typename vector<mm_vector_with_properties<N>>::iterator j = i+1; j != p.am.end(); ++j) {
                if ((*j).v[k]) {
                    (*j).v ^= (*i).v;
                    (*j).r ^= (*i).r;
                }
            }
            p.imi.push_back(k);
            ++i;
        }
    }
    return true;
}

/**
 * Sovle SLAE by Gaussian elimination using precomputed data.
 *
 * @param N: size of result and variable vectors.
 *
 * @param p: the presolve data;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
gauss_wp_solve(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b)
{
    mm_vector_with_properties<N> c;
    typename vector<mm_vector_with_properties<N>>::const_iterator j = p.am.begin();
    for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
        if (b.v[*i]) {
            c.v ^= (*j).v;
        }
        ++j;
    }
    return (c.v == b.v);
}

/**
 * Sovle SLAE by Gaussian elimination using precomputed data using memoization.
 *
 * @param N: size of result and variable vectors.
 *
 * @param p: the presolve data;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
gauss_wp_solve_cached(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b)
{
    /*
    c.r.reset();
    j = p.am.begin();
    for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
        if (b.v[*i]) {
            c.r ^= (*j).r;
        }
        ++j;
    }
    */
    // first check number of ones
    /*
    if (c.r.count() != b.r_count) {
        return false;
    }
    */
    // then every bit
    /*
    if (c.r != b.r)
        return false;
    */
    int matrix_multiplier = 0;
    for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
        matrix_multiplier <<= 1;
        matrix_multiplier += b.v[*i];
    }
    typename map<int, mm_vector_with_properties<N>>::const_iterator memo_result = p.cache.find(matrix_multiplier);
    if (memo_result != p.cache.end()) {
        return (memo_result->second.v == b.v);
    } else {
        mm_vector_with_properties<N> c;
        typename vector<mm_vector_with_properties<N>>::const_iterator j = p.am.begin();
        for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
            if (b.v[*i]) {
                c.v ^= (*j).v;
            }
            ++j;
        }
        p.cache[matrix_multiplier] = c;
        return (c.v == b.v);
    }
    // first check number of ones
    /*
    if (c.v.count() != b.v_count) {
        return false;
    }
    */
}

/**
 * Sovle SLAE by Gaussian elimination using precomputed data using memoization.
 *
 * @param N: size of result and variable vectors.
 *
 * @param p: the presolve data;
 * @param b: the resulting vector;
 *
 * @return if there is a solution.
 */
template <size_t N>
bool
gauss_wp_solve_fast_cached(Gauss_WP_Presolve_Data<N>& p, const mm_vector_with_properties<N>& b)
{
    int matrix_multiplier = 0;
    for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
        matrix_multiplier <<= 1;
        matrix_multiplier += b.v[*i];
    }
    if (p.memo_status[matrix_multiplier]) {
        return (p.memo_array[matrix_multiplier].v == b.v);
    } else {
        mm_vector_with_properties<N> c;
        typename vector<mm_vector_with_properties<N>>::const_iterator j = p.am.begin();
        for (vector<int>::const_iterator i = p.imi.begin(); i != p.imi.end(); ++i) {
            if (b.v[*i]) {
                c.v ^= (*j).v;
            }
            ++j;
        }
        p.memo_array[matrix_multiplier] = c;
        p.memo_status[matrix_multiplier] = true;
        return (c.v == b.v);
    }
}

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

#endif // SLAE_HPP_INCLUDED


// NormalFunction.cpp
//
// Some exact formulae.
//
// (C) Datasim Education BV 2011
//

#include <cmath>
#include <iostream>
using namespace std;
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions.hpp> // For non-member functions of distributions
using namespace boost::math;

using namespace boost::math;

double N(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return cdf(myNormal, x);

}

double n(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return pdf(myNormal, x);

}

int main()
{

	double x = 2.0;
	cout << "pdf: " << n(x) << ", cdf: " << N(x) << endl;

	return 0;
}


#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <math.h>
#include <vector>
#include <algorithm>

#include <time.h>

#include <cryptoTools/Network/IOService.h>

#include <cryptoTools/Common/CLP.h>
#include <Eigen/Dense>

#include "aby3-ML/Regression.h"
#include "aby3-ML/LinearModelGen.h"

#include "aby3-ML/aby3ML.h"
#include "aby3-ML/PlainML.h"

using namespace std;
using namespace Eigen;
using namespace oc;
namespace aby3
{

	int logistic_plain_main(CLP& cmd)
	{
		auto N = cmd.getOr<int>("N", 10000);
		auto D = cmd.getOr<int>("D", 1000);
		auto B = cmd.getOr<int>("B", 128);
		auto IT = cmd.getOr<int>("I", 10000);
		auto testN = cmd.getOr<int>("testN", 1000);

		PRNG prng(toBlock(1));
		LogisticModelGen gen;

		eMatrix<double> model(D, 1);
		for (u64 i = 0; i < D; ++i)
		{
			model(i, 0) = prng.get<int>() % 10;
			std::cout << model(i, 0) << " ";
		}

		std::cout << std::endl;
		gen.setModel(model);

		eMatrix<double> train_data(N, D), train_label(N, 1);
		eMatrix<double> test_data(testN, D), test_label(testN, 1);
		gen.sample(train_data, train_label);
		gen.sample(test_data, test_label);

		std::cout << "training __" << std::endl;

		RegressionParam params;
		params.mBatchSize = B;
		params.mIterations = IT;
		params.mLearningRate = 1.0 / (1 << 3);
		PlainML engine;

		eMatrix<double> W2(D, 1);
		W2.setZero();
		//engine.mPrint = cmd.isSet("print");

		SGD_Logistic(params, engine, train_data, train_label, W2, &test_data, &test_label);

		for (u64 i = 0; i < D; ++i)
		{
			std::cout << i << " " << gen.mModel(i, 0) << " " << W2(i, 0) << std::endl;
		}

		return 0;
	}

	int logistic_main_3pc_sh(int N, int dim, int B, int IT, int testN, int pIdx, bool print, CLP& cmd, Session& chlPrev, Session& chlNext)
	{

		PRNG prng(toBlock(1));
		LogisticModelGen gen;

		eMatrix<double> model(dim, 1);
		for (u64 i = 0; i < std::min(dim, 10); ++i)
		{
			model(i, 0) = prng.get<int>() % 10;
		}
		gen.setModel(model);

		eMatrix<double> val_train_data(N, dim), val_train_label(N, 1), val_W2(dim, 1);
		eMatrix<double> val_test_data(testN, dim), val_test_label(testN, 1);
		gen.sample(val_train_data, val_train_label);
		gen.sample(val_test_data, val_test_label);
		val_W2.setZero();

		RegressionParam params;
		params.mBatchSize = B;
		params.mIterations = IT;
		params.mLearningRate = 1.0 / (1 << 3);

		const Decimal D = D16;
		aby3ML p;

		p.init(pIdx, chlPrev, chlNext, toBlock(pIdx));

		//p.mPrint = cmd.isSet("print");

		sf64Matrix<D> train_data, train_label, W2, test_data, test_label;

		if (pIdx == 0)
		{
			train_data = p.localInput<D>(val_train_data);
			train_label = p.localInput<D>(val_train_label);
			W2 = p.localInput<D>(val_W2);
			test_data = p.localInput<D>(val_test_data);
			test_label = p.localInput<D>(val_test_label);
		}
		else
		{
			train_data = p.remoteInput<D>(0);
			train_label = p.remoteInput<D>(0);
			W2 = p.remoteInput<D>(0);
			test_data = p.remoteInput<D>(0);
			test_label = p.remoteInput<D>(0);
		}

		p.mPreproNext.resetStats();
		p.mPreproPrev.resetStats();

		auto preStart = std::chrono::system_clock::now();

		p.preprocess((B + dim) * IT, D);

		double preBytes = p.mPreproNext.getTotalDataSent() + p.mPreproPrev.getTotalDataSent();

		p.mNext.resetStats();
		p.mPrev.resetStats();

		auto start = std::chrono::system_clock::now();

		if (cmd.isSet("noOnline") == false)
			SGD_Logistic(params, p, train_data, train_label, W2);
		//val_W2 = p.reveal(W2);

		auto end = std::chrono::system_clock::now();

		//engine.sync();
		auto now = std::chrono::system_clock::now();
		auto preSeconds = std::chrono::duration_cast<std::chrono::milliseconds>(start - preStart).count() / 1000.0;
		auto seconds = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;

		double bytes = p.mNext.getTotalDataSent() + p.mPrev.getTotalDataSent();

		if (print)
		{
			ostreamLock ooo(std::cout);
			ooo << "N: " << N << " D:" << dim << " B:" << B << " IT:" << IT << " => "
				<< (double(IT) / seconds) << "  iters/s  " << (bytes * 8 / 1024 / 2024) / seconds << " Mbps"
				<< " offline: " << (double(IT) / preSeconds) << "  iters/s  " << (preBytes * 8 / 1024 / 2024) / preSeconds << " Mbps" << std::endl;
		}

		auto w2Val = p.reveal(W2);

		if (print)
		{

			for (u64 i = 0; i < dim; ++i)
			{
				std::cout << i << " " << gen.mModel(i, 0) << " " << w2Val(i, 0) << std::endl;
			}
		}

		return 0;
	}

	int logistic_main_3pc_sh(oc::CLP & cmd)
	{

		auto N = cmd.getManyOr<int>("N", { 10000 });
		auto D = cmd.getManyOr<int>("D", { 1000 });
		auto B = cmd.getManyOr<int>("B", { 128 });
		auto IT = cmd.getManyOr<int>("I", { 10000 });
		auto testN = cmd.getOr("testN", 1000);

		IOService ios(cmd.isSet("p") ? 2 : 6);

		std::vector<std::thread> thrds;
		for (u64 i = 0; i < 3; ++i)
		{
			if (cmd.isSet("p") == false || cmd.get<int>("p") == i)
			{
				thrds.emplace_back(std::thread([i, N, D, B, IT, testN, &cmd, &ios]() {

					auto next = (i + 1) % 3;
					auto prev = (i + 2) % 3;
					auto cNameNext = std::to_string(std::min(i, next)) + std::to_string(std::max(i, next));
					auto cNamePrev = std::to_string(std::min(i, prev)) + std::to_string(std::max(i, prev));

					auto modeNext = i < next ? SessionMode::Server : SessionMode::Client;
					auto modePrev = i < prev ? SessionMode::Server : SessionMode::Client;

					auto portNext = 1212 + std::min(i, next);
					auto portPrev = 1212 + std::min(i, prev);

					Session epNext(ios, "127.0.0.1", portNext, modeNext, cNameNext);
					Session epPrev(ios, "127.0.0.1", portPrev, modePrev, cNamePrev);

					std::cout << "party " << i << " next " << portNext << " mode=server?:" << (modeNext == SessionMode::Server) << " name " << cNameNext << std::endl;
					std::cout << "party " << i << " prev " << portPrev << " mode=server?:" << (modePrev == SessionMode::Server) << " name " << cNamePrev << std::endl;
					auto chlNext = epNext.addChannel();
					auto chlPrev = epPrev.addChannel();

					chlNext.waitForConnection();
					chlPrev.waitForConnection();

					chlNext.send(i);
					chlPrev.send(i);
					u64 prevAct, nextAct;
					chlNext.recv(nextAct);
					chlPrev.recv(prevAct);

					if (next != nextAct)
						std::cout << " bad next party idx, act: " << nextAct << " exp: " << next << std::endl;
					if (prev != prevAct)
						std::cout << " bad prev party idx, act: " << prevAct << " exp: " << prev << std::endl;

					ostreamLock(std::cout) << "party " << i << " start" << std::endl;

					auto print = cmd.isSet("p") || i == 0;

					for (auto n : N)
					{
						for (auto d : D)
						{
							for (auto b : B)
							{
								for (auto it : IT)
								{
									logistic_main_3pc_sh(n, d, b, it, testN, i, print, cmd, epPrev, epNext);
								}
							}
						}
					}
					}));
			}
		}

		for (auto& t : thrds)
			t.join();

		return 0;
	}

}


/**
 * @file exponentialintegrator.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "exponentialintegrator.h"

#include <Eigen/Core>
#include <cassert>
#include <unsupported/Eigen/MatrixFunctions>

namespace ExponentialIntegrator {

//! \brief Function $\phi$ used in the Exponential Euler
//! single step method for an autonomous ODE.
Eigen::MatrixXd phim(const Eigen::MatrixXd &Z) {
  int n = Z.cols();
  assert(n == Z.rows() && "Matrix must be square.");
  Eigen::MatrixXd C(2 * n, 2 * n);
  C << Z, Eigen::MatrixXd::Identity(n, n), Eigen::MatrixXd::Zero(n, 2 * n);
  return C.exp().block(0, n, n, n);
}

}  // namespace ExponentialIntegrator


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

template <class matrix>
matrix gramschmidt(const matrix &A){
	unsigned n=A.cols();
	matrix Q=A;
	Q.col(0).normalize();

	for(int i=1;i<n;i++){
		Q.col(i)-= Q.leftCols(i)*(Q.leftCols(i).transpose() * A.col(i));
	//assert(Q.col(i).norm()<10e-16*A.col(i).norm() && "can't compute because A has lin dependant rows");
	Q.col(i).normalize();
	}
	return Q;

};

int main() {
	int n=5;
	Eigen::MatrixXd A;
	Eigen::MatrixXd Q;
	A = Eigen::MatrixXd::Random(n,n);
	Q=gramschmidt(A);
	std::cout << A<< std::endl;
	Q*=Q.transpose();
	/*for(int i=0;i<n;i++){
		for(int j=0;j<n;j++){
			if (abs(Q(i,j))<10e-10)Q(i,j)=0;}}

*/
	std::cout << Q << std::endl;
	return 0;
}


#include <iostream>
#include <NTL/ZZ.h>
#include <chrono>
#include <vector>
#include <fstream>

/**
 * Compute fibonacci number for 2**i mod m. Using double and add method.
 */
NTL::ZZ fibonacci_mod(int i, const NTL::ZZ& m) {
    NTL::ZZ x;
    x = 0;
    NTL::ZZ y;
    y = 1;

    NTL::ZZ tmp;

    // add for the first 1
    tmp = x;
    x = y;
    y = (tmp + y) % m;

    while (true) {
        // double for each 0
        tmp = x;
        x = x*(2*y-x) % m;
        y = (tmp * tmp + y * y) % m;

        if (i == 1) {
            return x;
        }
        i -= 1;
    }
}

int main(int argc, char **argv) {
    // program takes two ints as arguments for the search bounds
    if (argc != 3)
        return 1;

    // get search bounds from arguments
    uint lower_bound = std::stoll(argv[1]);
    uint upper_bound = std::stoll(argv[2]);

    if (lower_bound < 2) {
        lower_bound = 2;
    }

    std::cout << "lower bound: " << lower_bound << std::endl;
    std::cout << "upper bound: " << upper_bound << std::endl;

    // start clock
    auto start = std::chrono::steady_clock::now();

    // vector of resulting pseudoprimes
    std::vector<NTL::ZZ> results;

    NTL::ZZ two;
    two = 2;

    for (uint i = lower_bound; i <= upper_bound; i++) {
        NTL::ZZ n; // n = 2**i-1
        n = 2;
        n = n << i-1;
        n -= 1;

        NTL::ZZ tmp;

        // fermat test
        tmp = NTL::PowerMod(two, n - 1, n);
        if (tmp != 1) {
            continue;
        }

        // lucas test
        tmp = fibonacci_mod(i, n);
        if (tmp != 0) {
            continue;
        }

        std::cout << "found: " << i << std::endl;
        results.push_back(n);
    }

    // stop clock
    auto stop = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start).count();
    std::cout << "duration (seconds): " << duration << std::endl;

    // write results to file
    std::ofstream f ("results.txt", std::ios::out | std::ios::trunc);
    f << "lower bound: " << lower_bound << std::endl;
    f << "upper bound: " << upper_bound << std::endl;
    for (const NTL::ZZ& p: results) {
        f << "found: " << p << std::endl;
    }
    f << "duration (seconds): " << duration << std::endl;
    f.close();

    return 0;
}


#include <Eigen/Core>
#include <iostream>
#include <LBFGSpp/LBFGS.h>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
public:
    Rosenbrock(int n_) : n(n_) {}
    double operator()(const VectorXd& x, VectorXd& grad)
    {
        double fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            double t1 = 1.0 - x[i];
            double t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
            fx += t1 * t1 + t2 * t2;
        }
        assert( ! std::isnan(fx) );
        return fx;
    }
};

int main()
{
    LBFGSParam<double> param;
    LBFGSSolver<double, LineSearchBracketing> solver(param);

    for( int n=2; n <= 16; n += 2 )
    {
        std::cout << "n = " << n << std::endl;
        Rosenbrock fun(n);
        for( int test=0; test < 1024; test++ )
        {
            VectorXd x = VectorXd::Random(n);
            double fx;
            int niter = solver.minimize(fun, x, fx);

            assert( ( (x.array() - 1.0).abs() < 1e-4 ).all() );
        }
        std::cout << "Test passed!" << std::endl << std::endl;
    }

    return 0;
}


#include <iostream>
#include <vector>
#include <functional>
#include <cmath>
#include <Eigen/Dense>

#include "Derivative.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;

using Eigen::Derivative;

using std::function;
using std::vector;

VectorXd LevenbergMarquardt(vector<Derivative> fs, VectorXd x){
    int x_size = x.size(), f_size = fs.size();

    vector< vector<Derivative> > Jac( f_size, vector<Derivative>(x_size) );

    for(int lf = 0;lf < f_size;lf++)
        for(int lx = 0;lx < x_size;lx++)
            Jac[lf][lx] = fs[lf].diffPartial(lx);

    // Set eps to be very larg
    double mu = 0.01, eps = 10000000;

    MatrixXd I = MatrixXd::Identity(x_size, x_size);

    for(int iter = 0;iter < 200;iter++){
        MatrixXd J(f_size, x_size);
        for(int lf = 0;lf < f_size;lf++)
            for(int lx = 0;lx < x_size;lx++)
                J(lf, lx) = Jac[lf][lx](x);

        VectorXd rx(f_size);
        for(int lf = 0;lf < f_size;lf++)
            rx[lf] = fs[lf](x);

        double rx_norm = rx.norm();

        VectorXd delta = (mu*I + J.transpose()*J).inverse()*J.transpose()*rx;
        x -= delta;

        if(eps < rx_norm)
            mu *= 2;
        else if(eps > rx_norm)
            mu /= 2;

        if(rx_norm < 0.0001 or delta.norm() < 0.0001)
            break;

        eps = rx_norm;
    }

    return x;
}

int main(){
    // Solve : xx + yy + zz = 2
    //          x + y + 2*z = 1

    Derivative x = Derivative::Variable(0),
               y = Derivative::Variable(1),
               z = Derivative::Variable(2);

    Derivative f1 = x*x + y*y + z*z - 2, f2 = x + y + 2*z - 1;

    VectorXd v(3);
    v << 1, 1, 1;

    std::cout << LevenbergMarquardt({f1, f2}, v).transpose() << std::endl;

    return 0;
}


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/// \file
/// Tests for the KalmanFilter.cpp functions.

#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

#include "SurgSim/Math/KalmanFilter.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Vector.h"

typedef Eigen::Matrix<double, 1, 1> Vector1d;
typedef Eigen::Matrix<double, 1, 1, Eigen::RowMajor> Matrix11d;

namespace SurgSim
{

namespace Math
{
TEST(KalmanFilterTests, 1DConstant)
{
	auto kalman = std::make_shared<KalmanFilter<1, 1>>();
	EXPECT_TRUE(boost::math::isnan(kalman->getState()[0]));

	const Vector1d initialState = Vector1d::Zero();
	kalman->setInitialState(initialState); // the state is a scalar, and we have an initial guess for it
	EXPECT_DOUBLE_EQ(initialState[0], kalman->getState()[0]);

	kalman->setInitialStateCovariance(Matrix11d::Constant(1000.0)); // the uncertainty about initial guess is high
	kalman->setStateTransition(Matrix11d::Constant(1.0)); // we predict the true state will stay constant
	kalman->setObservationMatrix(Matrix11d::Constant(1.0)); // we observe the actual state plus measurement error
	kalman->setProcessNoiseCovariance(Matrix11d::Constant(0.0001)); // the noise in the prediction action is low
	kalman->setMeasurementNoiseCovariance(Matrix11d::Constant(0.1)); // the assumed measurement noise

	const double measurements[] = {0.9, 0.8, 1.1, 1, 0.95, 1.05, 1.2, 0.9, 0.85, 1.15};
	for (double measurement : measurements)
	{
		kalman->update(Vector1d::Constant(measurement));
	}

	const double result = 0.99046032523740679;
	EXPECT_NEAR(result , kalman->getState()[0], 1e-9);
	// The state doesn't change unless there's an update.
	EXPECT_NEAR(result, kalman->getState()[0], 1e-9);
	// Updating a constant model without a measurement shouldn't change the state.
	EXPECT_NEAR(result, kalman->update()[0], 1e-9);
}

TEST(KalmanFilterTests, 1DVelocity)
{
	auto kalman = std::make_shared<KalmanFilter<2, 1>>();
	// The state is the position and velocity, and here's our initial guess.
	const Vector2d initialState = Vector2d::Zero();
	kalman->setInitialState(initialState);
	const Matrix22d initialStateCovariance = 1000.0 * Matrix22d::Identity();
	kalman->setInitialStateCovariance(initialStateCovariance);
	// The new position is the old position plus velocity times dt.  The velocity is constant.
	const double dt = 0.1;
	Matrix22d stateTransition;
	stateTransition << 1.0, dt,
						0.0, 1.0;
	kalman->setStateTransition(stateTransition);
	// The measurements are of the position, plus measurement error.
	Eigen::Matrix<double, 1, 2, Eigen::RowMajor> observationMatrix;
	observationMatrix.setIdentity();
	kalman->setObservationMatrix(observationMatrix);
	const double velocityNoise = 0.001;
	// Assume the noise is only in the velocity, so a continuous noise model of [0, 0; 0, velocityNoise], then
	// approximate that by a time-discrete process to get...
	Matrix22d processNoise;
	processNoise << dt * dt * dt * velocityNoise / 3.0, dt * dt * velocityNoise / 2.0,
					dt * dt * velocityNoise / 3.0,			dt * velocityNoise;
	kalman->setProcessNoiseCovariance(processNoise);
	kalman->setMeasurementNoiseCovariance(Matrix11d::Constant(0.1));

	const double measurements[] = {0.11, 0.29, 0.32, 0.5, 0.58, 0.54};
	for (double measurement : measurements)
	{
		kalman->update(Vector1d::Constant(measurement));
	}

	// check the state
	const double position = 0.61844193701221828;
	EXPECT_NEAR(position, kalman->getState()[0], 1e-9);
	const double velocity = 0.91376229444025137;
	EXPECT_NEAR(velocity, kalman->getState()[1], 1e-9);

	// The state doesn't change unless there's an update.
	EXPECT_NEAR(position, kalman->getState()[0], 1e-9);
	// Updating without a measurement will predict ahead.
	EXPECT_NEAR(position + velocity * dt, kalman->update()[0], 1e-9);
	EXPECT_NEAR(velocity, kalman->getState()[1], 1e-9);
}
}; // namespace Math

}; // namespace SurgSim