/*
 * @Description: Extended Kalman Filter for IMU-Lidar-GNSS-Odo-Mag fusion
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */
#include <limits>

#include <cmath>
#include <iostream>
#include <fstream>
#include <ostream>

// use sophus to handle so3 hat & SO3 log operations:
#include <sophus/so3.hpp>

// SVD for observability analysis:
#include <Eigen/SVD>

// mag table lookup:
#include "lidar_localization/models/mag_table/geo_mag_declination.hpp"

#include "lidar_localization/models/kalman_filter/extended_kalman_filter.hpp"

#include "lidar_localization/global_defination/global_defination.h"

#include "lidar_localization/tools/CSVWriter.hpp"

#include "glog/logging.h"

namespace lidar_localization {

ExtendedKalmanFilter::ExtendedKalmanFilter(const YAML::Node& node) {
    //
    // parse config:
    //
    // a. earth constants:
    EARTH.GRAVITY_MAGNITUDE = node["earth"]["gravity_magnitude"].as<double>();
    EARTH.ROTATION_SPEED = node["earth"]["rotation_speed"].as<double>();

    EARTH.LATITUDE = node["earth"]["latitude"].as<double>();
    EARTH.LONGITUDE = node["earth"]["longitude"].as<double>();

    /*
    // TODO: fix missing dependency mathlib
    double dec = get_mag_declination(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );
    double inc = get_mag_inclination(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );
    double mag = get_mag_strength(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );

    EARTH.MAG.B_U = -mag*sin(inc/180.0*M_PI);
    EARTH.MAG.B_N = +mag*cos(inc/180.0*M_PI)*cos(dec/180.0*M_PI);
    EARTH.MAG.B_E = +mag*cos(inc/180.0*M_PI)*sin(dec/180.0*M_PI);
    */

    EARTH.MAG.B_E = node["earth"]["magneto"]["B_E"].as<double>();
    EARTH.MAG.B_N = node["earth"]["magneto"]["B_N"].as<double>();
    EARTH.MAG.B_U = node["earth"]["magneto"]["B_U"].as<double>();

    EARTH.LATITUDE *= M_PI / 180.0;

    // b. prior state covariance:
    COV.PRIOR.POSI = node["covariance"]["prior"]["pos"].as<double>();
    COV.PRIOR.VEL = node["covariance"]["prior"]["vel"].as<double>();
    // TODO: find a better way for quaternion orientation prior covariance assignment
    COV.PRIOR.ORI = node["covariance"]["prior"]["ori"].as<double>();
    COV.PRIOR.EPSILON = node["covariance"]["prior"]["epsilon"].as<double>();
    COV.PRIOR.DELTA = node["covariance"]["prior"]["delta"].as<double>();
    // c. process noise:
    COV.PROCESS.GYRO = node["covariance"]["process"]["gyro"].as<double>();
    COV.PROCESS.ACCEL = node["covariance"]["process"]["accel"].as<double>();
    // d. measurement noise:
    COV.MEASUREMENT.POSE.POSI = node["covariance"]["measurement"]["pose"]["pos"].as<double>();
    COV.MEASUREMENT.POSE.ORI = node["covariance"]["measurement"]["pose"]["ori"].as<double>();
    COV.MEASUREMENT.POSI = node["covariance"]["measurement"]["pos"].as<double>();
    COV.MEASUREMENT.VEL = node["covariance"]["measurement"]["vel"].as<double>();
    COV.MEASUREMENT.MAG = node["covariance"]["measurement"]["mag"].as<double>();
    // e. motion constraint:
    MOTION_CONSTRAINT.ACTIVATED = node["motion_constraint"]["activated"].as<bool>();
    MOTION_CONSTRAINT.W_B_THRESH = node["motion_constraint"]["w_b_thresh"].as<double>();

    // prompt:
    LOG(INFO) << std::endl
              << "Iterative Extended Kalman Filter params:" << std::endl
              << "\tgravity magnitude: " << EARTH.GRAVITY_MAGNITUDE << std::endl
              << "\tearth rotation speed: " << EARTH.ROTATION_SPEED << std::endl
              << "\tlatitude: " << EARTH.LATITUDE << std::endl
              << "\tlongitude: " << EARTH.LATITUDE << std::endl
              << "\tmagneto: " << std::endl
              << "\t\tB_E: " << EARTH.MAG.B_E << std::endl
              << "\t\tB_N: " << EARTH.MAG.B_N << std::endl
              << "\t\tB_U: " << EARTH.MAG.B_U << std::endl
              << std::endl
              << "\tprior cov. pos.: " << COV.PRIOR.POSI  << std::endl
              << "\tprior cov. vel.: " << COV.PRIOR.VEL << std::endl
              << "\tprior cov. ori: " << COV.PRIOR.ORI << std::endl
              << "\tprior cov. epsilon.: " << COV.PRIOR.EPSILON  << std::endl
              << "\tprior cov. delta.: " << COV.PRIOR.DELTA << std::endl
              << std::endl
              << "\tprocess noise gyro.: " << COV.PROCESS.GYRO << std::endl
              << "\tprocess noise accel.: " << COV.PROCESS.ACCEL << std::endl
              << std::endl
              << "\tmeasurement noise pose.: " << std::endl
              << "\t\tpos: " << COV.MEASUREMENT.POSE.POSI << ", ori.: " << COV.MEASUREMENT.POSE.ORI << std::endl
              << "\tmeasurement noise pos.: " << COV.MEASUREMENT.POSI << std::endl
              << "\tmeasurement noise vel.: " << COV.MEASUREMENT.VEL << std::endl
              << "\tmeasurement noise mag.: " << COV.MEASUREMENT.MAG << std::endl
              << std::endl
              << "\tmotion constraint: " << std::endl
              << "\t\tactivated: " << (MOTION_CONSTRAINT.ACTIVATED ? "true" : "false") << std::endl
              << "\t\tw_b threshold: " << MOTION_CONSTRAINT.W_B_THRESH << std::endl
              << std::endl;

    //
    // init filter:
    //
    // a. earth constants:
    g_ = Eigen::Vector3d(
        0.0,
        0.0,
        EARTH.GRAVITY_MAGNITUDE
    );
    w_ = Eigen::Vector3d(
        0.0,
        EARTH.ROTATION_SPEED*cos(EARTH.LATITUDE),
        EARTH.ROTATION_SPEED*sin(EARTH.LATITUDE)
    );
    b_ = Eigen::Vector3d(
        EARTH.MAG.B_E,
        EARTH.MAG.B_N,
        EARTH.MAG.B_U
    );

    // b. prior state & covariance:
    ResetState();
    ResetCovariance();

    // c. process noise:
    Q_.block<3, 3>(0, 0) = COV.PROCESS.GYRO*Eigen::Matrix3d::Identity();
    Q_.block<3, 3>(3, 3) = COV.PROCESS.ACCEL*Eigen::Matrix3d::Identity();

    // d. measurement noise:
    RPose_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSE.POSI*Eigen::Matrix3d::Identity();
    RPose_.block<4, 4>(3, 3) = COV.MEASUREMENT.POSE.ORI*Eigen::Matrix4d::Identity();

    RPoseVel_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSE.POSI*Eigen::Matrix3d::Identity();
    RPoseVel_.block<4, 4>(3, 3) = COV.MEASUREMENT.POSE.ORI*Eigen::Matrix4d::Identity();
    RPoseVel_.block<3, 3>(7, 7) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();

    RPosi_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();

    RPosiVel_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiVel_.block<3, 3>(3, 3) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();

    RPosiMag_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiMag_.block<3, 3>(3, 3) = COV.MEASUREMENT.MAG*Eigen::Matrix3d::Identity();

    RPosiVelMag_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiVelMag_.block<3, 3>(3, 3) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();
    RPosiVelMag_.block<3, 3>(6, 6) = COV.MEASUREMENT.MAG*Eigen::Matrix3d::Identity();

    // e. process equation:
    F_.block<3, 3>( INDEX_POS, INDEX_VEL ) = Eigen::Matrix3d::Identity();

    // f. measurement equation:
    GPose_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();
    GPose_.block<4, 4>( 3, INDEX_ORI ) = Eigen::Matrix4d::Identity();

    GPoseVel_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();
    GPoseVel_.block<4, 4>( 3, INDEX_ORI ) = Eigen::Matrix4d::Identity();

    GPosi_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiVel_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiMag_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiVelMag_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    // init soms:
    QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>(0, 0) = GPose_;
    QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(0, 0) = GPoseVel_;
    QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>(0, 0) = GPosi_;
    QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(0, 0) = GPosiVel_;
    QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(0, 0) = GPosiMag_;
    QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(0, 0) = GPosiVelMag_;
}

/**
 * @brief  init filter
 * @param  pose, init pose
 * @param  vel, init vel
 * @param  imu_data, init IMU measurements
 * @return true if success false otherwise
 */
void ExtendedKalmanFilter::Init(
    const Eigen::Vector3d &vel,
    const IMUData &imu_data
) {
    // get init C_nb from IMU estimation:
    Eigen::Matrix3d C_nb = imu_data.GetOrientationMatrix().cast<double>();
    Eigen::Vector4d q_nb(
        imu_data.orientation.w,
        imu_data.orientation.x,
        imu_data.orientation.y,
        imu_data.orientation.z
    );
    // get init v_n from v_b:
    Eigen::Vector3d v_n = C_nb*vel;

    // set init pose:
    init_pose_.block<3, 3>(0, 0) = C_nb;

    // set init velocity:
    init_vel_ = v_n;

    // set init state:
    X_.block<3, 1>(INDEX_POS, 0) = init_pose_.block<3, 1>(0, 3);
    X_.block<3, 1>(INDEX_VEL, 0) = v_n;
    X_.block<4, 1>(INDEX_ORI, 0) = q_nb;
    X_.block<3, 1>(INDEX_GYRO_BIAS, 0) = gyro_bias_;
    X_.block<3, 1>(INDEX_ACCEL_BIAS, 0) = accel_bias_;

    // init IMU data buffer:
    imu_data_buff_.clear();
    imu_data_buff_.push_back(imu_data);

    // init filter time:
    time_ = imu_data.time;

    LOG(INFO) << std::endl
              << "IEKF Inited at " << static_cast<int>(time_) << std::endl
              << "Init Position: "
              << init_pose_(0, 3) << ", "
              << init_pose_(1, 3) << ", "
              << init_pose_(2, 3) << std::endl
              << "Init Velocity: "
              << init_vel_.x() << ", "
              << init_vel_.y() << ", "
              << init_vel_.z() << std::endl;
}

/**
 * @brief  Kalman update
 * @param  imu_data, input IMU measurements
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::Update(const IMUData &imu_data) {
    // update IMU buff:
    if (time_ < imu_data.time) {
        // Kalman prediction for covariance:
        UpdateCovarianceEstimation(imu_data);

        // Kalman prediction for state:
        imu_data_buff_.push_back(imu_data);
        UpdateStateEstimation();
        imu_data_buff_.pop_front();

        // move forward:
        time_ = imu_data.time;

        return true;
    }

    return false;
}

/**
 * @brief  Kalman correction, pose measurement and other measurement in body frame
 * @param  measurement_type, input measurement type
 * @param  measurement, input measurement
 * @return void
 */
bool ExtendedKalmanFilter::Correct(
    const IMUData &imu_data,
    const MeasurementType &measurement_type, const Measurement &measurement
) {
    static Measurement measurement_;

    // get time delta:
    double time_delta = measurement.time - time_;

    if ( time_delta > -0.05 ) {
        // perform Kalman prediction:
        if ( time_ < measurement.time ) {
            Update(imu_data);
        }

        // get observation in navigation frame:
        measurement_ = measurement;
        measurement_.T_nb = init_pose_ * measurement_.T_nb;

        // correct error estimation:
        CorrectStateEstimation(measurement_type, measurement_);

        return true;
    }

    LOG(INFO) << "IEKF Correct: Observation is not synced with filter. Skip, "
              << (int)measurement.time << " <-- " << (int)time_ << " @ " << time_delta
              << std::endl;

    return false;
}

/**
 * @brief  get odometry estimation
 * @param  pose, init pose
 * @param  vel, init vel
 * @return void
 */
void ExtendedKalmanFilter::GetOdometry(
    Eigen::Matrix4f &pose, Eigen::Vector3f &vel
) {
    // init:
    Eigen::Matrix4d pose_double = Eigen::Matrix4d::Identity();
    Eigen::Vector3d vel_double = Eigen::Vector3d::Zero();

    // eliminate error:
    // a. position:
    pose_double.block<3, 1>(0, 3) = X_.block<3, 1>(INDEX_POS, 0);
    // b. velocity:
    vel_double = X_.block<3, 1>(INDEX_VEL, 0);
    // c. orientation:
    Eigen::Quaterniond q_nb(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    pose_double.block<3, 3>(0, 0) = q_nb.toRotationMatrix();

    // finally:
    pose_double = init_pose_.inverse() * pose_double;
    vel_double = init_pose_.block<3, 3>(0, 0).transpose() * vel_double;

    pose = pose_double.cast<float>();
    vel = vel_double.cast<float>();
}

/**
 * @brief  get covariance estimation
 * @param  cov, covariance output
 * @return void
 */
void ExtendedKalmanFilter::GetCovariance(Cov &cov) {
    static int OFFSET_X = 0;
    static int OFFSET_Y = 1;
    static int OFFSET_Z = 2;

    // a. delta position:
    cov.pos.x = P_(INDEX_POS + OFFSET_X, INDEX_POS + OFFSET_X);
    cov.pos.y = P_(INDEX_POS + OFFSET_Y, INDEX_POS + OFFSET_Y);
    cov.pos.z = P_(INDEX_POS + OFFSET_Z, INDEX_POS + OFFSET_Z);

    // b. delta velocity:
    cov.vel.x = P_(INDEX_VEL + OFFSET_X, INDEX_VEL + OFFSET_X);
    cov.vel.y = P_(INDEX_VEL + OFFSET_Y, INDEX_VEL + OFFSET_Y);
    cov.vel.z = P_(INDEX_VEL + OFFSET_Z, INDEX_VEL + OFFSET_Z);

    // c. delta orientation:
    cov.ori.w = P_(INDEX_ORI + 0, INDEX_ORI + 0);
    cov.ori.x = P_(INDEX_ORI + 1, INDEX_ORI + 1);
    cov.ori.y = P_(INDEX_ORI + 2, INDEX_ORI + 2);
    cov.ori.z = P_(INDEX_ORI + 3, INDEX_ORI + 3);

    // d. gyro. bias:
    cov.gyro_bias.x = P_(INDEX_GYRO_BIAS + OFFSET_X, INDEX_GYRO_BIAS + OFFSET_X);
    cov.gyro_bias.y = P_(INDEX_GYRO_BIAS + OFFSET_Y, INDEX_GYRO_BIAS + OFFSET_Y);
    cov.gyro_bias.z = P_(INDEX_GYRO_BIAS + OFFSET_Z, INDEX_GYRO_BIAS + OFFSET_Z);

    // e. accel bias:
    cov.accel_bias.x = P_(INDEX_ACCEL_BIAS + OFFSET_X, INDEX_ACCEL_BIAS + OFFSET_X);
    cov.accel_bias.y = P_(INDEX_ACCEL_BIAS + OFFSET_Y, INDEX_ACCEL_BIAS + OFFSET_Y);
    cov.accel_bias.z = P_(INDEX_ACCEL_BIAS + OFFSET_Z, INDEX_ACCEL_BIAS + OFFSET_Z);
}

/**
 * @brief  get unbiased angular velocity in body frame
 * @param  angular_vel, angular velocity measurement
 * @param  C_nb, corresponding orientation of measurement
 * @return unbiased angular velocity in body frame
 */
inline Eigen::Vector3d ExtendedKalmanFilter::GetUnbiasedAngularVel(
    const Eigen::Vector3d &angular_vel,
    const Eigen::Matrix3d &C_nb
) {
    return angular_vel - gyro_bias_;
}

/**
 * @brief  get unbiased linear acceleration in navigation frame
 * @param  linear_acc, linear acceleration measurement
 * @param  C_nb, corresponding orientation of measurement
 * @return unbiased linear acceleration in navigation frame
 */
inline Eigen::Vector3d ExtendedKalmanFilter::GetUnbiasedLinearAcc(
    const Eigen::Vector3d &linear_acc,
    const Eigen::Matrix3d &C_nb
) {
    return C_nb*(linear_acc - accel_bias_) - g_;
}

/**
 * @brief  remove gravity component from accel measurement
 * @param  f_b, accel measurement measurement
 * @param  C_nb, orientation matrix
 * @return f_b
 */
inline Eigen::Vector3d ExtendedKalmanFilter::RemoveGravity (
    const Eigen::Vector3d &f_b,
    const Eigen::Matrix3d &C_nb
) {
    return f_b - C_nb.transpose()*g_;
}

/**
 * @brief  apply motion constraint on velocity estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ApplyMotionConstraint(void) {
    const Eigen::Quaterniond q_nb(
        X_( INDEX_ORI + 0, 0),
        X_( INDEX_ORI + 1, 0),
        X_( INDEX_ORI + 2, 0),
        X_( INDEX_ORI + 3, 0)
    );
    const Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();

    Eigen::Vector3d v_b = C_nb.transpose() * X_.block<3, 1>( INDEX_VEL, 0 );
    v_b.y() = 0.0;
    X_.block<3, 1>( INDEX_VEL, 0 ) = C_nb * v_b;
}

/**
 * @brief  get angular delta
 * @param  index_curr, current imu measurement buffer index
 * @param  index_prev, previous imu measurement buffer index
 * @param  angular_delta, angular delta output
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::GetAngularDelta(
    const size_t index_curr, const size_t index_prev,
    Eigen::Vector3d &angular_delta
) {
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    double delta_t = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d angular_vel_curr = Eigen::Vector3d(
        imu_data_curr.angular_velocity.x,
        imu_data_curr.angular_velocity.y,
        imu_data_curr.angular_velocity.z
    );
    Eigen::Matrix3d R_curr = imu_data_curr.GetOrientationMatrix().cast<double>();
    angular_vel_curr = GetUnbiasedAngularVel(angular_vel_curr, R_curr);

    Eigen::Vector3d angular_vel_prev = Eigen::Vector3d(
        imu_data_prev.angular_velocity.x,
        imu_data_prev.angular_velocity.y,
        imu_data_prev.angular_velocity.z
    );
    Eigen::Matrix3d R_prev = imu_data_prev.GetOrientationMatrix().cast<double>();
    angular_vel_prev = GetUnbiasedAngularVel(angular_vel_prev, R_prev);

    angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev);

    return true;
}

/**
 * @brief  update orientation with effective rotation angular_delta
 * @param  angular_delta, effective rotation
 * @param  R_curr, current orientation
 * @param  R_prev, previous orientation
 * @return void
 */
void ExtendedKalmanFilter::UpdateOrientation(
    const Eigen::Vector3d &angular_delta,
    Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
) {
    // magnitude:
    double angular_delta_mag = angular_delta.norm();
    // direction:
    Eigen::Vector3d angular_delta_dir = angular_delta.normalized();

    // build delta q:
    double angular_delta_cos = cos(angular_delta_mag/2.0);
    double angular_delta_sin = sin(angular_delta_mag/2.0);

    Eigen::Quaterniond q(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    Eigen::Quaterniond dq(
        angular_delta_cos,
        angular_delta_sin*angular_delta_dir.x(),
        angular_delta_sin*angular_delta_dir.y(),
        angular_delta_sin*angular_delta_dir.z()
    );

    // update:
    R_prev = q.toRotationMatrix();

    q = q*dq;
    q.normalize();

    R_curr = q.toRotationMatrix();

    // write back:
    X_(INDEX_ORI + 0, 0) = q.w();
    X_(INDEX_ORI + 1, 0) = q.x();
    X_(INDEX_ORI + 2, 0) = q.y();
    X_(INDEX_ORI + 3, 0) = q.z();
}

/**
 * @brief  get velocity delta
 * @param  index_curr, current imu measurement buffer index
 * @param  index_prev, previous imu measurement buffer index
 * @param  R_curr, corresponding orientation of current imu measurement
 * @param  R_prev, corresponding orientation of previous imu measurement
 * @param  velocity_delta, velocity delta output
 * @param  linear_acc_mid, mid-value unbiased linear acc
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::GetVelocityDelta(
    const size_t index_curr, const size_t index_prev,
    const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev,
    double &T,
    Eigen::Vector3d &velocity_delta
) {
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    T = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d linear_acc_curr = Eigen::Vector3d(
        imu_data_curr.linear_acceleration.x,
        imu_data_curr.linear_acceleration.y,
        imu_data_curr.linear_acceleration.z
    );
    linear_acc_curr = GetUnbiasedLinearAcc(linear_acc_curr, R_curr);
    Eigen::Vector3d linear_acc_prev = Eigen::Vector3d(
        imu_data_prev.linear_acceleration.x,
        imu_data_prev.linear_acceleration.y,
        imu_data_prev.linear_acceleration.z
    );
    linear_acc_prev = GetUnbiasedLinearAcc(linear_acc_prev, R_prev);

    // mid-value acc can improve error state prediction accuracy:
    velocity_delta = 0.5*T*(linear_acc_curr + linear_acc_prev);

    return true;
}

/**
 * @brief  update orientation with effective velocity change velocity_delta
 * @param  T, timestamp delta
 * @param  velocity_delta, effective velocity change
 * @return void
 */
void ExtendedKalmanFilter::UpdatePosition(const double &T, const Eigen::Vector3d &velocity_delta) {
    X_.block<3, 1>(INDEX_POS, 0) += T*X_.block<3, 1>(INDEX_VEL, 0) + 0.5*T*velocity_delta;
    X_.block<3, 1>(INDEX_VEL, 0) += velocity_delta;
}

/**
 * @brief  update state estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::UpdateStateEstimation(void) {
    // get deltas:
    Eigen::Vector3d angular_delta;
    GetAngularDelta(1, 0, angular_delta);

    // update orientation:
    Eigen::Matrix3d R_curr, R_prev;
    UpdateOrientation(angular_delta, R_curr, R_prev);

    // get velocity delta:
    double T;
    Eigen::Vector3d velocity_delta;
    // save mid-value unbiased linear acc for error-state update:
    GetVelocityDelta(1, 0, R_curr, R_prev, T, velocity_delta);

    // update position:
    UpdatePosition(T, velocity_delta);

    // apply motion constraint:
    ApplyMotionConstraint();
}

/**
 * @brief  get block matrix for velocity update by orientation quaternion
 * @param  f_b, accel measurement
 * @param  q_nb, orientation quaternion
 * @return block matrix Fvq
 */
Eigen::Matrix<double, 3, 4> ExtendedKalmanFilter::GetFVelOri(
    const Eigen::Vector3d &f_b,
    const Eigen::Quaterniond &q_nb
) {
    // get F:
    Eigen::Matrix<double, 4, 3> T_Ff;
    T_Ff << +q_nb.w(), -q_nb.z(), +q_nb.y(),
            +q_nb.x(), +q_nb.y(), +q_nb.z(),
            -q_nb.y(), +q_nb.x(), +q_nb.w(),
            -q_nb.z(), -q_nb.w(), +q_nb.x();
    Eigen::Vector4d F = 2 * T_Ff * f_b;

    // get Fvq:
    Eigen::Matrix<double, 3, 4> Fvq;
    Fvq << +F(0), +F(1), +F(2), +F(3),
           -F(3), -F(2), +F(1), +F(0),
           +F(2), -F(3), -F(0), +F(1);

    return Fvq;
}

/**
 * @brief  get block matrix for orientation quaternion update by orientation quaternion
 * @param  w_b, gyro measurement
 * @return block matrix Fqq
 */
Eigen::Matrix<double, 4, 4> ExtendedKalmanFilter::GetFOriOri(
    const Eigen::Vector3d &w_b
) {
    // get Fqq:
    Eigen::Matrix<double, 4, 4> Fqq;
    Fqq <<      0.0, -w_b.x(), -w_b.y(), -w_b.z(),
           +w_b.x(),      0.0, +w_b.z(), -w_b.y(),
           +w_b.y(), -w_b.z(),      0.0, +w_b.x(),
           +w_b.z(), +w_b.y(), -w_b.x(),      0.0;

    return 0.5 * Fqq;
}

/**
 * @brief  get block matrix for orientation quaternion update by epsilon, angular velocity bias
 * @param  q_nb, orientation quaternion
 * @return block matrix Fqe
 */
Eigen::Matrix<double, 4, 3> ExtendedKalmanFilter::GetFOriEps(
    const Eigen::Quaterniond &q_nb
) {
    // get Fqe:
    Eigen::Matrix<double, 4, 3> Fqe;
    Fqe << -q_nb.x(), -q_nb.y(), -q_nb.z(),
           +q_nb.w(), -q_nb.z(), +q_nb.y(),
           +q_nb.z(), +q_nb.w(), -q_nb.x(),
           -q_nb.y(), +q_nb.x(), +q_nb.w();

    return 0.5 * Fqe;
}

/**
 * @brief  set process equation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::SetProcessEquation(const IMUData &imu_data) {
    // parse IMU measurement:
    const Eigen::Vector3d f_b(
        imu_data.linear_acceleration.x,
        imu_data.linear_acceleration.y,
        imu_data.linear_acceleration.z
    );
    const Eigen::Vector3d w_b(
        imu_data.angular_velocity.x,
        imu_data.angular_velocity.y,
        imu_data.angular_velocity.z
    );

    // parse orientation:
    const Eigen::Quaterniond q_nb(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    const Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();

    //
    // EKF is linearized around VectorX::Zero
    //
    // a. set equation for velocity:
    F_.block<3, 4>(INDEX_VEL,         INDEX_ORI) = GetFVelOri(f_b, q_nb);
    F_.block<3, 3>(INDEX_VEL,  INDEX_ACCEL_BIAS) = B_.block<3, 3>(INDEX_VEL, 3) = C_nb;

    // b. set equation for orientation quaternion:
    F_.block<4, 4>(INDEX_ORI,         INDEX_ORI) = GetFOriOri(w_b);
    F_.block<4, 3>(INDEX_ORI,   INDEX_GYRO_BIAS) = B_.block<4, 3>(INDEX_ORI, 0) = GetFOriEps(q_nb);
}

/**
 * @brief  update covariance estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::UpdateCovarianceEstimation(
    const IMUData &imu_data
) {
    // update process equation:
    SetProcessEquation(imu_data);

    // get discretized process equations:
    double T = imu_data.time - time_;

    // approximate to 1st order:
    MatrixF F = MatrixF::Identity() + T*F_;

    MatrixB B = T*B_;

    // perform Kalman prediction for covariance:
    P_ = F*P_*F.transpose() + B*Q_*B.transpose();
}

/**
 * @brief  get block matrix for observation by orientation quaternion
 * @param  m_n, measurement in navigation frame
 * @param  q_nb, orientation quaternion
 * @return block matrix Gq
 */
Eigen::Matrix<double, 3, 4> ExtendedKalmanFilter::GetGMOri(
    const Eigen::Vector3d &m_n,
    const Eigen::Quaterniond &q_nb
) {
    // get F:
    Eigen::Matrix<double, 4, 3> T_Gm;
    T_Gm << +q_nb.w(), +q_nb.z(), -q_nb.y(),
            +q_nb.x(), +q_nb.y(), +q_nb.z(),
            -q_nb.y(), +q_nb.x(), -q_nb.w(),
            -q_nb.z(), +q_nb.w(), +q_nb.x();
    Eigen::Vector4d G = 2 * T_Gm * m_n;

    // get Fvq:
    Eigen::Matrix<double, 3, 4> Gq;
    Gq << +G(0), +G(1), +G(2), +G(3),
          +G(3), -G(2), +G(1), -G(0),
          -G(2), -G(3), +G(0), +G(1);

    return Gq;
}

/**
 * @brief  correct state estimation using frontend pose
 * @param  T_nb, input frontend pose estimation
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPose(
    const Eigen::Matrix4d &T_nb
) {
    // parse measurement:
    YPose_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    Eigen::Quaterniond q_nb(T_nb.block<3, 3>(0,0));
    YPose_(3, 0) = q_nb.w();
    YPose_(4, 0) = q_nb.x();
    YPose_(5, 0) = q_nb.y();
    YPose_(6, 0) = q_nb.z();

    // build Kalman gain:
    MatrixRPose R = GPose_*P_*GPose_.transpose() + RPose_;
    MatrixKPose K = P_*GPose_.transpose()*R.inverse();

    // perform Kalman correct:
    P_ = (MatrixP::Identity() - K*GPose_)*P_;
    X_ = X_ + K*(YPose_ - GPose_*X_);

    // normalize quaternion:
    X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
}

/**
 * @brief  correct state estimation using GNSS position
 * @param  T_nb, input GNSS position
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosi(
    const Eigen::Matrix4d &T_nb
) {
    // parse measurement:
    YPosi_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);

    // build Kalman gain:
    MatrixRPosi R = GPosi_*P_*GPosi_.transpose() + RPosi_;
    MatrixKPosi K = P_*GPosi_.transpose()*R.inverse();

    // perform Kalman correct:
    P_ = (MatrixP::Identity() - K*GPosi_)*P_;
    X_ = X_ + K*(YPosi_ - GPosi_*X_);

    // normalize quaternion:
    X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
}

/**
 * @brief  correct state estimation using frontend pose & body velocity measurement
 * @param  T_nb, input frontend pose estimation
 * @param  v_b, input odo
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPoseVel(
    const Eigen::Matrix4d &T_nb,
    const Eigen::Vector3d &v_b
) {
    // parse measurement:
    YPoseVel_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);

    Eigen::Quaterniond q_nb_obs(T_nb.block<3, 3>(0,0));
    YPoseVel_(3, 0) = q_nb_obs.w();
    YPoseVel_(4, 0) = q_nb_obs.x();
    YPoseVel_(5, 0) = q_nb_obs.y();
    YPoseVel_(6, 0) = q_nb_obs.z();

    YPoseVel_.block<3, 1>(7, 0) = v_b;

    // iterative observation:
    for (size_t i = 0; i < 3; ++i) {
        // set observation equation:
        Eigen::Quaterniond q_nb_pred(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb_pred = q_nb_pred.toRotationMatrix();
        GPoseVel_.block<3, 3>( 7, INDEX_VEL ) = C_nb_pred.transpose();
        GPoseVel_.block<3, 4>( 7, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb_pred);

        // build Kalman gain:
        MatrixRPoseVel R = GPoseVel_*P_*GPoseVel_.transpose() + RPoseVel_;
        MatrixKPoseVel K = P_*GPoseVel_.transpose()*R.inverse();
        VectorYPoseVel Y = VectorYPoseVel::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>(INDEX_POS, 0);
        Y.block<4, 1>(3, 0) = X_.block<4, 1>(INDEX_ORI, 0);
        Y.block<3, 1>(7, 0) = C_nb_pred.transpose() * X_.block<3, 1>(INDEX_VEL, 0);

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPoseVel_)*P_;
        X_ = X_ + K*(YPoseVel_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position and odometer measurement
 * @param  T_nb, input GNSS position
 * @param  v_b, input odo
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiVel(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b
) {
    // parse measurement:
    YPosiVel_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiVel_.block<3, 1>(3, 0) = v_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiVel_.block<3, 3>( 3, INDEX_VEL ) = C_nb.transpose();
        GPosiVel_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb);

        // build Kalman gain:
        MatrixRPosiVel R = GPosiVel_*P_*GPosiVel_.transpose() + RPosiVel_;
        MatrixKPosiVel K = P_*GPosiVel_.transpose()*R.inverse();
        VectorYPosiVel Y = X_.block<6, 1>(0, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * Y.block<3, 1>(3, 0);

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiVel_)*P_;
        X_ = X_ + K*(YPosiVel_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position and magneto measurement
 * @param  T_nb, input GNSS position
 * @param  B_b, input magneto
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiMag(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &B_b
) {
    // parse measurement:
    YPosiMag_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiMag_.block<3, 1>(3, 0) = B_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        const Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiMag_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(b_, q_nb);

        // build Kalman gain:
        MatrixRPosiMag R = GPosiMag_*P_*GPosiMag_.transpose() + RPosiMag_;
        MatrixKPosiMag K = P_*GPosiMag_.transpose()*R.inverse();
        VectorYPosiMag Y = VectorYPosiMag::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>( INDEX_POS, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * b_;

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiMag_)*P_;
        X_ = X_ + K*(YPosiMag_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position, odometer and magneto measurement
 * @param  T_nb, input GNSS position
 * @param  v_b, input odo
 * @param  B_b, input magneto
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiVelMag(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &B_b
) {
    // parse measurement:
    YPosiVelMag_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiVelMag_.block<3, 1>(3, 0) = v_b;
    YPosiVelMag_.block<3, 1>(6, 0) = B_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        const Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiVelMag_.block<3, 3>( 3, INDEX_VEL ) = C_nb.transpose();
        GPosiVelMag_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb);
        GPosiVelMag_.block<3, 4>( 6, INDEX_ORI ) = GetGMOri(b_, q_nb);

        // build Kalman gain:
        MatrixRPosiVelMag R = GPosiVelMag_*P_*GPosiVelMag_.transpose() + RPosiVelMag_;
        MatrixKPosiVelMag K = P_*GPosiVelMag_.transpose()*R.inverse();
        VectorYPosiVelMag Y = VectorYPosiVelMag::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>( INDEX_POS, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * X_.block<3, 1>(INDEX_VEL, 0);
        Y.block<3, 1>(6, 0) = C_nb.transpose() * b_;

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiVelMag_)*P_;
        X_ = X_ + K*(YPosiVelMag_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation
 * @param  measurement_type, measurement type
 * @param  measurement, input measurement
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimation(
    const MeasurementType &measurement_type,
    const Measurement &measurement
) {
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            CorrectStateEstimationPose(measurement.T_nb);
            break;
        case MeasurementType::POSE_VEL:
            CorrectStateEstimationPoseVel(measurement.T_nb, measurement.v_b);
            break;
        case MeasurementType::POSI:
            CorrectStateEstimationPosi(measurement.T_nb);
            break;
        case MeasurementType::POSI_VEL:
            CorrectStateEstimationPosiVel(measurement.T_nb, measurement.v_b);
            break;
        case MeasurementType::POSI_MAG:
            CorrectStateEstimationPosiMag(measurement.T_nb, measurement.B_b);
            break;
        case MeasurementType::POSI_VEL_MAG:
            CorrectStateEstimationPosiVelMag(measurement.T_nb, measurement.v_b, measurement.B_b);
            break;
        default:
            break;
    }

    // update bias:
    if ( IsCovStable(INDEX_GYRO_BIAS, 1.0e-6) )  gyro_bias_ = X_.block<3, 1>(INDEX_GYRO_BIAS, 0);
    if ( IsCovStable(INDEX_ACCEL_BIAS, 1.0e-6) ) accel_bias_ = X_.block<3, 1>(INDEX_ACCEL_BIAS, 0);
}

/**
 * @brief  is covariance stable
 * @param  INDEX_OFFSET, state index offset
 * @param  THRESH, covariance threshold, defaults to 1.0e-5
 * @return void
 */
bool ExtendedKalmanFilter::IsCovStable(
    const int INDEX_OFFSET,
    const double THRESH
) {
    if ( INDEX_ORI == INDEX_OFFSET ) {
        for (int i = 0; i < 4; ++i) {
            if ( P_(INDEX_OFFSET + i, INDEX_OFFSET + i) > THRESH ) {
                return false;
            }
        }
    } else {
        for (int i = 0; i < 3; ++i) {
            if ( P_(INDEX_OFFSET + i, INDEX_OFFSET + i) > THRESH ) {
                return false;
            }
        }
    }

    return true;
}

/**
 * @brief  reset filter state
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ResetState(void) {
    // reset current state:
    X_ = VectorX::Zero();
}

/**
 * @brief  reset filter covariance
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ResetCovariance(void) {
    P_ = MatrixP::Zero();

    P_.block<3, 3>(       INDEX_POS,        INDEX_POS) = COV.PRIOR.POSI*Eigen::Matrix3d::Identity();
    P_.block<3, 3>(       INDEX_VEL,        INDEX_VEL) = COV.PRIOR.VEL*Eigen::Matrix3d::Identity();
    // TODO: find a better way for quaternion orientation prior covariance assignment
    P_.block<4, 4>(       INDEX_ORI,        INDEX_ORI) = COV.PRIOR.ORI*Eigen::Matrix4d::Identity();
    P_.block<3, 3>( INDEX_GYRO_BIAS,  INDEX_GYRO_BIAS) = COV.PRIOR.EPSILON*Eigen::Matrix3d::Identity();
    P_.block<3, 3>(INDEX_ACCEL_BIAS, INDEX_ACCEL_BIAS) = COV.PRIOR.DELTA*Eigen::Matrix3d::Identity();
}

/**
 * @brief  get Q for pose measurement
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::GetQPose(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    for (int i = 1; i < DIM_STATE; ++i) {
        QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>(i*DIM_MEASUREMENT_POSE, 0) = (
            QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSE, 0) * F_
        );
    }

    Q = QPose_;
}

/**
 * @brief  get Q for pose & body velocity measurement
 * @param  void
 * @return void
 */
void  ExtendedKalmanFilter::GetQPoseVel(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(0, 0) = GPoseVel_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(i*DIM_MEASUREMENT_POSE_VEL, 0) = (
            QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSE_VEL, 0) * F_
        );
    }

    Q = QPoseVel_;
}

/**
 * @brief  get Q for GNSS position measurement
 * @param  void
 * @return QPosi
 */
void ExtendedKalmanFilter::GetQPosi(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>(i*DIM_MEASUREMENT_POSI, 0) = (
            QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI, 0) * F_
        );
    }

    Q = QPosi_;
}

/**
 * @brief  get Q for GNSS position & body velocity measurement
 * @param  void
 * @return QPosiVel
 */
 void ExtendedKalmanFilter::GetQPosiVel(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(0, 0) = GPosiVel_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(i*DIM_MEASUREMENT_POSI_VEL, 0) = (
            QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_VEL, 0) * F_
        );
    }

    Q = QPosiVel_;
}

/**
 * @brief  get Q for GNSS position & magneto measurement
 * @param  void
 * @return QPosiMag
 */
 void ExtendedKalmanFilter::GetQPosiMag(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(0, 0) = GPosiMag_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(i*DIM_MEASUREMENT_POSI_MAG, 0) = (
            QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_MAG, 0) * F_
        );
    }

    Q = QPosiMag_;
}

/**
 * @brief  get Q for GNSS position, body velocity & magneto measurement
 * @param  void
 * @return QPosiVelMag
 */
 void ExtendedKalmanFilter::GetQPosiVelMag(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(0, 0) = GPosiVelMag_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(i*DIM_MEASUREMENT_POSI_VEL_MAG, 0) = (
            QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_VEL_MAG, 0) * F_
        );
    }
    Q = QPosVelMag_;
}

/**
 * @brief  update observability analysis
 * @param  measurement_type, measurement type
 * @return void
 */
void ExtendedKalmanFilter::UpdateObservabilityAnalysis(
    const double &time,
    const MeasurementType &measurement_type
) {
    // get Q:
    Eigen::MatrixXd Q;
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            GetQPose(Q);
            break;
        case MeasurementType::POSE_VEL:
            GetQPoseVel(Q);
            break;
        case MeasurementType::POSI:
            GetQPosi(Q);
            break;
        case MeasurementType::POSI_VEL:
            GetQPosiVel(Q);
            break;
        case MeasurementType::POSI_MAG:
            GetQPosiMag(Q);
            break;
        case MeasurementType::POSI_VEL_MAG:
            GetQPosiVelMag(Q);
            break;
        default:
            break;
    }

    observability.time_.push_back(time);
    observability.Q_.push_back(Q);
}

/**
 * @brief  save observability analysis to persistent storage
 * @param  measurement_type, measurement type
 * @return void
 */
bool ExtendedKalmanFilter::SaveObservabilityAnalysis(
    const MeasurementType &measurement_type
) {
    // get fusion strategy:
    std::string type;
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            type = std::string("pose");
            break;
        case MeasurementType::POSE_VEL:
            type = std::string("pose_velocity");
            break;
        case MeasurementType::POSI:
            type = std::string("position");
            break;
        case MeasurementType::POSI_VEL:
            type = std::string("position_velocity");
            break;
        case MeasurementType::POSI_MAG:
            type = std::string("position_magneto");
            break;
        case MeasurementType::POSI_VEL_MAG:
            type = std::string("position_velocity_magneto");
            break;
        default:
            return false;
            break;
    }

    // build Q_so:
    const int N = observability.Q_.at(0).rows();

    std::vector<std::vector<double>> q_data, q_so_data;

    Eigen::MatrixXd Qso(
        observability.Q_.size() * N,
        DIM_STATE
    );
    for (size_t i = 0; i < observability.Q_.size(); ++i) {
        const double &time = observability.time_.at(i);
        const Eigen::MatrixXd &Q = observability.Q_.at(i);

        Qso.block(i * N, 0, N, DIM_STATE) = Q;

        KalmanFilter::AnalyzeQ(DIM_STATE, time, Q, q_data);

        KalmanFilter::AnalyzeQ(DIM_STATE, time, Qso.block(0, 0, (i + 1)*N, DIM_STATE), q_so_data);
    }

    std::string q_data_csv = WORK_SPACE_PATH + "/slam_data/observability/" + type + ".csv";
    std::string q_so_data_csv = WORK_SPACE_PATH + "/slam_data/observability/" + type + "_som.csv";

    KalmanFilter::WriteAsCSV(DIM_STATE, q_data, q_data_csv);
    KalmanFilter::WriteAsCSV(DIM_STATE, q_so_data, q_so_data_csv);

    return true;
}

} // namespace lidar_localization


/**
 * @file /yocs_math_toolkit/include/yocs_math_toolkit/sophus_helpers.hpp
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_
#define yocs_math_toolkit_SOPHUS_HELPERS_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ecl/config/macros.hpp>
#include <ecl/converters.hpp>
#include <ecl/exceptions/standard_exception.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#if defined(ECL_CXX11_FOUND)
    #include <memory>
#endif
#include <sophus/se3.hpp>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>
#include <sophus/so3.hpp>
#include <string>

#include "formatters.hpp"

/*****************************************************************************
** Namespace
*****************************************************************************/

namespace Sophus {

/*****************************************************************************
 ** C++11 Api Only
 *****************************************************************************/

#if defined(ECL_CXX11_FOUND)
    typedef std::shared_ptr<SE3f> SE3fPtr;

    /// Converts a line drawn between two points on the z-plane into the transform of a sophus frame relative to the origin.
    Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
#endif

/*****************************************************************************
** Interfaces
*****************************************************************************/

template<typename T>
std::ostream & operator << ( std::ostream & out, const SE3<T> & se3 )
{
//  typename SE3<T>::Tangent tanget_vector = SE3<T>::log( se3 );
//  out << tanget_vector.transpose();
  const Eigen::Matrix<T,3,1> & t = se3.translation();
  const Eigen::Quaternion<T> & q = se3.unit_quaternion();
  out << t.transpose() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
  return out;
}

template<typename T>
std::ostream & operator << ( std::ostream & out, const SE2<T> & se2 )
{
  typename SE2<T>::Tangent tanget_vector = SE2<T>::log( se2 );
  out << tanget_vector.transpose();
  return out;
}

/**
 * @brief Convert a full Sophus pose into a 2 dimensional pose.
 *
 * The 2d pose is a typical mobile robot 2d pose with (x, y, heading) with
 * heading measured in radians.
 **/
Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
/**
 * @brief Convert a 2 dimensional pose to a full Sophus pose in 3d.
 *
 * The 2d pose is a typical mobile robot 2d pose with (x, y, heading) with
 * heading measured in radians.
 **/
Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);

class PlanarRotation2Quaternion
{
public:
  PlanarRotation2Quaternion(){}
  Eigen::Quaternionf operator() ( float theta )
  {
    Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
    Eigen::Quaternionf q(R);
    q.normalize();
    return q;
//    Eigen::Quaternionf q;
//    // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
//    q.vec() << 0, 0, sin(theta*0.5f);
//    q.w() = cos(theta*0.5f);
//    q.normalize();
//    return q;
  }

  void convert( float theta, Eigen::Quaternionf & q )
  {
    q.vec() << 0, 0, sin(theta*0.5f);
    q.w() = cos(theta*0.5f);
  }
};

} // namespace Sophus

/*****************************************************************************
** Converters
*****************************************************************************/

namespace ecl {

/**
 * @brief Converter class representing the Sophus::toPose3D method.
 */
template<>
class Converter<Sophus::SE3f, Eigen::Vector3f> {
public:
  Sophus::SE3f operator()(const Eigen::Vector3f& pose);
};

/**
 * @brief Converter class representing the Sophus::toPose2D method.
 */
template<>
class Converter<Eigen::Vector3f, Sophus::SE3f> {
public:
  Eigen::Vector3f operator()(const Sophus::SE3f& pose);
};

} // namespace ecl

#endif /* yocs_math_toolkit_SOPHUS_HELPERS_HPP_ */


#include "lwtnn/Stack.hh"
#include <Eigen/Dense>

namespace lwt
{
  // Definitions for double overloads

  ILayer* get_raw_activation_layer(ActivationConfig activation) {
    return get_raw_activation_layerT<double>(activation);
  }

  std::function<double(double)> get_activation(lwt::ActivationConfig act) {
    return get_activationT<double>(act);
  }

  double nn_sigmoid( double x ) {
    return nn_sigmoidT<double>(x);
  }

  double nn_hard_sigmoid( double x ) {
    return nn_hard_sigmoidT<double>(x);
  }

  double nn_tanh( double x ) {
    return nn_tanhT<double>(x);
  }

  double nn_relu( double x) {
    return nn_reluT<double>(x);
  }

  MatrixX<double> build_matrix(const std::vector<double>& weights, size_t n_inputs) {
    return build_matrixT<double, double>(weights, n_inputs);
  }

  VectorX<double> build_vector(const std::vector<double>& bias) {
    return build_vectorT<double, double>(bias);
  }

  DenseComponents get_component(const lwt::LayerConfig& layer, size_t n_in) {
    return get_componentT<double>(layer, n_in);
  }

  // consistency checks
  void throw_if_not_maxout(const LayerConfig& layer) {
    bool wt_ok = layer.weights.size() == 0;
    bool bias_ok = layer.bias.size() == 0;
    bool maxout_ok = layer.sublayers.size() > 0;
    bool act_ok = layer.activation.function == Activation::NONE;
    if (wt_ok && bias_ok && maxout_ok && act_ok) return;
    throw NNConfigurationException("layer has wrong info for maxout");
  }
  void throw_if_not_dense(const LayerConfig& layer) {
    if (layer.sublayers.size() > 0) {
      throw NNConfigurationException("sublayers in dense layer");
    }
  }

  void throw_if_not_normalization(const LayerConfig& layer) {
    if (layer.sublayers.size() > 0) {
      throw NNConfigurationException("sublayers in normalization layer");
    }
  }
}


/* Boost test/overflow.cpp
 * test if extended precision exponent does not disturb interval computation
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: overflow.cpp,v 1.3 2003/02/05 17:34:36 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/test/minimal.hpp>

template<class I>
void test_one(typename I::base_type x, typename I::base_type f) {
  I y = x;
  typename I::base_type g = 1 / f;
  const int nb = 10000;
  for(int i = 0; i < nb; i++) y *= f;
  for(int i = 0; i < nb; i++) y *= g;
  BOOST_TEST(in(x, y));
}

template<class I>
void test() {
  test_one<I>(1., 25.);
  test_one<I>(1., 0.04);
  test_one<I>(-1., 25.);
  test_one<I>(-1., 0.04);
}

int test_main(int, char *[]) {
  test<boost::numeric::interval<float> >();
  test<boost::numeric::interval<double> >();
  test<boost::numeric::interval<long double> >();
  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <sstream>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/iterators/ever_circling_iterator.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>

#include <boost/range/adaptor/transformed.hpp>

template <typename G>
void test_geometry(G const& geo)
{
    typedef typename boost::range_iterator<G const>::type iterator_type;

    // Run 3 times through the geometry
    std::size_t n = boost::size(geo) * 3;

    {
        std::ostringstream out;
        bg::ever_circling_iterator<iterator_type> it(boost::begin(geo), boost::end(geo));
        for (std::size_t i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "123451234512345");
    }

    {
        std::ostringstream out;
        // Start somewhere
        bg::ever_circling_iterator<iterator_type> it(
            boost::begin(geo), boost::end(geo), boost::begin(geo) + 1);
        for (std::size_t i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "234512345123451");
    }

    {
        std::ostringstream out;

        // Navigate to somewhere
        bg::ever_circling_iterator<iterator_type> it(boost::begin(geo), boost::end(geo));
        for (std::size_t i = 0; i < n; ++i, ++it)
        {
            std::size_t const m = boost::size(geo);
            it.moveto(boost::begin(geo) + m - (i % m) - 1);
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "543215432154321");
    }

    // Check the range_iterator-one
    {
        std::ostringstream out;
        bg::ever_circling_range_iterator<G const> it(geo);
        for (std::size_t i = 0; i < n; ++i, ++it)
        {
            out << bg::get<0>(*it);
        }
        BOOST_CHECK_EQUAL(out.str(), "123451234512345");
    }
}

template <typename G>
void test_geometry(std::string const& wkt)
{
    G geo;
    bg::read_wkt(wkt, geo);
    test_geometry(geo);
}

template <typename P>
P transform_point(P const& p)
{
    P result;
    bg::set<0>(result, bg::get<0>(p) + 1);
    bg::set<1>(result, bg::get<1>(p) + 1);
    return result;
}

template <typename G>
struct transformed_geometry_type
{
    typedef typename bg::point_type<G>::type point_type;
    typedef boost::transformed_range<point_type(*)(point_type const&), G> type;
};

template <typename G>
void test_transformed_geometry(G const& geo)
{
    typedef typename bg::point_type<G>::type point_type;
    test_geometry(geo | boost::adaptors::transformed(&transform_point<point_type>));
}

template <typename G>
void test_transformed_geometry(std::string const& wkt)
{
    G geo;
    bg::read_wkt(wkt, geo);
    test_transformed_geometry(geo);
}

BOOST_GEOMETRY_REGISTER_LINESTRING(transformed_geometry_type< bg::model::linestring< bg::model::d2::point_xy<double> > >::type)

template <typename P>
void test_all()
{
    test_geometry<bg::model::linestring<P> >("linestring(1 1,2 2,3 3,4 4,5 5)");
    test_transformed_geometry<bg::model::linestring<P> >("linestring(0 0,1 1,2 2,3 3,4 4)");
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


#include <cfloat>
#include <type_traits>

#include <boost/qvm/all.hpp>

#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include <cppid/gm3d/vector_3d.hpp>

using cppid::gm3d::vector_3d;
using vec3f = vector_3d<float>;

TEST_CASE("float_vector3_test traits")
{
  REQUIRE(std::is_trivial_v<vec3f>);
  REQUIRE(std::is_standard_layout_v<vec3f>);
  REQUIRE(sizeof(vec3f) == (sizeof(float) * 3));

  REQUIRE((
   std::is_same_v<float, typename boost::qvm::vec_traits<vec3f>::scalar_type>));
  REQUIRE(boost::qvm::vec_traits<vec3f>::dim == 3);
}

TEST_CASE("float_vector3_test initialization")
{
  {
    auto vt = vec3f{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
    REQUIRE(vt.z() == 0.0f);
  }
  {
    auto vt = vec3f{1.5, 2.2, 8.9};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.2f);
    REQUIRE(vt.z() == 8.9f);
  }
  {
    auto vt = vec3f{vec3f{1.5, 2.3, 6.4}};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.3f);
    REQUIRE(vt.z() == 6.4f);
  }
  {
    auto vt = vec3f{vector_3d<int>{0}};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
    REQUIRE(vt.z() == 0.0f);
  }
  {
    auto vt = vec3f{0};
    vt = vector_3d<int>{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
    REQUIRE(vt.z() == 0.0f);
  }
  {
    auto vd = vector_3d<double>{0};
    vec3f vt = vd;
    CHECK(vt.x() == 0.0f);
    CHECK(vt.y() == 0.0f);
    CHECK(vt.z() == 0.0f);
  }
}

TEST_CASE("float_vector3_test comparison")
{
  REQUIRE((vec3f{0} == vec3f{0, 0, 0}));
  REQUIRE((vec3f{3.5} == vec3f{3.5, 3.5, 3.5}));
  REQUIRE((vec3f{-.5} == vec3f{-.5, -.5, -.5}));

  REQUIRE((vec3f{-.5} != vec3f{-.5, -.1, -.5}));
}

TEST_CASE("float_vector3_test addition")
{
  {
    auto vt = vec3f{-10.4, 0.4, -15.4};
    vt += vec3f{0};
    REQUIRE(vt.x() == Approx(-10.4f));
    REQUIRE(vt.y() == Approx(0.4f));
    REQUIRE(vt.z() == Approx(-15.4f));
  }
  {
    auto vt = vec3f{10.4, -2.2, 5.5};
    vt += 3.2;
    REQUIRE(vt.x() == Approx(13.6f));
    REQUIRE(vt.y() == Approx(1.0f));
    REQUIRE(vt.z() == Approx(8.7f));
    ;
  }
  {
    auto vt = vec3f{0} + vec3f{2.5, 0.3, 7.4};
    REQUIRE(vt.x() == Approx(2.5f));
    REQUIRE(vt.y() == Approx(0.3f));
    REQUIRE(vt.z() == Approx(7.4f));
  }
  {
    auto vt = vec3f{-1., 0, 1} + 0.2;
    REQUIRE(vt.x() == Approx(-0.8f));
    REQUIRE(vt.y() == Approx(0.2f));
    REQUIRE(vt.z() == Approx(1.2f));
  }
  {
    auto vt = 0.2 + vec3f{-1., 0, 1};
    REQUIRE(vt.x() == Approx(-0.8f));
    REQUIRE(vt.y() == Approx(0.2f));
    REQUIRE(vt.z() == Approx(1.2f));
  }
  {
    auto vt = vector_3d<float>{0} + vector_3d<double>{0};
    CHECK(vt.x() == Approx(0.));
    CHECK(vt.y() == Approx(0.));
    CHECK(vt.z() == Approx(0.));
  }
}

TEST_CASE("float_vector3_test subtraction")
{
  {
    auto vt = vec3f{0};
    vt -= vec3f(-10.4, 0.4, -15.4);
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = vec3f{1.5, 2.2, 8.9};
    vt -= 3.2;
    REQUIRE(vt.x() == Approx(-1.7f));
    REQUIRE(vt.y() == Approx(-1.f));
    REQUIRE(vt.z() == Approx(5.7f));
  }
  {
    auto vt = vec3f{1.5, 2.2, 8.9} - vec3f{0};
    REQUIRE(vt.x() == Approx(1.5f));
    REQUIRE(vt.y() == Approx(2.2f));
    REQUIRE(vt.z() == Approx(8.9f));
  }
  {
    auto vt = vec3f{2.5, -0.3, 7.4} - 1;
    REQUIRE(vt.x() == Approx(1.5f));
    REQUIRE(vt.y() == Approx(-1.3f));
    REQUIRE(vt.z() == Approx(6.4f));
  }
  {
    auto vt = 1u - vec3f{2.5, -0.3, 7.4};
    REQUIRE(vt.x() == Approx(-1.5f));
    REQUIRE(vt.y() == Approx(1.3f));
    REQUIRE(vt.z() == Approx(-6.4f));
  }
  {
    auto vt = vector_3d<float>{0} - vector_3d<double>{0};
    CHECK(vt.x() == Approx(0.));
    CHECK(vt.y() == Approx(0.));
    CHECK(vt.z() == Approx(0.));
  }
}

TEST_CASE("float_vector3_test multiplication")
{
  {
    auto vt = vec3f(-10.4, 0.4, -15.4);
    vt *= vec3f{-1};
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = vec3f(-10.4, 0.4, -15.4);
    vt *= -1l;
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = vec3f(-10.4, 0.4, -15.4) * vec3f{-1};
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = vec3f(-10.4, 0.4, -15.4) * -1.0;
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = -1 * vec3f(-10.4, 0.4, -15.4);
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
    REQUIRE(vt.z() == Approx(15.4f));
  }
  {
    auto vt = vector_3d<float>{0} * vector_3d<double>{0};
    CHECK(vt.x() == Approx(0.));
    CHECK(vt.y() == Approx(0.));
    CHECK(vt.z() == Approx(0.));
  }
}

TEST_CASE("float_vector3_test division")
{
  {
    auto vt = vec3f(-10.0, 0.5, 2);
    vt /= vec3f{2};
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
    REQUIRE(vt.z() == Approx(1.0f));
  }
  {
    auto vt = vec3f(-10.0, 0.5, 2);
    vt /= 2ull;
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
    REQUIRE(vt.z() == Approx(1.0f));
  }
  {
    auto vt = vec3f(-10.0, 0.5, 2) / vec3f{2};
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
    REQUIRE(vt.z() == Approx(1.0f));
  }
  {
    auto vt = vec3f(-10.0, 0.5, 2) / 2;
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
    REQUIRE(vt.z() == Approx(1.0f));
  }
  {
    auto vt = 2 / vec3f(-10.0, 0.5, 2);
    REQUIRE(vt.x() == Approx(-0.2f));
    REQUIRE(vt.y() == Approx(4.0f));
    REQUIRE(vt.z() == Approx(1.0f));
  }
  {
    auto vt = vector_3d<float>{0} / vector_3d<double>{1};
    CHECK(vt.x() == Approx(0.));
    CHECK(vt.y() == Approx(0.));
    CHECK(vt.z() == Approx(0.));
  }
}

TEST_CASE("float_vector3_test reflect")
{
  {
    auto d = vec3f{10, -2, 0};
    auto r = d.reflect({0, 1, 0});
    REQUIRE(r.x() == Approx(10.f));
    REQUIRE(r.y() == Approx(2.0f));
    REQUIRE(r.z() == Approx(0.0f));
  }
}

TEST_CASE("float_vector3_test to_unit")
{
  {
    auto vt = vec3f(-10.0, 0.5, 2) / 2;
    auto n = vt.to_unit();
    REQUIRE(n.magnitude() == Approx(1.0f));
  }
}


#include <MEL/Communications/MelShare.hpp>
#include <MEL/Core/Timer.hpp>
#include <MEL/Daq/Quanser/Q8Usb.hpp>
#include <MEL/Devices/VoltPaqX4.hpp>
#include <MEL/Logging/Log.hpp>
#include <MEL/Math/Functions.hpp>
#include <MEL/Core/Console.hpp>
#include <MEL/Utility/Options.hpp>
#include <MEL/Utility/System.hpp>
#include "OpenWrist.hpp"

//added libraries for implementing a filter
#include <MEL/Math/Butterworth.hpp>
#include <MEL/Math/Filter.hpp>

//added library for matrix math
#include <Eigen/Dense>
#include <iostream>

//added for keyboard interface
#include <MEL/Devices/Windows/Keyboard.hpp>

//added for data logging
#include <MEL/Logging/DataLogger.hpp>

using namespace mel;
using namespace Eigen;

// ctrl-c signal handler
ctrl_bool ctrlc(false);
bool handler(CtrlEvent event) {
    print("Ctrl+C Pressed");
    ctrlc = true;
    return true;
}

// this is my function for measuring the human's applied force
Vector3d getForce(Butterworth& lp_filter0, Butterworth& lp_filter1, Butterworth& lp_filter2, OpenWrist& ow) {

    VectorXd TORQUE_THRESHOLD(3);
    TORQUE_THRESHOLD << 0.3, 0.2, 0.3;

    Vector3d u_h(3);
    u_h(0) = -lp_filter0.update(ow[0].get_torque_sense());
    u_h(1) = -lp_filter1.update(ow[1].get_torque_sense());
    u_h(2) = -lp_filter2.update(ow[2].get_torque_sense());
    for (int index = 0; index < 3; index++) {
        if (std::abs(u_h(index)) <= TORQUE_THRESHOLD(index)) {
            u_h(index) = 0.0;
        }
        else if (u_h(index) > TORQUE_THRESHOLD(index)) {
            u_h(index) -= TORQUE_THRESHOLD(index);
        }
        else {
            u_h(index) += TORQUE_THRESHOLD(index);
        }
    }
    return u_h;

}

// this is my function for getting the desired pose
VectorXd getDesired(VectorXd theta, double t) {

    double a = theta(0);
    double b = theta(1);
    double c = theta(2);
    double d = theta(3);
    double e = theta(4);
    double f = theta(5);
    double g = theta(6);

    VectorXd q_d(3);
    q_d << c, a*mel::cos(g*t)*mel::cos(f) - b*mel::sin(g*t)*mel::sin(f) + d, a*mel::cos(g*t)*mel::sin(f) + b*mel::sin(g*t)*mel::cos(f) + e;

    VectorXd qdot_d(3);
    qdot_d << 0, -a*g*mel::sin(g*t)*mel::cos(f) - b*g*mel::cos(g*t)*mel::sin(f), -a*g*mel::sin(g*t)*mel::sin(f) + b*g*mel::cos(g*t)*mel::cos(f);

    VectorXd x_d(6);
    x_d << q_d, qdot_d;

    return x_d;

}

// this is my function for getting the Jacobian of the desired trajectory
MatrixXd getJacobian(VectorXd psi, VectorXd x_d, double t) {

    double DELTA = 0.01;
    int m = (int)psi.size();
    MatrixXd xdJacobian(6, m);

    for (int index = 0; index < m; index++) {

        VectorXd psi_p = psi;
        psi_p(index) += DELTA;

        VectorXd x_d_p = getDesired(psi_p, t);

        VectorXd partialxd(6);
        partialxd << 1 / DELTA * (x_d_p - x_d);
        xdJacobian.col(index) = partialxd;

    }
    return xdJacobian;

}

// this is my function for logging all the data
// log: time (1), mode (2), u_h (3-5), x (6-11), x_d (12-17), x_d_theta (18-23), psi (24-30), u_r (31-33)
void getData(std::vector<double>& row, double count, double mode, Vector3d u_h, std::vector<double>& x, VectorXd x_d, VectorXd x_d_theta, VectorXd psi, Vector3d u_r) {
    row[0] = count;
    row[1] = mode;
    row[2] = u_h(0);
    row[3] = u_h(1);
    row[4] = u_h(2);
    row[5] = x[0];
    row[6] = x[1];
    row[7] = x[2];
    row[8] = x[3];
    row[9] = x[4];
    row[10] = x[5];
    row[11] = x_d(0);
    row[12] = x_d(1);
    row[13] = x_d(2);
    row[14] = x_d(3);
    row[15] = x_d(4);
    row[16] = x_d(5);
    row[17] = x_d_theta(0);
    row[18] = x_d_theta(1);
    row[19] = x_d_theta(2);
    row[20] = x_d_theta(3);
    row[21] = x_d_theta(4);
    row[22] = x_d_theta(5);
    row[23] = psi(0);
    row[24] = psi(1);
    row[25] = psi(2);
    row[26] = psi(3);
    row[27] = psi(4);
    row[28] = psi(5);
    row[29] = psi(6);
    row[30] = u_r(0);
    row[31] = u_r(1);
    row[32] = u_r(2);
}

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

    // make options
    Options options("openwrist_q8usb.exe", "OpenWrist Q8 USB Demo");
    options.add_options()
        ("c,calibrate", "Calibrates the OpenWrist")
        ("l,learn", "Runs the proposed algorithm for changing the desired trajectory")
        ("s,shared", "Runs the shared control method which decreases control gains")
        ("d,deform", "Runs the local deformation method from PITD")
        ("i,impedance", "Runs impedance for training")
        ("h,help", "Prints this help message");

    auto result = options.parse(argc, argv);
    if (result.count("help") > 0) {
        print(options.help());
        return 0;
    }

    // register ctrl-c handler
    register_ctrl_handler(handler);

    // make Q8 USB that's configured for current control with VoltPAQ-X4
    QuanserOptions qoptions;
    qoptions.set_update_rate(QuanserOptions::UpdateRate::Fast);
    qoptions.set_analog_output_mode(0, QuanserOptions::AoMode::CurrentMode1, 0, 2.0, 20.0, 0, -1, 0, 1000);
    qoptions.set_analog_output_mode(1, QuanserOptions::AoMode::CurrentMode1, 0, 2.0, 20.0, 0, -1, 0, 1000);
    qoptions.set_analog_output_mode(2, QuanserOptions::AoMode::CurrentMode1, 0, 2.0, 20.0, 0, -1, 0, 1000);
    Q8Usb q8(qoptions);

    VoltPaqX4 vpx4(q8.DO[{ 0, 1, 2 }], q8.AO[{ 0, 1, 2 }], q8.DI[{0, 1, 2}], q8.AI[{ 0, 1, 2 }]);

    // create OpenWrist and bind Q8 channels to it
    OwConfiguration config(
        q8,
        q8.watchdog,
        q8.encoder[{ 0, 1, 2 }],
        vpx4.amplifiers
    );
    OpenWrist ow(config);

    // run calibration script
    if (result.count("calibrate") > 0) {
        ow.calibrate(ctrlc);
        disable_realtime();
        return 0;
    }

    // determine whether or not we want to log data
    int record_data;
    std::cout << "Should I save the data? (Press 1 if Yes) \n";
    std::cin >> record_data;

    // set time parameters for all methods
    int T = 1;
    double t_f = 35.0001;

    // initialize the desired trajectory for all methods
    VectorXd psi(7);
    //psi << 0.2, 0.2, 0, 0, 0, 0, 1; // Training
    //psi << 0.2, 0.1, 0, 0.2, 0.2, 0, 1; // TASK 1
    psi << 0.5, 0.0, 0.0, 0.0, 0.0, PI / 8, 1.5; // TASK 2
    //psi << 0.3, 0.3, -0.1, 0, 0, PI/8, 0.8; // TASK 3
    //psi << 0.3, 0, 0, 0.1, 0, 0, 1; // TASK 4

    // initialize the true desired trajectory for all methods
    VectorXd theta(7);
    //theta << 0.4, 0.3, 0.1, 0, 0, 0, 1; // Training
    //theta << 0.5, 0.5, 0.2, 0.0, 0.0, 0, 1; // TASK 1
    theta << 0.5, 0.0, 0.0, 0.0, 0.0, PI/8, 0.5; // TASK 2
    //theta << 0.3, 0.4, -0.1, 0.1, -0.1, PI / 8, 0.8; // TASK 3
    //theta << 0.5, 0, 0, 0.1, 0, 0, 1; // TASK 4

    // notification that the human can start for all methods
    int start_interaction = 0;

    // set up for unity
    MelShare ms_ow_position("ow_position");
    MelShare ms_ow_theta("ow_theta");
    MelShare ms_ow_interaction("ow_start");
    MelShare ms_ow_time("ow_time");
    std::vector<double> ow_position_data(3);
    std::vector<double> ow_theta_data(3);
    std::vector<double> ow_interaction_data(1);
    std::vector<double> ow_time_data(1);

    // construct the keyboard
    Keyboard mykeyboard;
    // create critically damped PD controller for each exis ( Nm/rad , Nm-s/rad )
    PdController pd0(0, 0);
    PdController pd1(0, 0);
    PdController pd2(0, 0);
    // construct filters
    Butterworth lp_filter0(2, 0.05, Butterworth::Lowpass);
    Butterworth lp_filter1(2, 0.05, Butterworth::Lowpass);
    Butterworth lp_filter2(2, 0.05, Butterworth::Lowpass);

    // construct the holder for state
    std::vector<double> x(6);

    int subj_num;
    if (record_data == 1) {
        // record the subject number for datalog
        std::cout << "What is the subject number? \n";
        std::cin >> subj_num;
    }
    // start my logger
    DataLogger mylogger;
    std::vector<double> row(33);
    std::fill(row.begin(), row.end(), 0.0);

    // enter proposed learning algorithm
    if (result.count("learn") > 0) {

        // set the learning rate parameters
        VectorXd alphavalues(7);
        alphavalues << 1, 1, 1, 1, 1, 1, 1;
        alphavalues *= 1.0;
        double LAMBDA = 0.0;

        // initialize values
        int interaction_mode = 2;
        double gain_interp = 0.0;
        double t = -T / 1000.0;
        double count = -T / 1000.0;
        int m = (int)psi.size();
        // create the alpha matrix
        MatrixXd alpha(m, m);
        alpha.setZero();
        alpha.diagonal() = alphavalues;
        // create the M matrix
        MatrixXd eye(3, 3);
        eye.setIdentity();
        MatrixXd M(6, 3);
        M << eye, LAMBDA * eye;

        // enable hardware
        q8.enable();
        ow.enable();
        // start watchdog and timer
        q8.watchdog.start();
        Timer timer(milliseconds(T));

        // enter control loop
        while (!ctrlc) {

            // kick watchdog
            q8.watchdog.kick();
            // update inputs
            q8.update_input();

            // get the current time
            t += T / 1000.0;
            count += T / 1000.0;
            if (count > t_f) {
                break;
            }

            // get the interaction mode
            if (mykeyboard.is_key_pressed(Key::Num1, false)) {
                interaction_mode = 1;
            }
            else if (mykeyboard.is_key_pressed(Key::Num2, false)) {
                interaction_mode = 2;
            }

            // perform learning method operations
            VectorXd x_d_theta = getDesired(theta, count);
            VectorXd x_d = getDesired(psi, t);
            VectorXd u_h = getForce(lp_filter0, lp_filter1, lp_filter2, ow);
            MatrixXd xdJacobian = getJacobian(psi, x_d, t);

            // get the robot's current state
            x[0] = ow[0].get_position();
            x[1] = ow[1].get_position();
            x[2] = ow[2].get_position();
            x[3] = ow[0].get_velocity();
            x[4] = ow[1].get_velocity();
            x[5] = ow[2].get_velocity();

            // get data for logging
            if (record_data == 1) {
                Vector3d u_r;
                u_r(0) = ow[0].get_torque_command();
                u_r(1) = ow[1].get_torque_command();
                u_r(2) = ow[2].get_torque_command();
                getData(row, count, interaction_mode, u_h, x, x_d, x_d_theta, psi, u_r);
            }

            // perform start up to avoid snap
            if (count < 5) {
                if (gain_interp < 0.25) {
                    gain_interp += 0.0001;
                    pd0.set_gains(gain_interp * 25, gain_interp * 1.15);
                    pd1.set_gains(gain_interp * 20, gain_interp * 1.00);
                    pd2.set_gains(gain_interp * 20, gain_interp * 0.25);
                }
            }
            else {

                // turn on the interaction notification
                start_interaction = 1;

                // update the parameter psi
                VectorXd psi_delta(m);
                psi_delta = T / 1000.0 * alpha * xdJacobian.transpose() * M * u_h;

                // if we are changing the shape
                if (interaction_mode == 1) {
                    psi.head(m - 1) += psi_delta.head(m - 1);
                }

                // if we are changing the timing
                else {
                    // put limits on the timing changes
                    if (psi_delta(m - 1) > 0 && psi(m - 1) < 2.0) {
                        psi_delta(m - 1) = 0.002;
                    }
                    else if (psi_delta(m - 1) < 0 && psi(m - 1) > 0.1) {
                        psi_delta(m - 1) = -0.002;
                    }
                    else {
                        psi_delta(m - 1) = 0;
                    }
                    // make locally invariant wrt timing
                    t = t * psi(m - 1) / (psi(m - 1) + psi_delta(m - 1));
                    psi.tail(1) += psi_delta.tail(1);
                }

            }

            // set the controller
            double torque0 = pd0.calculate(x_d(0), x[0], x_d(3), x[3]);
            double torque1 = pd1.calculate(x_d(1), x[1], x_d(4), x[4]);
            double torque2 = pd2.calculate(x_d(2), x[2], x_d(5), x[5]);
            // set the commanded torques
            ow[0].set_torque(torque0 + ow.compute_gravity_compensation(0));
            ow[1].set_torque(torque1 + ow.compute_gravity_compensation(1));
            ow[2].set_torque(torque2 + ow.compute_gravity_compensation(2));

            // pass to unity
            ow_position_data[0] = x[0];
            ow_position_data[1] = x[1];
            ow_position_data[2] = x[2];
            ms_ow_position.write_data(ow_position_data);
            ow_theta_data[0] = x_d_theta(0);
            ow_theta_data[1] = x_d_theta(1);
            ow_theta_data[2] = x_d_theta(2);
            ms_ow_theta.write_data(ow_theta_data);
            ow_interaction_data[0] = start_interaction;
            ms_ow_interaction.write_data(ow_interaction_data);
            ow_time_data[0] = count;
            ms_ow_time.write_data(ow_time_data);

            // check limits
            if (ow.any_limit_exceeded())
                ctrlc = true;
            // update outputs
            q8.update_output();
            if (record_data == 1) {
                mylogger.buffer(row);
            }
            // wait timer
            timer.wait();

        }

        // save the data
        if (record_data == 1) {
            std::string logger_name = "data_learn_" + std::to_string(subj_num) + "_task_2.csv";
            mylogger.save_data(logger_name, "/Users/MAHI/Desktop/Dylan's Data");
        }
        std::cout << "Done!";
        return 0;

    }

    // enter shared control code
    if (result.count("shared") > 0) {

        // initialize values
        bool grav_mode = false;
        int time_since_int = 0;
        double gain_interp = 0.0;
        double t = -T / 1000.0;
        double count = -T / 1000.0;

        // enable hardware
        q8.enable();
        ow.enable();

        // start watchdog and timer
        q8.watchdog.start();
        Timer timer(milliseconds(T));

        // enter control loop
        while (!ctrlc) {

            // kick watchdog
            q8.watchdog.kick();
            // update inputs
            q8.update_input();

            // get the current time
            t += T / 1000.0;
            count += T / 1000.0;
            if (count > t_f) {
                break;
            }

            // perform learning method operations
            VectorXd x_d_theta = getDesired(theta, t);
            VectorXd x_d = getDesired(psi, t);
            VectorXd u_h = getForce(lp_filter0, lp_filter1, lp_filter2, ow);

            // get the robot's current state
            x[0] = ow[0].get_position();
            x[1] = ow[1].get_position();
            x[2] = ow[2].get_position();
            x[3] = ow[0].get_velocity();
            x[4] = ow[1].get_velocity();
            x[5] = ow[2].get_velocity();

            // get data for logging
            if (record_data == 1) {
                Vector3d u_r;
                u_r(0) = ow[0].get_torque_command();
                u_r(1) = ow[1].get_torque_command();
                u_r(2) = ow[2].get_torque_command();
                getData(row, count, gain_interp, u_h, x, x_d, x_d_theta, psi, u_r);
            }

            // perform start up to avoid snap
            if (count < 5) {
                if (gain_interp < 0.25) {
                    gain_interp += 0.0001;
                    pd0.set_gains(gain_interp * 25, gain_interp * 1.15);
                    pd1.set_gains(gain_interp * 20, gain_interp * 1.00);
                    pd2.set_gains(gain_interp * 20, gain_interp * 0.25);
                }
            }
            else {

                // turn on the interaction notification
                start_interaction = 1;

                // if there is a human interaction
                if (u_h.norm() > 0.01) {
                    time_since_int = 0;
                    // enter gravity comp mode if not already
                    if (grav_mode == false) {
                        grav_mode = true;
                        gain_interp = 0.0;
                        pd0.set_gains(0.0, 0.0);
                        pd1.set_gains(0.0, 0.0);
                        pd2.set_gains(0.0, 0.0);
                    }
                }
                // if not a human interaction
                else {
                    time_since_int++;
                }
                // determine whether we should leave gravity comp mode
                if (time_since_int > 1000 && grav_mode) {
                    grav_mode = false;
                }
                // increase the gains if not in grav_comp mode
                if (gain_interp < 0.25 && grav_mode == false) {
                    gain_interp += 0.0001;
                    pd0.set_gains(gain_interp * 25, gain_interp * 1.15);
                    pd1.set_gains(gain_interp * 20, gain_interp * 1.00);
                    pd2.set_gains(gain_interp * 20, gain_interp * 0.25);
                }

            }

            // set the controller
            double torque0 = pd0.calculate(x_d(0), x[0], x_d(3), x[3]);
            double torque1 = pd1.calculate(x_d(1), x[1], x_d(4), x[4]);
            double torque2 = pd2.calculate(x_d(2), x[2], x_d(5), x[5]);
            // set the commanded torques
            ow[0].set_torque(torque0 + ow.compute_gravity_compensation(0));
            ow[1].set_torque(torque1 + ow.compute_gravity_compensation(1));
            ow[2].set_torque(torque2 + ow.compute_gravity_compensation(2));

            // pass to unity
            ow_position_data[0] = x[0];
            ow_position_data[1] = x[1];
            ow_position_data[2] = x[2];
            ms_ow_position.write_data(ow_position_data);
            ow_theta_data[0] = x_d_theta(0);
            ow_theta_data[1] = x_d_theta(1);
            ow_theta_data[2] = x_d_theta(2);
            ms_ow_theta.write_data(ow_theta_data);
            ow_interaction_data[0] = start_interaction;
            ms_ow_interaction.write_data(ow_interaction_data);
            ow_time_data[0] = count;
            ms_ow_time.write_data(ow_time_data);

            // check limits
            if (ow.any_limit_exceeded())
                ctrlc = true;
            // update outputs
            q8.update_output();
            if (record_data == 1) {
                mylogger.buffer(row);
            }
            // wait timer
            timer.wait();

        }

        // save the data
        if (record_data == 1) {
            std::string logger_name = "data_share_" + std::to_string(subj_num) + "_task_2.csv";
            mylogger.save_data(logger_name, "/Users/MAHI/Desktop/Dylan's Data");
        }
        std::cout << "Done!";
        return 0;

    }

    // enter deform control code
    if (result.count("deform") > 0) {

        // set the deform parameters
        double tau = 1.0;
        double mu = 0.001;

        // precompute the deformation shape
        int r = (int)floor(tau / T * 1000.0);
        MatrixXd A = MatrixXd::Zero(r + 3, r);
        for (int i = 0; i < r; i++) {
            A(i, i) = 1;
            A(i + 1, i) = -3;
            A(i + 2, i) = 3;
            A(i + 3, i) = -1;
        }
        MatrixXd R = A.transpose()*A;
        MatrixXd H = R.inverse() * MatrixXd::Ones(r, 1);
        H /= H.maxCoeff();
        // create the local desired trajectory
        MatrixXd gamma_d(r, 3);
        for (int i = 0; i < r; i++) {
            double t = i / 1000.0;
            VectorXd x_d = getDesired(psi, t);
            gamma_d.row(i) = x_d.head(3);
        }
        // create the dummy matrix for gamma_d
        MatrixXd dummy_gamma_d(r, 3);

        // initialize values
        double gain_interp = 0.0;
        double t = -T / 1000.0;
        double count = -T / 1000.0;

        // enable hardware
        q8.enable();
        ow.enable();

        // start watchdog and timer
        q8.watchdog.start();
        Timer timer(milliseconds(T));

        // enter control loop
        while (!ctrlc) {

            // kick watchdog
            q8.watchdog.kick();
            // update inputs
            q8.update_input();

            // get the current time
            t += T / 1000.0;
            count += T / 1000.0;
            if (count > t_f) {
                break;
            }

            // perform learning method operations
            VectorXd x_d_theta = getDesired(theta, t);
            VectorXd u_h = getForce(lp_filter0, lp_filter1, lp_filter2, ow);

            // get the desired position
            VectorXd q_d(3);
            q_d = gamma_d.row(0);
            // get the desired velocity
            VectorXd q_d_next(3);
            VectorXd qdot_d(3);
            q_d_next = gamma_d.row(1);
            qdot_d = (1000.0 / T) * (q_d_next - q_d);
            // get the desired state
            VectorXd x_d(6);
            x_d << q_d, qdot_d;

            // get the robot's current state
            x[0] = ow[0].get_position();
            x[1] = ow[1].get_position();
            x[2] = ow[2].get_position();
            x[3] = ow[0].get_velocity();
            x[4] = ow[1].get_velocity();
            x[5] = ow[2].get_velocity();

            // get data for logging
            if (record_data == 1) {
                Vector3d u_r;
                u_r(0) = ow[0].get_torque_command();
                u_r(1) = ow[1].get_torque_command();
                u_r(2) = ow[2].get_torque_command();
                getData(row, count, gain_interp, u_h, x, x_d, x_d_theta, psi, u_r);
            }

            // perform start up to avoid snap
            if (count < 5) {
                if (gain_interp < 0.25) {
                    gain_interp += 0.0001;
                    pd0.set_gains(gain_interp * 25, gain_interp * 1.15);
                    pd1.set_gains(gain_interp * 20, gain_interp * 1.00);
                    pd2.set_gains(gain_interp * 20, gain_interp * 0.25);
                }
            }
            else {

                // turn on the interaction notification
                start_interaction = 1;

                // deform the robot's desired trajectory
                gamma_d.col(0) += mu * H * u_h(0);
                gamma_d.col(1) += mu * H * u_h(1);
                gamma_d.col(2) += mu * H * u_h(2);

            }

            // create placeholder for updates desired trajectory
            for (int i = 0; i < r - 1; i++) {
                dummy_gamma_d.row(i) = gamma_d.row(i + 1);
            }
            // append the next point along the original desired trajectory
            VectorXd x_d_new = getDesired(psi, t + T / 1000.0 + tau);
            dummy_gamma_d.row(r - 1) = x_d_new.head(3);
            gamma_d = dummy_gamma_d;

            // set the controller
            double torque0 = pd0.calculate(x_d(0), x[0], x_d(3), x[3]);
            double torque1 = pd1.calculate(x_d(1), x[1], x_d(4), x[4]);
            double torque2 = pd2.calculate(x_d(2), x[2], x_d(5), x[5]);
            // set the commanded torques
            ow[0].set_torque(torque0 + ow.compute_gravity_compensation(0));
            ow[1].set_torque(torque1 + ow.compute_gravity_compensation(1));
            ow[2].set_torque(torque2 + ow.compute_gravity_compensation(2));

            // pass to unity
            ow_position_data[0] = x[0];
            ow_position_data[1] = x[1];
            ow_position_data[2] = x[2];
            ms_ow_position.write_data(ow_position_data);
            ow_theta_data[0] = x_d_theta(0);
            ow_theta_data[1] = x_d_theta(1);
            ow_theta_data[2] = x_d_theta(2);
            ms_ow_theta.write_data(ow_theta_data);
            ow_interaction_data[0] = start_interaction;
            ms_ow_interaction.write_data(ow_interaction_data);
            ow_time_data[0] = count;
            ms_ow_time.write_data(ow_time_data);

            // check limits
            if (ow.any_limit_exceeded())
                ctrlc = true;
            // update outputs
            q8.update_output();
            if (record_data == 1) {
                mylogger.buffer(row);
            }
            // wait timer
            timer.wait();

        }

        // save the data
        if (record_data == 1) {
            std::string logger_name = "data_deform_" + std::to_string(subj_num) + "_task_2.csv";
            mylogger.save_data(logger_name, "/Users/MAHI/Desktop/Dylan's Data");
        }
        std::cout << "Done!";
        return 0;

    }

    // enter impedance code
    if (result.count("impedance") > 0) {

        // initialize values
        double gain_interp = 0.0;
        double t = -T / 1000.0;
        double count = -T / 1000.0;

        // enable hardware
        q8.enable();
        ow.enable();

        // start watchdog and timer
        q8.watchdog.start();
        Timer timer(milliseconds(T));

        // enter control loop
        while (!ctrlc) {

            // kick watchdog
            q8.watchdog.kick();
            // update inputs
            q8.update_input();

            // get the current time
            t += T / 1000.0;
            count += T / 1000.0;
            if (count > t_f) {
                break;
            }

            // perform learning method operations
            VectorXd x_d_theta = getDesired(theta, t);
            VectorXd x_d = getDesired(psi, t);

            // get the robot's current state
            x[0] = ow[0].get_position();
            x[1] = ow[1].get_position();
            x[2] = ow[2].get_position();
            x[3] = ow[0].get_velocity();
            x[4] = ow[1].get_velocity();
            x[5] = ow[2].get_velocity();

            // perform start up to avoid snap
            if (count < 5) {
                if (gain_interp < 0.25) {
                    gain_interp += 0.0001;
                    pd0.set_gains(gain_interp * 25, gain_interp * 1.15);
                    pd1.set_gains(gain_interp * 20, gain_interp * 1.00);
                    pd2.set_gains(gain_interp * 20, gain_interp * 0.25);
                }
            }
            else {
                // turn on the interaction notification
                start_interaction = 1;
            }

            // set the controller
            double torque0 = pd0.calculate(x_d(0), x[0], x_d(3), x[3]);
            double torque1 = pd1.calculate(x_d(1), x[1], x_d(4), x[4]);
            double torque2 = pd2.calculate(x_d(2), x[2], x_d(5), x[5]);
            // set the commanded torques
            ow[0].set_torque(torque0 + ow.compute_gravity_compensation(0));
            ow[1].set_torque(torque1 + ow.compute_gravity_compensation(1));
            ow[2].set_torque(torque2 + ow.compute_gravity_compensation(2));

            // pass to unity
            ow_position_data[0] = x[0];
            ow_position_data[1] = x[1];
            ow_position_data[2] = x[2];
            ms_ow_position.write_data(ow_position_data);
            ow_theta_data[0] = x_d_theta(0);
            ow_theta_data[1] = x_d_theta(1);
            ow_theta_data[2] = x_d_theta(2);
            ms_ow_theta.write_data(ow_theta_data);
            ow_interaction_data[0] = start_interaction;
            ms_ow_interaction.write_data(ow_interaction_data);
            ow_time_data[0] = count;
            ms_ow_time.write_data(ow_time_data);

            // check limits
            if (ow.any_limit_exceeded())
                ctrlc = true;
            // update outputs
            q8.update_output();
            // wait timer
            timer.wait();

        }

        std::cout << "Done!";
        return 0;

    }

    // disable Windows realtime
    disable_realtime();
    return 0;

}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/list.hpp>

#include <boost/hana/integral.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto out = [](auto x) {
    return [=](auto y) { return not_equal(x, y); };
};

// filter requires the predicate to return an Integral boolean, so we need
// the comparison to be purely compile-time.
template <int i>
BOOST_HANA_CONSTEXPR_LAMBDA auto x = int_<i>;

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto z = x<999>;

    BOOST_HANA_CONSTANT_ASSERT(filter(list(), out(z)) == list());

    BOOST_HANA_CONSTANT_ASSERT(filter(list(z), out(z)) == list());
    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>), out(z)) == list(x<1>));

    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>, x<2>), out(z)) == list(x<1>, x<2>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(z, x<2>), out(z)) == list(x<2>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>, z), out(z)) == list(x<1>));

    BOOST_HANA_CONSTANT_ASSERT(filter(list(z, x<2>, x<3>), out(z)) == list(x<2>, x<3>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>, z, x<3>), out(z)) == list(x<1>, x<3>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>, x<2>, z), out(z)) == list(x<1>, x<2>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(x<1>, z, z), out(z)) == list(x<1>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(z, x<2>, z), out(z)) == list(x<2>));
    BOOST_HANA_CONSTANT_ASSERT(filter(list(z, z, x<3>), out(z)) == list(x<3>));
}

int main() {
    test<List::mcd<void>>();
    (void)out;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps PinholeCamera classes to python
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include "gtsam/geometry/PinholeCamera.h"
#include "gtsam/geometry/Cal3_S2.h"

using namespace boost::python;
using namespace gtsam;

typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;

// Wrapper on PinholeBaseK<Cal3_S2> because it has pure virtual method calibration()
struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCal3_S2>
{
  const Cal3_S2 & calibration () const {
    return this->get_override("calibration")();
  }
};

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 1, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3)

// Function pointers to disambiguate project() calls
Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
Point2  (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;

// function pointers to disambiguate range() calls
double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range;
double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose,   OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose)  const = &PinholeBaseKCal3_S2::range;
double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range;

// wrap projectSafe in a function that returns None or a tuple
// TODO(frank): find out how to return an ndarray instead
object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) {
  auto result = camera.projectSafe(p);
  if (result.second)
    return make_tuple(result.first.x(), result.first.y());
  else
    return object();
}

void exportPinholeBaseK() {
  class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
      .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration),
           return_value_policy<copy_const_reference>())
      .def("project_safe", make_function(project_safe))
      .def("project", project1, project_overloads())
      .def("project", project2, project_overloads())
      .def("backproject", &PinholeBaseKCal3_S2::backproject)
      .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
      .def("range", range1, range_overloads())
      .def("range", range2, range_overloads())
      .def("range", range3, range_overloads());
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 2> test_int_6_2(const Eigen::Matrix<int, 6, 2> & M)
{
	return M;
}
void export_int_6_2()
{
	boost::python::def("test_int_6_2",test_int_6_2);
}