#include "ackermann_ekf/ackermann_ekf.h"

#include <boost/algorithm/clamp.hpp>
#include <math.h>
#include <vector>

namespace ackermann_ekf {

AckermannEkf::AckermannEkf(const Eigen::VectorXd x_min,
                           const Eigen::VectorXd x_max, double wheelbase,
                           double control_acceleration_gain,
                           double max_control_acceleration,
                           double control_angle_speed_gain,
                           double max_control_angle_speed)
    : x(STATE_SIZE),
      P(STATE_SIZE, STATE_SIZE),
      Q(STATE_SIZE, STATE_SIZE),
      x_min_(x_min),
      x_max_(x_max),
      wheelbase_(wheelbase),
      control_acceleration_gain_(control_acceleration_gain),
      max_control_acceleration_(max_control_acceleration),
      control_angle_speed_gain_(control_angle_speed_gain),
      max_control_angle_speed_(max_control_angle_speed) {
    // Initialize the state to zeros, this is compensated for with a high
    // initial state covariance (P), see below
    // \todo Read initial state vector from a parameter
    x.setZero();

    // Set the initial state covariance to a constant matrix
    // \todo Read the initial state covariance matrix from a parameter
    P.setIdentity();
    P *= 1e1;

    // Set the process noise covariance to a constant matrix
    // \todo Read the process noise covariance matrix from a parameter
    Q.setZero();
    Q(State::X, State::X) = 1e-3;
    Q(State::Y, State::Y) = 1e-3;
    Q(State::Z, State::Z) = 1e-4;
    Q(State::speed, State::speed) = 1e-0;
    Q(State::accel, State::accel) = 1e+1;
    Q(State::Roll, State::Roll) = 1e-4;
    Q(State::Pitch, State::Pitch) = 1e-4;
    Q(State::Yaw, State::Yaw) = 1e-3;
    Q(State::droll_dx, State::droll_dx) = 1e-6;
    Q(State::dpitch_dx, State::dpitch_dx) = 1e-6;
    Q(State::dyaw_dx, State::dyaw_dx) = 1e+0;
}

void AckermannEkf::process_measurement(const Measurement &measurement) {
    bring_time_forward_to(measurement.time);

    correct(measurement);
}

void AckermannEkf::process_control_signal(const ControlSignal &control_signal) {
    bring_time_forward_to(control_signal.time);

    // Enable control signals for prediction step as soon as one is recieved
    control_signal_enabled_ = true;
    control_signal_ = control_signal;
}

void AckermannEkf::bring_time_forward_to(double time) {
    if (time > this->time) {
        // Only predict if the filter time has been initialized, otherwise dt
        // may be VERY large
        if (this->time >= 0) {
            predict(time - this->time);
        }
        this->time = time;
    }
}

void AckermannEkf::predict(double dt) {
    if (control_signal_enabled_) {
        // Accelration is linear gain on velocity error
        x(State::accel) = boost::algorithm::clamp(
            (control_signal_.u(ControlSignal::speed) - x(State::speed)) *
                control_acceleration_gain_,
            -max_control_acceleration_, max_control_acceleration_);

        double steering_angle = atan(wheelbase_ * x(State::dyaw_dx));
        // Add (steering angle speed) * dt to steering angle, where steering
        // angle speed is linear gain on steering angle error
        steering_angle +=
            boost::algorithm::clamp(
                (control_signal_.u(ControlSignal::angle) - steering_angle) *
                    control_angle_speed_gain_,
                -max_control_angle_speed_, max_control_angle_speed_) *
            dt;
        steering_angle = boost::algorithm::clamp(steering_angle, -M_PI * 0.499,
                                                 M_PI * 0.499);

        x(State::dyaw_dx) = tan(steering_angle) / wheelbase_;

        constrain_state();
    }

    Eigen::VectorXd f(STATE_SIZE);
    Eigen::MatrixXd F(STATE_SIZE, STATE_SIZE);
    f.setZero();
    F.setZero();

    // clang-format off

    // The code here has been autogenerated by matlab and manually pasted in.
    // Do not edit directly!

    f(State::X) = x(State::X)-((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll))))-x(State::accel)*cos(x(State::Pitch))*cos(x(State::Yaw))))/2.0+dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Yaw));
    f(State::Y) = x(State::Y)+((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))+x(State::accel)*cos(x(State::Pitch))*sin(x(State::Yaw))))/2.0+dt*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Yaw));
    f(State::Z) = x(State::Z)-((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Roll))-x(State::dyaw_dx)*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Roll)))+x(State::accel)*sin(x(State::Pitch))))/2.0-dt*x(State::speed)*sin(x(State::Pitch));
    f(State::speed) = x(State::speed)+x(State::accel)*dt;
    f(State::accel) = x(State::accel);
    f(State::Roll) = (x(State::Roll)*cos(x(State::Pitch))+x(State::droll_dx)*dt*x(State::speed)*cos(x(State::Pitch))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    f(State::Pitch) = x(State::Pitch)+dt*x(State::speed)*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll)));
    f(State::Yaw) = (x(State::Yaw)*cos(x(State::Pitch))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Roll)))/cos(x(State::Pitch));
    f(State::droll_dx) = x(State::droll_dx);
    f(State::dpitch_dx) = x(State::dpitch_dx);
    f(State::dyaw_dx) = x(State::dyaw_dx);

    F(State::X, State::X) = 1.0;
    F(State::X, State::speed) = -dt*(-cos(x(State::Pitch))*cos(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Roll))*sin(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch))-dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)));
    F(State::X, State::accel) = ((dt*dt)*cos(x(State::Pitch))*cos(x(State::Yaw)))/2.0;
    F(State::X, State::Roll) = (dt*dt)*x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))-x(State::dyaw_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch))))*(-1.0/2.0);
    F(State::X, State::Pitch) = dt*cos(x(State::Yaw))*(x(State::speed)*sin(x(State::Pitch))*2.0+x(State::accel)*dt*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::X, State::Yaw) = (dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))+x(State::accel)*cos(x(State::Pitch))*sin(x(State::Yaw)))*(-1.0/2.0)-dt*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Yaw));
    F(State::X, State::dpitch_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))*(-1.0/2.0);
    F(State::X, State::dyaw_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::Y, State::Y) = 1.0;
    F(State::Y, State::speed) = dt*(cos(x(State::Pitch))*sin(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*cos(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Yaw))*sin(x(State::Roll))-x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)));
    F(State::Y, State::accel) = ((dt*dt)*cos(x(State::Pitch))*sin(x(State::Yaw)))/2.0;
    F(State::Y, State::Roll) = ((dt*dt)*x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))-x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))))/2.0;
    F(State::Y, State::Pitch) = dt*sin(x(State::Yaw))*(x(State::speed)*sin(x(State::Pitch))*2.0+x(State::accel)*dt*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::Y, State::Yaw) = (dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll))))-x(State::accel)*cos(x(State::Pitch))*cos(x(State::Yaw)))*(-1.0/2.0)+dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Yaw));
    F(State::Y, State::dpitch_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw))))/2.0;
    F(State::Y, State::dyaw_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))/2.0;
    F(State::Z, State::Z) = 1.0;
    F(State::Z, State::speed) = -dt*(sin(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Roll)));
    F(State::Z, State::accel) = (dt*dt)*sin(x(State::Pitch))*(-1.0/2.0);
    F(State::Z, State::Roll) = ((dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll))))/2.0;
    F(State::Z, State::Pitch) = (dt*dt)*(x(State::accel)*cos(x(State::Pitch))-x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))-x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll))))*(-1.0/2.0)-dt*x(State::speed)*cos(x(State::Pitch));
    F(State::Z, State::dpitch_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))*(-1.0/2.0);
    F(State::Z, State::dyaw_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))/2.0;
    F(State::speed, State::speed) = 1.0;
    F(State::speed, State::accel) = dt;
    F(State::accel, State::accel) = 1.0;
    F(State::Roll, State::speed) = (dt*(x(State::droll_dx)*cos(x(State::Pitch))+x(State::dyaw_dx)*cos(x(State::Roll))*sin(x(State::Pitch))+x(State::dpitch_dx)*sin(x(State::Pitch))*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Roll, State::Roll) = (cos(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))-dt*x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Roll, State::Pitch) = dt*x(State::speed)*1.0/pow(cos(x(State::Pitch)),2.0)*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Roll, State::droll_dx) = dt*x(State::speed);
    F(State::Roll, State::dpitch_dx) = (dt*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Roll, State::dyaw_dx) = (dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch)))/cos(x(State::Pitch));
    F(State::Pitch, State::speed) = dt*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll)));
    F(State::Pitch, State::Roll) = -dt*x(State::speed)*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Pitch, State::Pitch) = 1.0;
    F(State::Pitch, State::dpitch_dx) = dt*x(State::speed)*cos(x(State::Roll));
    F(State::Pitch, State::dyaw_dx) = -dt*x(State::speed)*sin(x(State::Roll));
    F(State::Yaw, State::speed) = (dt*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Yaw, State::Roll) = (dt*x(State::speed)*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Yaw, State::Pitch) = dt*x(State::speed)*1.0/pow(cos(x(State::Pitch)),2.0)*sin(x(State::Pitch))*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Yaw, State::Yaw) = 1.0;
    F(State::Yaw, State::dpitch_dx) = (dt*x(State::speed)*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Yaw, State::dyaw_dx) = (dt*x(State::speed)*cos(x(State::Roll)))/cos(x(State::Pitch));
    F(State::droll_dx, State::droll_dx) = 1.0;
    F(State::dpitch_dx, State::dpitch_dx) = 1.0;
    F(State::dyaw_dx, State::dyaw_dx) = 1.0;

    // clang-format on

    // Kalman prediction equations according to wikipeadia on EKF
    x = f;
    constrain_state();
    P = F * P * F.transpose() + dt * Q;
}

void AckermannEkf::correct(const Measurement &measurement) {
    Eigen::VectorXd h(MEASUREMENT_SIZE);
    Eigen::MatrixXd H(MEASUREMENT_SIZE, STATE_SIZE);
    h.setZero();
    H.setZero();

    // clang-format off

    // The code here has been autogenerated by matlab and manually pasted in.
    // Do not edit directly!

    h(Measurement::X) = x(State::X)-measurement.sensor_position(1)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))+measurement.sensor_position(2)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(0)*cos(x(State::Pitch))*cos(x(State::Yaw));
    h(Measurement::Y) = x(State::Y)+measurement.sensor_position(1)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))-measurement.sensor_position(2)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+measurement.sensor_position(0)*cos(x(State::Pitch))*sin(x(State::Yaw));
    h(Measurement::Z) = x(State::Z)-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll));
    h(Measurement::dx_dt) = x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0);
    h(Measurement::dy_dt) = -x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(0));
    h(Measurement::dz_dt) = -x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)-x(State::droll_dx)*measurement.sensor_position(1));
    h(Measurement::d2x_dt2) = x(State::accel)-measurement.gravity*sin(x(State::Pitch))+x(State::accel)*x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::accel)*x(State::dyaw_dx)*measurement.sensor_position(1)-x(State::dpitch_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)*x(State::speed)-x(State::droll_dx)*measurement.sensor_position(1)*x(State::speed))+x(State::dyaw_dx)*x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(0)*x(State::speed));
    h(Measurement::d2y_dt2) = x(State::dyaw_dx)*(x(State::speed)*x(State::speed))+measurement.gravity*cos(x(State::Pitch))*sin(x(State::Roll))-x(State::accel)*x(State::droll_dx)*measurement.sensor_position(2)+x(State::accel)*x(State::dyaw_dx)*measurement.sensor_position(0)+x(State::droll_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)*x(State::speed)-x(State::droll_dx)*measurement.sensor_position(1)*x(State::speed))+x(State::dyaw_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(1)*x(State::speed));
    h(Measurement::d2z_dt2) = -x(State::dpitch_dx)*(x(State::speed)*x(State::speed))+measurement.gravity*cos(x(State::Pitch))*cos(x(State::Roll))-x(State::accel)*x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::accel)*x(State::droll_dx)*measurement.sensor_position(1)-x(State::dpitch_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(1)*x(State::speed))-x(State::droll_dx)*x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(0)*x(State::speed));
    h(Measurement::Roll) = x(State::Roll);
    h(Measurement::Pitch) = x(State::Pitch);
    h(Measurement::Yaw) = x(State::Yaw);
    h(Measurement::droll_dt) = x(State::droll_dx)*x(State::speed);
    h(Measurement::dpitch_dt) = x(State::dpitch_dx)*x(State::speed);
    h(Measurement::dyaw_dt) = x(State::dyaw_dx)*x(State::speed);

    H(Measurement::X, State::X) = 1.0;
    H(Measurement::X, State::Roll) = measurement.sensor_position(1)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(2)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::X, State::Pitch) = cos(x(State::Yaw))*(-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::X, State::Yaw) = -measurement.sensor_position(1)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))+measurement.sensor_position(2)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))-measurement.sensor_position(0)*cos(x(State::Pitch))*sin(x(State::Yaw));
    H(Measurement::Y, State::Y) = 1.0;
    H(Measurement::Y, State::Roll) = -measurement.sensor_position(1)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))-measurement.sensor_position(2)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)));
    H(Measurement::Y, State::Pitch) = sin(x(State::Yaw))*(-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::Y, State::Yaw) = -measurement.sensor_position(1)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))+measurement.sensor_position(2)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(0)*cos(x(State::Pitch))*cos(x(State::Yaw));
    H(Measurement::Z, State::Z) = 1.0;
    H(Measurement::Z, State::Roll) = cos(x(State::Pitch))*(measurement.sensor_position(1)*cos(x(State::Roll))-measurement.sensor_position(2)*sin(x(State::Roll)));
    H(Measurement::Z, State::Pitch) = -measurement.sensor_position(0)*cos(x(State::Pitch))-measurement.sensor_position(2)*cos(x(State::Roll))*sin(x(State::Pitch))-measurement.sensor_position(1)*sin(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::dx_dt, State::speed) = x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0;
    H(Measurement::dx_dt, State::dpitch_dx) = measurement.sensor_position(2)*x(State::speed);
    H(Measurement::dx_dt, State::dyaw_dx) = -measurement.sensor_position(1)*x(State::speed);
    H(Measurement::dy_dt, State::speed) = -x(State::droll_dx)*measurement.sensor_position(2)+x(State::dyaw_dx)*measurement.sensor_position(0);
    H(Measurement::dy_dt, State::droll_dx) = -measurement.sensor_position(2)*x(State::speed);
    H(Measurement::dy_dt, State::dyaw_dx) = measurement.sensor_position(0)*x(State::speed);
    H(Measurement::dz_dt, State::speed) = -x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::droll_dx)*measurement.sensor_position(1);
    H(Measurement::dz_dt, State::droll_dx) = measurement.sensor_position(1)*x(State::speed);
    H(Measurement::dz_dt, State::dpitch_dx) = -measurement.sensor_position(0)*x(State::speed);
    H(Measurement::d2x_dt2, State::speed) = x(State::speed)*((x(State::dpitch_dx)*x(State::dpitch_dx))*measurement.sensor_position(0)+(x(State::dyaw_dx)*x(State::dyaw_dx))*measurement.sensor_position(0)-x(State::dpitch_dx)*x(State::droll_dx)*measurement.sensor_position(1)-x(State::droll_dx)*x(State::dyaw_dx)*measurement.sensor_position(2))*-2.0;
    H(Measurement::d2x_dt2, State::accel) = x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0;
    H(Measurement::d2x_dt2, State::Pitch) = -measurement.gravity*cos(x(State::Pitch));
    H(Measurement::d2x_dt2, State::droll_dx) = (x(State::speed)*x(State::speed))*(x(State::dpitch_dx)*measurement.sensor_position(1)+x(State::dyaw_dx)*measurement.sensor_position(2));
    H(Measurement::d2x_dt2, State::dpitch_dx) = x(State::accel)*measurement.sensor_position(2)-x(State::dpitch_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))*2.0+x(State::droll_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed));
    H(Measurement::d2x_dt2, State::dyaw_dx) = -x(State::accel)*measurement.sensor_position(1)+x(State::droll_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))-x(State::dyaw_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2y_dt2, State::speed) = x(State::speed)*(x(State::dyaw_dx)-(x(State::droll_dx)*x(State::droll_dx))*measurement.sensor_position(1)-(x(State::dyaw_dx)*x(State::dyaw_dx))*measurement.sensor_position(1)+x(State::dpitch_dx)*x(State::droll_dx)*measurement.sensor_position(0)+x(State::dpitch_dx)*x(State::dyaw_dx)*measurement.sensor_position(2))*2.0;
    H(Measurement::d2y_dt2, State::accel) = -x(State::droll_dx)*measurement.sensor_position(2)+x(State::dyaw_dx)*measurement.sensor_position(0);
    H(Measurement::d2y_dt2, State::Roll) = measurement.gravity*cos(x(State::Pitch))*cos(x(State::Roll));
    H(Measurement::d2y_dt2, State::Pitch) = -measurement.gravity*sin(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::d2y_dt2, State::droll_dx) = -x(State::accel)*measurement.sensor_position(2)+x(State::dpitch_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))-x(State::droll_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2y_dt2, State::dpitch_dx) = (x(State::speed)*x(State::speed))*(x(State::droll_dx)*measurement.sensor_position(0)+x(State::dyaw_dx)*measurement.sensor_position(2));
    H(Measurement::d2y_dt2, State::dyaw_dx) = x(State::accel)*measurement.sensor_position(0)+x(State::speed)*x(State::speed)+x(State::dpitch_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))-x(State::dyaw_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2z_dt2, State::speed) = x(State::speed)*(x(State::dpitch_dx)+(x(State::dpitch_dx)*x(State::dpitch_dx))*measurement.sensor_position(2)+(x(State::droll_dx)*x(State::droll_dx))*measurement.sensor_position(2)-x(State::dpitch_dx)*x(State::dyaw_dx)*measurement.sensor_position(1)-x(State::droll_dx)*x(State::dyaw_dx)*measurement.sensor_position(0))*-2.0;
    H(Measurement::d2z_dt2, State::accel) = -x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::droll_dx)*measurement.sensor_position(1);
    H(Measurement::d2z_dt2, State::Roll) = -measurement.gravity*cos(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::d2z_dt2, State::Pitch) = -measurement.gravity*cos(x(State::Roll))*sin(x(State::Pitch));
    H(Measurement::d2z_dt2, State::droll_dx) = x(State::accel)*measurement.sensor_position(1)-x(State::droll_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))*2.0+x(State::dyaw_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed));
    H(Measurement::d2z_dt2, State::dpitch_dx) = -x(State::accel)*measurement.sensor_position(0)-x(State::speed)*x(State::speed)-x(State::dpitch_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))*2.0+x(State::dyaw_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed));
    H(Measurement::d2z_dt2, State::dyaw_dx) = (x(State::speed)*x(State::speed))*(x(State::dpitch_dx)*measurement.sensor_position(1)+x(State::droll_dx)*measurement.sensor_position(0));
    H(Measurement::Roll, State::Roll) = 1.0;
    H(Measurement::Pitch, State::Pitch) = 1.0;
    H(Measurement::Yaw, State::Yaw) = 1.0;
    H(Measurement::droll_dt, State::speed) = x(State::droll_dx);
    H(Measurement::droll_dt, State::droll_dx) = x(State::speed);
    H(Measurement::dpitch_dt, State::speed) = x(State::dpitch_dx);
    H(Measurement::dpitch_dt, State::dpitch_dx) = x(State::speed);
    H(Measurement::dyaw_dt, State::speed) = x(State::dyaw_dx);
    H(Measurement::dyaw_dt, State::dyaw_dx) = x(State::speed);

    // clang-format on

    // Find the indicies of values to include in correction step
    std::vector<int> measurement_indices;
    for (int i = 0; i < MEASUREMENT_SIZE; i++) {
        if (measurement.mask[i]) {
            measurement_indices.push_back(i);
        }
    }

    // Create subsets of z, R, h and H from the indicies of given by the
    // measurement mask. There is no need to work with e.g. the full h vector
    // if only two out of MEASUREMENT_SIZE components are actually used
    // By creating this subset we e.g. in this example only have to invert a 2x2
    // matrix as opposed to a MEASUREMENT_SIZExMEASUREMENT_SIZE one
    Eigen::MatrixXd R(measurement_indices.size(), measurement_indices.size());
    Eigen::VectorXd y(measurement_indices.size());
    Eigen::MatrixXd H_subset(measurement_indices.size(), STATE_SIZE);
    for (int i_ = 0; i_ < measurement_indices.size(); i_++) {
        int i = measurement_indices[i_];

        // Subset h and z (and y)
        y(i_) = measurement.z(i) - h(i);

        // Wrap Yaw error (a == 2*pi*n + a)
        if (i_ == Measurement::Yaw) {
            y(i_) -= round(y(i_) / (2 * M_PI)) * 2 * M_PI;
        }

        // Subset R
        for (int j_ = 0; j_ < measurement_indices.size(); j_++) {
            int j = measurement_indices[j_];

            R(i_, j_) = measurement.R(i, j);
        }

        // Subset H
        for (int j = 0; j < STATE_SIZE; j++) { H_subset(i_, j) = H(i, j); }
    }

    // Kalman correction equations according to wikipeadia on EKF
    Eigen::MatrixXd K = P * H_subset.transpose() *
                        (H_subset * P * H_subset.transpose() + R).inverse();
    x.noalias() += K * y;
    constrain_state();
    P = (Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE) - K * H_subset) * P;
}

void AckermannEkf::constrain_state() {
    x.noalias() = x.cwiseMax(x_min_).cwiseMin(x_max_);
}

} // namespace ackermann_ekf


/*====================================================================
 * Simple pendulum example
 * Copyright (c) 2015 Matthew Millard
 * <matthew.millard@iwr.uni-heidelberg.de>
 *
 *///=================================================================

#include <string>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <rbdl/rbdl.h>
#include "csvtools.h"

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
//using namespace std;
using namespace boost::numeric::odeint;

#ifndef RBDL_BUILD_ADDON_LUAMODEL
    #error "Error: RBDL addon LuaModel not enabled."
#endif

#include <rbdl/addons/luamodel/luamodel.h>
#include <rbdl/addons/luamodel/luatables.h>

using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

//====================================================================
// Boost stuff
//====================================================================

typedef std::vector< double > state_type;

typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

class rbdlToBoost {
    public:
        rbdlToBoost(Model& model,std::vector<ConstraintSet>& cs
                        ) : model(model),cs(cs) {
            q = VectorNd::Zero(model.dof_count);
            qd = VectorNd::Zero(model.dof_count);
            qdd = VectorNd::Zero(model.dof_count);
            tau = VectorNd::Zero(model.dof_count);

        }

        void operator() (const state_type &x,
                         state_type &dxdt,
                         const double t){

            //q
            int j = 0;
            for(unsigned int i=0; i<model.dof_count; i++){
                q[i] = (double)x[j];
                j++;
            }

            //qd
            for(unsigned int i=0; i<model.dof_count; i++){
                qd[i] = (double)x[j];
                j++;
            }
            //tau = for now. This could call a control
            //               function.
            for(unsigned int i=0; i<model.dof_count; i++){
                tau[i] = 0;
            }

            //2c. A special forward dynamics function needs to be called in
            //    order to compute qdd which simultaneously satisfies the
            //    constraint set and the equations of motion.
            ForwardDynamicsConstraintsDirect (model, q, qd, tau, cs[0], qdd);

            //populate dxdt
            j = 0;
            for(unsigned int i = 0; i < model.dof_count; i++){
                dxdt[j] = (double)qd[i];
                j++;
            }
            for(unsigned int i = 0; i < model.dof_count; i++){
                dxdt[j] = (double)qdd[i];
                j++;
            }

        }

    private:
        Model& model;
        std::vector< RigidBodyDynamics::ConstraintSet >& cs;
        VectorNd q, qd, qdd, tau;
};

struct pushBackStateAndTime
{
    std::vector< state_type >& states;
    std::vector< double >& times;

    pushBackStateAndTime( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : states( states ) , times( times ) { }

    void operator()( const state_type &x , double t )
    {
        states.push_back( x );
        times.push_back( t );
    }
};

void f(const state_type &x, state_type &dxdt, const double t);

/* Problem Constants */
int main (int argc, char* argv[]) {
    rbdl_check_api_version (RBDL_API_VERSION);

    //problem specific constants
    int     nPts    = 100;
    double  t0      = 0;
    double  t1      = 3;

    //Integration settings
    double absTolVal   = 1e-6;
    double relTolVal   = 1e-6;

    VectorNd q, qd, qdd, tau;

    RigidBodyDynamics::Model model;
    string modelFile = "./../model/constrainedDoublePendulum.lua";

    //2a. Some extra work is needed to load in the model, and the
    //    constraint sets
    std::vector<std::string> constraintSetNames =
        Addons::LuaModelGetConstraintSetNames(modelFile.c_str());
    std::vector<RigidBodyDynamics::ConstraintSet> constraintSets;

    constraintSets.resize(constraintSetNames.size());

    if (! Addons::LuaModelReadFromFileWithConstraints(
                                      modelFile.c_str(),
                                      &model,
                                      constraintSets,
                                      constraintSetNames,
                                      false)            ){
        std::cerr     << "Error loading model" << std::endl;
        abort();
    }

    q       = VectorNd::Zero (model.dof_count);
    qd      = VectorNd::Zero (model.dof_count);
    qdd     = VectorNd::Zero (model.dof_count);
    tau     = VectorNd::Zero (model.dof_count);

    printf("DoF: %i\n",model.dof_count);

    printf("==============================\n");
    printf("1. Forward Dynamics \n");
    printf("==============================\n");

    // rx0 x1,y1,z1,rx1,ry1,rz1
    //  0,  1, 2, 3,  4,  5,  6,
    q[2] = 1.0;

    rbdlToBoost rbdlModel(model,constraintSets);
    state_type xState(model.dof_count*2);

    int j = 0;
    for(unsigned int i = 0; i< (model.dof_count); ++i){
        xState[j++] = q(i);
    }
    for(unsigned int i = 0; i< (model.dof_count); ++i){
        xState[j++] = qd(i);
    }

    double dt   = (t1-t0)/((double)nPts);

    double ke, pe = 0;

    std::vector<std::vector< double > > matrixData;
    std::vector<std::vector< double > > matrixPlotData;
    std::vector< double > rowData(model.dof_count+1);
    std::vector< double > rowPlotData(4);

    double a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type
    controlled_stepper(
        default_error_checker< double ,
                               range_algebra ,
                               default_operations >
        ( absTolVal , relTolVal , a_x , a_dxdt ) );

    double tp = 0;
    rowData[0] = 0;
    for(unsigned int z=0; z < model.dof_count; z++){
        rowData[z+1] = xState[model.dof_count + z];
    }
    matrixData.push_back(rowData);
    double t = 0;

    VectorNd constraintPosError = VectorNd::Zero(5);
    VectorNd constraintVelError = VectorNd::Zero(5);

    double constraintPosNorm, constraintVelNorm;

    printf("Columns\n");
    printf("      t,        ke,       pe,    ke+pe ,norm(cons_pos), norm(cons_vel)\n");
    for(int i = 0; i <= nPts; i++){

        t = t0 + dt*i;

        integrate_adaptive(
            controlled_stepper ,
            rbdlModel , xState , tp , t , (t-tp)/10000 );
        tp = t;

        j = 0;
        for(unsigned int k = 0; k < model.dof_count; ++k){
            q[k] = xState[j++];
        }
        for(unsigned int k = 0; k < model.dof_count; ++k){
            qd[k] = xState[j++];
        }

        //Ensure that the constraint set quantites are up to date
        ForwardDynamicsConstraintsDirect (model, q, qd, tau,
          constraintSets[0], qdd);

        //2d. The position-level and velocity level constraint set errors
        //    can be fished out of the vectors in the ConstraintSet struct
        //
        //    Note: In the coming months there will be some functions added so that you
        //          can easily access this information.
        //
        //    Homework: After reading Featherstone & Orin Sec. 3.4, go and have a look
        //              at the fields in the ConstraintSet struct in
        //              rbdl-orb/include/rbdl/Constraints.h
        //
        //              At the same time, open the doxygen for RBDL and read the sections
        //              on Constraints and ConstraintSets
        ConstraintSet &ci = constraintSets[0];
        constraintPosNorm = 0;
        constraintVelNorm = 0;
        for(int k=0; k<5;++k){
          constraintPosError(k) = ci.err(k);
          constraintVelError(k) = ci.errd(k);

          constraintPosNorm += ci.err(k)*ci.err(k);
          constraintVelNorm += ci.errd(k)*ci.errd(k);
        }
        constraintPosNorm = sqrt(constraintPosNorm);
        constraintVelNorm = sqrt(constraintVelNorm);

        pe = Utils::CalcPotentialEnergy(model, q, true);
        ke = Utils::CalcKineticEnergy(model, q, qd, true);

        printf("%f, %f, %f, %f, %f, %f\n",
                    t, ke,pe, ke+pe, constraintPosNorm, constraintVelNorm);

        rowData[0] = t;
        for(unsigned int z=0; z < model.dof_count; z++){
            rowData[z+1] = xState[z];
        }
        matrixData.push_back(rowData);

        rowPlotData[0] = t;
        rowPlotData[1] = constraintPosNorm;
        rowPlotData[2] = constraintVelNorm;
        rowPlotData[3] =(ke+pe);

        matrixPlotData.push_back(rowPlotData);
    }
    printf("Columns\n");
    printf("      t,        ke,       pe,    ke+pe ,norm(cons_pos), norm(cons_vel)\n");

    std::string header = "";
    std::string fname   = "../output/meshup.csv";
    printMatrixToFile(matrixData, header, fname);
    printf("Wrote: ../output/meshup.csv (meshup animation file)\n");

    fname   = "../output/simulationData.csv";
    header = "time,constraintPositionNorm,constraintVelocityNorm,systemEnergy,";
    printMatrixToFile(matrixPlotData, header, fname);
    printf("Wrote: ../output/simulationData.csv (error data)\n");

    return 0;

}


#include <GL/freeglut.h>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

#include "m_estimators.h"
#include "example_func_ax_plus_b_eq_y_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

double a = 0.3;
double b = 1.0;

double a_barron = 0.3;
double b_barron = 1.0;

double a_cauchy = 0.3;
double b_cauchy = 1.0;

double a_huber = 0.3;
double b_huber = 1.0;

double barron_c = 0.25;
double barron_alpha = 1;

std::vector<std::pair<double, double>> input_data;

int main(int argc, char *argv[]){
	double y;
	for(double x = -10; x <= 10; x+= 0.01){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 * 0.1;
		input_data.emplace_back(x,y);
	}

	//outliers
	for(double x = -10; x <= 0; x+= 0.02){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 + 5;
		input_data.emplace_back(x,y);
	}
	for(double x = 0; x <= 10; x+= 0.02){
		example_func_ax_plus_b(y,  x,  a,  b);
		y += ((double(rand()%1000000)/1000000.0) - 0.5) * 2.0 - 5;
		input_data.emplace_back(x,y);
	}

	a = 0.5;
	a_barron = 0.5;
	a_cauchy = 0.5;
	a_huber = 0.5;

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("adaptive_robust_loss_function_demo");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glPointSize(4);
	glLineWidth(5);
	glColor3f(1,0,0);
	glBegin(GL_LINE_STRIP);
	double y;
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_barron,  b_barron);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,1,0);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_cauchy,  b_cauchy);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,0,1);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a_huber,  b_huber);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0,0,0);
	glBegin(GL_LINE_STRIP);
	for(double x = -10; x < 10; x+=0.01){
		example_func_ax_plus_b(y,  x,  a,  b);
		glVertex3f(x, y, 0);
	}
	glEnd();

	glColor3f(0.5,0.5,0.9);
	glBegin(GL_POINTS);
	for(const auto &d:input_data){
		glVertex3f(d.first, d.second, 0);
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'o':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a, b);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a, b);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir,  1);
				tripletListB.emplace_back(ir, 0,  delta);

			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a += h_x[0];
				b += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'b':{
			double min_sum = 1000000000.0;
			for(double alpha = -10; alpha <=2; alpha += 0.1){
				double Z_tilde = get_approximate_partition_function(-10, 10, alpha, 1, 100);
				double sum = 0;
				for(size_t i = 0; i < input_data.size() ; i++){
					double delta;
					observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a, b);
					sum += get_truncated_robust_kernel(delta, alpha, barron_c, Z_tilde);
				}
				if(sum < min_sum){
					min_sum = sum;
					barron_alpha = alpha;
				}
			}
			std::cout << "barron_alpha: " << barron_alpha << std::endl;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_barron, b_barron);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_barron, b_barron);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, barron_alpha, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_barron += h_x[0];
				b_barron += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'c':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_cauchy, b_cauchy);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_cauchy, b_cauchy);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, 0, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_cauchy += h_x[0];
				b_cauchy += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'l':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < input_data.size() ; i++){
				double delta;
				observation_equation_example_func_ax_plus_b_eq_y(delta, input_data[i].first, input_data[i].second, a_huber, b_huber);

				Eigen::Matrix<double, 1, 2> jacobian;
				observation_equation_example_func_ax_plus_b_eq_y_jacobian(jacobian, input_data[i].first, input_data[i].second, a_huber, b_huber);

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir, 0, -jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -jacobian(0,1));
				tripletListP.emplace_back(ir, ir, get_barron_w(delta, 1, barron_c));
				tripletListB.emplace_back(ir, 0,  delta);
			}

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				a_huber += h_x[0];
				b_huber += h_x[1];

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '-':{
			barron_alpha -= 0.1;
			std::cout << "barron_alpha " << barron_alpha << std::endl;
			break;
		}
		case '=':{
			barron_alpha += 0.1;
			std::cout << "barron_alpha " << barron_alpha << std::endl;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "'o': optimize" << std::endl;
	std::cout << "'b': optimize robust Barron" << std::endl;
	std::cout << "'c': optimize robust Cauchy" << std::endl;
	std::cout << "'l': optimize robust L1L2" << std::endl;
	std::cout << "'-': barron_alpha -= 0.1" << std::endl;
	std::cout << "'=': barron_alpha += 0.1" << std::endl;
}


/* H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX */
/* H0 X                                                                            */
/* H0 X   libAtoms+QUIP: atomistic simulation library                              */
/* H0 X                                                                            */
/* H0 X   Portions of this code were written by                                    */
/* H0 X     Albert Bartok-Partay, Silvia Cereda, Gabor Csanyi, James Kermode,      */
/* H0 X     Ivan Solt, Wojciech Szlachta, Csilla Varnai, Steven Winfield.          */
/* H0 X                                                                            */
/* H0 X   Copyright 2006-2010.                                                     */
/* H0 X                                                                            */
/* H0 X   These portions of the source code are released under the GNU General     */
/* H0 X   Public License, version 2, http://www.gnu.org/copyleft/gpl.html          */
/* H0 X                                                                            */
/* H0 X   If you would like to license the source code under different terms,      */
/* H0 X   please contact Gabor Csanyi, gabor@csanyi.net                            */
/* H0 X                                                                            */
/* H0 X   Portions of this code were written by Noam Bernstein as part of          */
/* H0 X   his employment for the U.S. Government, and are not subject              */
/* H0 X   to copyright in the USA.                                                 */
/* H0 X                                                                            */
/* H0 X                                                                            */
/* H0 X   When using this software, please cite the following reference:           */
/* H0 X                                                                            */
/* H0 X   http://www.libatoms.org                                                  */
/* H0 X                                                                            */
/* H0 X  Additional contributions by                                               */
/* H0 X    Alessio Comisso, Chiara Gattinoni, and Gianpietro Moras                 */
/* H0 X                                                                            */
/* H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX */

/* This file contains routines which use the Computational Geometry
   Algorithms Library (CGAL, http://www.cgal.org) to compute alpha
   shapes. It is used to determine a crack front given the set of
   crack surface atoms. */

#define _USE_MATH_DEFINES
#include <math.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/algorithm.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/bounding_box.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <list>

#include <boost/iterator/transform_iterator.hpp>

extern "C" {
#include "libatoms.h"
}

typedef CGAL::Exact_predicates_exact_constructions_kernel K;

typedef K::FT FT;

typedef K::Point_2  Point;
typedef K::Segment_2  Segment;

typedef CGAL::Alpha_shape_vertex_base_2<K> Vb;
typedef CGAL::Alpha_shape_face_base_2<K>  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Triangulation_2;

typedef CGAL::Alpha_shape_2<Triangulation_2>  Alpha_shape_2;

typedef Alpha_shape_2::Face  Face;
typedef Alpha_shape_2::Vertex Vertex;
typedef Alpha_shape_2::Edge Edge;
typedef Alpha_shape_2::Face_handle  Face_handle;
typedef Alpha_shape_2::Vertex_handle Vertex_handle;

typedef Alpha_shape_2::Face_circulator  Face_circulator;
typedef Alpha_shape_2::Vertex_circulator  Vertex_circulator;

typedef Alpha_shape_2::Locate_type Locate_type;

typedef Alpha_shape_2::Face_iterator  Face_iterator;
typedef Alpha_shape_2::Vertex_iterator  Vertex_iterator;
typedef Alpha_shape_2::Edge_iterator  Edge_iterator;
typedef Alpha_shape_2::Edge_circulator  Edge_circulator;

typedef Alpha_shape_2::Alpha_iterator Alpha_iterator;
typedef Alpha_shape_2::Alpha_shape_edges_iterator Alpha_shape_edges_iterator;
typedef Alpha_shape_2::Alpha_shape_vertices_iterator Alpha_shape_vertices_iterator;

typedef CGAL::Arr_segment_traits_2<K> Traits_2;
typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
typedef CGAL::Arr_walk_along_line_point_location<Arrangement_2> Walk_pl;

std::map<Point,int>::key_type get_key(std::map<Point,int>::value_type aPair) {
  return aPair.first;
}

typedef std::map<Point,int>::key_type (*get_key_t)(std::map<Point,int>::value_type);
typedef std::map<Point,int>::iterator map_iterator;
typedef boost::transform_iterator<get_key_t, map_iterator> mapkey_iterator;

extern "C" void c_alpha_shape_2(int *n, double x[], double y[],
                                double *alpha, int *shape_n, int shape_list[],
                                int *error)
{
  INIT_ERROR;

  // Map from points to original indices in input data
  std::map<Point,int> point_map;

  for (int i=0; i<*n; i++) {
    point_map[Point(x[i],y[i])] = i;
  }

  mapkey_iterator points_begin(point_map.begin(), get_key);
  mapkey_iterator points_end(point_map.end(), get_key);

  Alpha_shape_2 A(points_begin, points_end, FT(*alpha), Alpha_shape_2::GENERAL);

  Arrangement_2 arr;

  // Traverse alpha shape edges and add to arrangement
  for(Alpha_shape_edges_iterator it =  A.alpha_shape_edges_begin();
      it != A.alpha_shape_edges_end();
      ++it){
    CGAL::insert(arr, A.segment(*it));
  }

  // Check we have one bounded face
  if (arr.number_of_faces() != 2) {
    RAISE_ERROR("c_alpha_shape_2: number of bounded faces != 1, try increasing alpha");
  }

  Arrangement_2::Face_const_iterator fi = arr.faces_begin();
  ++fi; // skip over the unbounded face which all arrangements have

  Arrangement_2::Ccb_halfedge_const_circulator ccb = fi->outer_ccb();

  // Check shape_list is big enough to store alpha shape
  int tmp_shape_length = 0;
  do {
    tmp_shape_length++;
    ++ccb;
  } while (ccb != fi->outer_ccb());
  if (tmp_shape_length > *shape_n) {
    RAISE_ERROR("c_alpha_shape_2: size of alpha shape exceeds length of shape_list");
  }

  // traverse outer CCB again, adding results to shape_list
  ccb = fi->outer_ccb();
  *shape_n = 0;
  do {
    shape_list[(*shape_n)++] = point_map[ccb->source()->point()];
    ++ccb;
  } while (ccb != fi->outer_ccb());
}

extern "C" void c_crack_front_alpha_shape(int *n, double x[], double y[],
                                          double *alpha, double *angle_threshold,
                                          int *front_n, int front_list[], int *error)
{
  INIT_ERROR;

  // Map from points to original indices in input data
  std::map<Point,int> point_map;

  for (int i=0; i<*n; i++) {
    point_map[Point(x[i],y[i])] = i;
  }

  mapkey_iterator points_begin(point_map.begin(), get_key);
  mapkey_iterator points_end(point_map.end(), get_key);

  Alpha_shape_2 A(points_begin, points_end, FT(*alpha), Alpha_shape_2::GENERAL);

  Arrangement_2 arr;

  // Traverse alpha shape edges and add to arrangement
  for(Alpha_shape_edges_iterator it =  A.alpha_shape_edges_begin();
      it != A.alpha_shape_edges_end();
      ++it){
    CGAL::insert(arr, A.segment(*it));
  }

  // Check we have one bounded face
  if (arr.number_of_faces() != 2) {
    RAISE_ERROR("c_crack_front_alpha_shape: number of bounded faces != 1, try increasing alpha");
  }

  // Compute bounding box of original point set and use it
  // to perform a vertical ray shooting query
  K::Iso_rectangle_2 bbox = bounding_box(points_begin, points_end);
  Point p = Point(bbox.xmax(), bbox.ymin());
  Walk_pl walk_pl(arr);
  CGAL::Object    obj = walk_pl.ray_shoot_up (p);
  Arrangement_2::Vertex_const_handle xmax_vertex;
  if (!CGAL::assign (xmax_vertex, obj)) {
    RAISE_ERROR("c_crack_front_alpha_shape: vertical line search did not hit a vertex");
  }

  Arrangement_2::Face_const_iterator fi = arr.faces_begin();
  ++fi; // skip over the unbounded face which all arrangements have

  // find half-line with source at xmax_vertex
  Arrangement_2::Ccb_halfedge_const_circulator curr, xmax_edge;
  for(xmax_edge = fi->outer_ccb(); xmax_edge->source() != xmax_vertex; ++xmax_edge);

  double bearing;
  Point p1, p2;
  std::list<int> front;

  // start at xmax_edge and traverse outer CCB until angle along line
  // exceeds angle_threshold
  curr = xmax_edge;
  do {
    p1 = curr->source()->point();
    p2 = curr->target()->point();

    bearing = 180./M_PI*std::atan2(CGAL::to_double(p2.y())-CGAL::to_double(p1.y()),
                                   CGAL::to_double(p2.x())-CGAL::to_double(p1.x()));

    front.push_back(point_map[curr->source()->point()]);
  } while (++curr != xmax_edge && bearing < *angle_threshold);

  // now go back to xmax_edge, and go around backwards until
  // angle is less than -angle_theshold
  curr = xmax_edge;
  --curr; // don't want xmax_edge vertex to appear twice in output list
  do {
    p1 = curr->twin()->source()->point();
    p2 = curr->twin()->target()->point();

    bearing = 180./M_PI*std::atan2(CGAL::to_double(p2.y())-CGAL::to_double(p1.y()),
                                   CGAL::to_double(p2.x())-CGAL::to_double(p1.x()));

    front.push_front(point_map[curr->source()->point()]);
  } while (--curr != xmax_edge && bearing > -*angle_threshold);

  // Copy output into array front_list
  if (front.size() > *front_n) {
    RAISE_ERROR("c_crack_front_alpha_shape: Not enough space in front_list");
  }
  *front_n=0;
  for (std::list<int>::iterator it = front.begin(); it != front.end(); ++it) {
    front_list[(*front_n)++] = *it;
  }
}

#ifdef MAIN_PROGRAM
int main()
{
  FILE *stream = fopen("pos_xz","r");
  char buffer[256];
  int i,n;

  fgets(buffer,256,stream);
  sscanf(buffer,"%d",&n);

  double *x = new double[n];
  double *y = new double[n];

  for (i=0; i<n; i++) {
    fgets(buffer,256,stream);
    sscanf(buffer,"%lf %lf",&(x[i]),&(y[i]));
  }
  fclose(stream);

  double alpha = 100.0;
  double angle_threshold = 160.0;

  int front_n = 100;
  int *front_list = new int[front_n];

  int error = 0;
  crack_front_alpha_shape(&n, x, y, &alpha, &angle_threshold, &front_n, front_list, &error);

  if (error == 0) {
    for (i = 0; i < front_n; i++)
      std::cout << front_list[i] << std::endl;
  }
}
#endif


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;

void rotate_quat(Quaternionf & q, Vector3f & gibbs_vector, Quaternionf* ret) {
  Quaternionf t(2, gibbs_vector.x(), gibbs_vector.y(), gibbs_vector.z());
  // Eigen's multiply doesn't work the same as simulink...
  // let's keep this to be safe
  ret->x() = q.x() * t.w() + q.w() * t.x() - q.z() * t.y() + q.y() * t.z();
  ret->y() = q.y() * t.w() + q.z() * t.x() + q.w() * t.y() - q.x() * t.z();
  ret->z() = q.z() * t.w() - q.y() * t.x() + q.x() * t.y() + q.w() * t.z();
  ret->w() = q.w() * t.w() - q.x() * t.x() - q.y() * t.y() - q.z() * t.z();
  if (ret->w() < 0) {
    ret->w() = -ret->w(); ret->x() = -ret->x(); ret->y() = -ret->y(); ret->z() = -ret->z();
  }
  // Eigen normalize function doesn't work here, I don't understand why
  if (ret->w() != 0) {
    float mag = sqrt(ret->x() * ret->x() + ret->y() * ret->y() + ret->z() * ret->z() + ret->w() * ret->w());
    ret->x() /= mag; ret->y() /= mag; ret->z() /= mag; ret->w() /= mag;
  }
}

void apply_delta_state(
    float* delta_state, int delta_state_length, unsigned short update_flag,
    float* quat_ISS2B_in, float* gyro_bias_in,
    float* V_B_ISS_ISS_in, float* accel_bias_in, float* P_B_ISS_ISS_in,
    float* ml_quat_ISS2cam_in, float* ml_P_cam_ISS_ISS_in, unsigned short kfl_status_in,
    float* of_quat_ISS2cam_in, float* of_P_cam_ISS_ISS_in,
    float* quat_ISS2B_out, float* gyro_bias_out,
    float* V_B_ISS_ISS_out, float* accel_bias_out, float* P_B_ISS_ISS_out,
    float* ml_quat_ISS2cam_out, float* ml_P_cam_ISS_ISS_out, unsigned short* kfl_status_out,
    float* of_quat_ISS2cam_out, float* of_P_cam_ISS_ISS_out) {
  int num_augs = (delta_state_length - 21) / 6;
  Map<VectorXf> ds(delta_state, delta_state_length);
  Map<Quaternionf> quat(quat_ISS2B_in);
  Map<Quaternionf> out_quat(quat_ISS2B_out);
  Map<Vector3f> gyro_bias(gyro_bias_in);
  Map<Vector3f> out_gyro_bias(gyro_bias_out);
  Map<Vector3f> vel(V_B_ISS_ISS_in);
  Map<Vector3f> out_vel(V_B_ISS_ISS_out);
  Map<Vector3f> accel_bias(accel_bias_in);
  Map<Vector3f> out_accel_bias(accel_bias_out);
  Map<Vector3f> pos(P_B_ISS_ISS_in);
  Map<Vector3f> out_pos(P_B_ISS_ISS_out);
  Map<Quaternionf> ml_quat(ml_quat_ISS2cam_in);
  Map<Quaternionf> out_ml_quat(ml_quat_ISS2cam_out);
  Map<Vector3f> ml_pos(ml_P_cam_ISS_ISS_in);
  Map<Vector3f> out_ml_pos(ml_P_cam_ISS_ISS_out);

  Quaternionf temp_quat1, temp_quat2;
  Vector3f temp_vector = ds.segment<3>(0);
  temp_quat1 = quat;
  rotate_quat(temp_quat1, temp_vector, &temp_quat2);
  out_quat = temp_quat2;
  out_gyro_bias = gyro_bias + ds.segment<3>(3);
  out_vel = vel + ds.segment<3>(6);
  out_accel_bias = accel_bias + ds.segment<3>(9);
  out_pos = pos + ds.segment<3>(12);

  temp_quat1 = ml_quat;
  temp_vector = ds.segment<3>(15);
  rotate_quat(temp_quat1, temp_vector, &temp_quat2);
  out_ml_quat = temp_quat2;
  out_ml_pos = ml_pos + ds.segment<3>(18);

  *kfl_status_out = (kfl_status_in & ~3) | update_flag;

  Map<MatrixXf> of_quat(of_quat_ISS2cam_in, num_augs, 4);
  Map<MatrixXf> out_of_quat(of_quat_ISS2cam_out, num_augs, 4);
  Map<MatrixXf> of_pos(of_P_cam_ISS_ISS_in, num_augs, 3);
  Map<MatrixXf> out_of_pos(of_P_cam_ISS_ISS_out, num_augs, 3);

  for (int i = 0; i < num_augs; i++) {
    Quaternionf t_quat(of_quat(i, 3), of_quat(i, 0), of_quat(i, 1), of_quat(i, 2));
    Quaternionf r;
    temp_vector = ds.segment<3>(21 + 6 * i);
    rotate_quat(t_quat, temp_vector, &r);
    out_of_quat(i, 0) = r.x();
    out_of_quat(i, 1) = r.y();
    out_of_quat(i, 2) = r.z();
    out_of_quat(i, 3) = r.w();
    out_of_pos.row(i) = of_pos.row(i) + ds.segment<3>(24 + 6 * i).transpose();
  }
}


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

#include "paddle/phi/kernels/matrix_rank_tol_kernel.h"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include "paddle/phi/core/kernel_registry.h"
#include "paddle/phi/kernels/elementwise_multiply_kernel.h"
#include "paddle/phi/kernels/full_kernel.h"
#include "paddle/phi/kernels/funcs/compare_functors.h"
#include "paddle/phi/kernels/funcs/eigen/common.h"
#include "paddle/phi/kernels/funcs/elementwise_base.h"
#include "paddle/phi/kernels/impl/matrix_rank_kernel_impl.h"
#include "paddle/phi/kernels/reduce_max_kernel.h"
#include "paddle/phi/kernels/reduce_sum_kernel.h"

namespace phi {

template <typename T>
void BatchEigenvalues(const T* x_data,
                      T* eigenvalues_data,
                      int batches,
                      int rows,
                      int cols,
                      int k) {
  // Eigen::Matrix API need non-const pointer.
  T* input = const_cast<T*>(x_data);
  int stride = rows * cols;
  for (int i = 0; i < batches; i++) {
    auto m = Eigen::Map<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        input + i * stride, rows, rows);
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
        eigen_solver(m);
    auto eigenvalues = eigen_solver.eigenvalues().cwiseAbs();
    for (int j = 0; j < k; j++) {
      *(eigenvalues_data + i * k + j) = eigenvalues[j];
    }
  }
}

template <typename T>
void BatchSVD(const T* x_data,
              T* eigenvalues_data,
              int batches,
              int rows,
              int cols,
              int k) {
  // Eigen::Matrix API need non-const pointer.
  T* input = const_cast<T*>(x_data);
  int stride = rows * cols;
  Eigen::BDCSVD<
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      svd;
  for (int i = 0; i < batches; i++) {
    auto m = Eigen::Map<
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(
        input + i * stride, rows, cols);
    svd.compute(m);
    auto res_s = svd.singularValues();
    for (int j = 0; j < k; j++) {
      eigenvalues_data[i * k + j] = res_s[j];
    }
  }
}

template <typename T, typename Context>
void MatrixRankTolKernel(const Context& dev_ctx,
                         const DenseTensor& x,
                         const DenseTensor& atol_tensor,
                         bool use_default_tol,
                         bool hermitian,
                         DenseTensor* out) {
  auto* x_data = x.data<T>();
  dev_ctx.template Alloc<int64_t>(out);
  auto dim_x = x.dims();
  auto dim_out = out->dims();
  int rows = dim_x[dim_x.size() - 2];
  int cols = dim_x[dim_x.size() - 1];
  int k = std::min(rows, cols);
  auto numel = x.numel();
  int batches = numel / (rows * cols);

  T rtol_T = 0;

  if (use_default_tol) {
    rtol_T = std::numeric_limits<T>::epsilon() * std::max(rows, cols);
  }

  DenseTensor eigenvalue_tensor;
  eigenvalue_tensor.Resize(detail::GetEigenvalueDim(dim_x, k));
  auto* eigenvalue_data = dev_ctx.template Alloc<T>(&eigenvalue_tensor);

  if (hermitian) {
    BatchEigenvalues<T>(x_data, eigenvalue_data, batches, rows, cols, k);
  } else {
    BatchSVD<T>(x_data, eigenvalue_data, batches, rows, cols, k);
  }

  DenseTensor max_eigenvalue_tensor;
  max_eigenvalue_tensor.Resize(detail::RemoveLastDim(eigenvalue_tensor.dims()));
  dev_ctx.template Alloc<T>(&max_eigenvalue_tensor);
  phi::MaxKernel<T, Context>(dev_ctx,
                             eigenvalue_tensor,
                             std::vector<int64_t>{-1},
                             false,
                             &max_eigenvalue_tensor);

  DenseTensor temp_rtol_tensor;
  temp_rtol_tensor =
      phi::Full<T, Context>(dev_ctx, {1}, static_cast<T>(rtol_T));

  DenseTensor rtol_tensor =
      phi::Multiply<T>(dev_ctx, temp_rtol_tensor, max_eigenvalue_tensor);

  DenseTensor tol_tensor;
  tol_tensor.Resize(dim_out);
  dev_ctx.template Alloc<T>(&tol_tensor);
  funcs::ElementwiseCompute<GreaterElementFunctor<T>, T, T>(
      dev_ctx,
      atol_tensor,
      rtol_tensor,
      -1,
      GreaterElementFunctor<T>(),
      &tol_tensor);

  tol_tensor.Resize(detail::NewAxisDim(tol_tensor.dims(), 1));

  DenseTensor compare_result;
  compare_result.Resize(detail::NewAxisDim(dim_out, k));
  dev_ctx.template Alloc<int64_t>(&compare_result);
  int axis = -1;
  if (eigenvalue_tensor.dims().size() >= tol_tensor.dims().size()) {
    funcs::ElementwiseCompute<funcs::GreaterThanFunctor<T, int64_t>, T, int>(
        dev_ctx,
        eigenvalue_tensor,
        tol_tensor,
        axis,
        funcs::GreaterThanFunctor<T, int64_t>(),
        &compare_result);
  } else {
    funcs::ElementwiseCompute<funcs::LessThanFunctor<T, int64_t>, T, int>(
        dev_ctx,
        eigenvalue_tensor,
        tol_tensor,
        axis,
        funcs::LessThanFunctor<T, int64_t>(),
        &compare_result);
  }

  phi::SumKernel<int64_t>(dev_ctx,
                          compare_result,
                          std::vector<int64_t>{-1},
                          compare_result.dtype(),
                          false,
                          out);
}
}  // namespace phi

PD_REGISTER_KERNEL(
    matrix_rank_tol, CPU, ALL_LAYOUT, phi::MatrixRankTolKernel, float, double) {
}


/**
 * \file TimeVaryingSecondOrderFilter.cpp
 * @see http://abvolt.com/research/publications2.htm
 * @see http://www.music.mcgill.ca/~ich/classes/FiltersChap2.pdf for the allpass filter
 */

#include <ATK/EQ/TimeVaryingSecondOrderFilter.h>
#include <ATK/EQ/TimeVaryingIIRFilter.h>

#include <cassert>
#include <cmath>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  /// Time varying coefficient base class. Two input ports, the first is the data, the second one is the central frequency
  template <typename DataType>
  TimeVaryingBaseSecondOrderCoefficients<DataType>::TimeVaryingBaseSecondOrderCoefficients()
    :Parent(2, 1)
  {
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.clear();
    coefficients_in.reserve((in_order+1) * number_of_steps);
    coefficients_out.clear();
    coefficients_out.reserve(out_order * number_of_steps);
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_min_frequency(double min_frequency)
  {
    if(min_frequency <= 0)
    {
      throw std::out_of_range("Min frequency must be positive");
    }
    this->min_frequency = min_frequency;
    setup();
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_min_frequency() const
  {
    return min_frequency;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_max_frequency(double max_frequency)
  {
    if(max_frequency <= min_frequency)
    {
      throw std::out_of_range("Max frequency must be greater than min frequency");
    }
    this->max_frequency = max_frequency;
    setup();
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_max_frequency() const
  {
    return max_frequency;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_number_of_steps(int number_of_steps)
  {
    if(number_of_steps <= 0)
    {
      throw std::out_of_range("Number of steps must be strictly positive");
    }
    this->number_of_steps = number_of_steps;
    setup();
  }

  template <typename DataType>
  int TimeVaryingBaseSecondOrderCoefficients<DataType>::get_number_of_steps() const
  {
    return number_of_steps;
  }

  template <typename DataType>
  void TimeVaryingBaseSecondOrderCoefficients<DataType>::set_memory(double memory)
  {
    if(memory < 0 || memory >= 1)
    {
      throw std::out_of_range("Memory for time varying EQ had to be in the range [0, 1[");
    }
    this->memory = memory;
  }

  template <typename DataType>
  double TimeVaryingBaseSecondOrderCoefficients<DataType>::get_memory() const
  {
    return memory;
  }

  template<typename DataType>
  TimeVaryingBandPassCoefficients<DataType>::TimeVaryingBandPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);
      DataType Q_inv = 1 / Q;

      coefficients_in.push_back(-Q_inv * c / d);
      coefficients_in.push_back(0);
      coefficients_in.push_back(Q_inv * c / d);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template <typename DataType_>
  void TimeVaryingBandPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  TimeVaryingLowPassCoefficients<DataType>::TimeVaryingLowPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in.push_back(c * c / d);
      coefficients_in.push_back(2 * c * c / d);
      coefficients_in.push_back(c * c / d);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template<typename DataType>
  TimeVaryingHighPassCoefficients<DataType>::TimeVaryingHighPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

      coefficients_in.push_back(1);
      coefficients_in.push_back(-2);
      coefficients_in.push_back(1);
      coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
      coefficients_out.push_back(- 2 * (c * c - 1) / d);
    }
  }

  template<typename DataType>
  TimeVaryingBandPassPeakCoefficients<DataType>::TimeVaryingBandPassPeakCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      DataType Q_inv = 1 / Q;
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = 1 + V0 * Q_inv * c + c * c;

        coefficients_in.push_back((1 - Q_inv * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + Q_inv * c + c * c) / d);
        coefficients_out.push_back(-(1 - V0 * Q_inv * c + c * c) / d);
        coefficients_out.push_back(-2 * (c * c - 1) / d);
      }
      else
      {
        DataType V0 = gain;
        DataType d = 1 + Q_inv * c + c * c;

        coefficients_in.push_back((1 - V0 * Q_inv * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + V0 * Q_inv * c + c * c) / d);
        coefficients_out.push_back(-(1 - Q_inv * c + c * c) / d);
        coefficients_out.push_back(-2 * (c * c - 1) / d);
      }
    }
  }

  template <typename DataType_>
  void TimeVaryingBandPassPeakCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void TimeVaryingBandPassPeakCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  TimeVaryingAllPassCoefficients<DataType>::TimeVaryingAllPassCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * Q);
      DataType d = -std::cos(2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);

      coefficients_in.push_back(1);
      coefficients_in.push_back(d * (1 - c));
      coefficients_in.push_back(-c);
      coefficients_out.push_back(c);
      coefficients_out.push_back(-d * (1 - c));
    }
  }

  template <typename DataType_>
  void TimeVaryingAllPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  TimeVaryingLowShelvingCoefficients<DataType>::TimeVaryingLowShelvingCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c);

        coefficients_in.push_back((1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_in.push_back(2 * (c * c - 1) / d);
        coefficients_in.push_back((1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.) * V0) * c + V0 * c * c) / d);
        coefficients_out.push_back(- 2 * (V0 * c * c - 1) / d);
      }
      else
      {
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

        coefficients_in.push_back((1 - std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d);
        coefficients_in.push_back(2 * (gain * c * c - 1) / d);
        coefficients_in.push_back((1 + std::sqrt(static_cast<DataType>(2.) * gain) * c + gain * c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.)) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - 1) / d);
      }
    }
  }

  template <typename DataType_>
  void TimeVaryingLowShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  TimeVaryingHighShelvingCoefficients<DataType>::TimeVaryingHighShelvingCoefficients()
    :Parent()
  {
  }

  template <typename DataType>
  void TimeVaryingHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    for(gsl::index i = 0; i < number_of_steps; ++i)
    {
      DataType cut_frequency = static_cast<DataType>((max_frequency - min_frequency) * i / (number_of_steps - 1) + min_frequency);
      DataType c = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
      if(gain <= 1)
      {
        DataType V0 = 1 / gain;
        DataType d = (V0 + std::sqrt(static_cast<DataType>(2.) * V0) * c + c * c);

        coefficients_in.push_back(-(1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_in.push_back(-2 * (c * c - 1) / d);
        coefficients_in.push_back(-(1 + std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_out.push_back(- (V0 - std::sqrt(static_cast<DataType>(2.0) * V0) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - V0) / d);
      }
      else
      {
        DataType d = (1 + std::sqrt(static_cast<DataType>(2.)) * c + c * c);

        coefficients_in.push_back(-(gain - std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d);
        coefficients_in.push_back(-2 * (c * c - gain) / d);
        coefficients_in.push_back(-(gain + std::sqrt(static_cast<DataType>(2.0) * gain) * c + c * c) / d);
        coefficients_out.push_back(- (1 - std::sqrt(static_cast<DataType>(2.0)) * c + c * c) / d);
        coefficients_out.push_back(- 2 * (c * c - 1) / d);
      }
    }
  }

  template<typename DataType_>
  void TimeVaryingHighShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ TimeVaryingHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

#if ATK_ENABLE_INSTANTIATION
  template class TimeVaryingBaseSecondOrderCoefficients<float>;

  template class ATK_EQ_EXPORT TimeVaryingBandPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingLowPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingHighPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingBandPassPeakCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingAllPassCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingLowShelvingCoefficients<float>;
  template class ATK_EQ_EXPORT TimeVaryingHighShelvingCoefficients<float>;
#endif
  template class TimeVaryingBaseSecondOrderCoefficients<double>;

  template class ATK_EQ_EXPORT TimeVaryingBandPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingLowPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingHighPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingBandPassPeakCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingAllPassCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingLowShelvingCoefficients<double>;
  template class ATK_EQ_EXPORT TimeVaryingHighShelvingCoefficients<double>;
}


///\file IsingModel-Visual.cpp
///\author Ethan Knox
///\date 7/19/2020.

#include <boost/filesystem/fstream.hpp>
#include <boost/program_options.hpp>
#include "../include/Ising.h"
#include "../include/run_simulation.h"
#include <iostream>

namespace opt = boost::program_options;

int main(int argc, const char* argv[])
{
    long nrows, ncols, stopiter, framestep;
    double J, h, T;
    char method, geometry;

    opt::options_description params("Simulation Parameters");

    params.add_options()
            ("help", "Show usage")
            ("nrows,r", opt::value<long>(&nrows)->default_value(500), "Number of rows")
            ("ncols,c", opt::value<long>(&ncols)->default_value(500), "Number of columns")
            ("stopiter,s", opt::value<long>(&stopiter)->default_value(250000), "Number of iterations")
            ("framestep,f", opt::value<long>(&framestep)->default_value(2500), "Number of iterations between frames")
            ("J,j", opt::value<double>(&J)->default_value(1.0), "Ferromagnetic Coupling Constant")
            ("h,h", opt::value<double>(&h)->default_value(0.0), "Magnetic Field Strength")
            ("T,t", opt::value<double>(&T)->default_value(1.8), "Temperature")
            ("method,m", opt::value<char>(&method)->default_value('M'), "Solution Method")
            ("geometry,g", opt::value<char>(&geometry)->default_value('S'), "Grid Geometry")
            ;

    opt::variables_map vm;
    opt::store(opt::parse_command_line(argc, argv, params), vm);

    if (vm.count("help")) {
        std::cout << params << std::endl;
        return 1;
    }
    else {
        opt::notify(vm);

        std::cout << "\n2D ISING MODEL - VISUAL SIMULATION" << std::endl;
        std::cout << "----------------------------------" << std::endl;

        Ising p = Ising(nrows, ncols, stopiter, framestep, J, h, T, method, geometry);

        std::cout << "Using the " << (p.method == 'M' ? "Metropolis-Hastings" : "Wolff") << " Algorithm ";
        std::cout << "with " << (p.geometry == 'S' ? "Standard Square" : "Hexagonal") << " Geometry" << std::endl;
        std::cout << p.nrows << "x" << p.ncols << " Grid, ";
        std::cout << p.stopiter << " Iterations, " << p.framestep << " Steps Between Frames" << std::endl;
        std::cout << "J = " << p.J << ", h = " << p.h << ", T = " << p.T << std::endl;
        std::cout << "\nRunning..." << std::endl;

        p.randomize();
        run_simulation(p); // Go baby go

        std::cout << "Done!" << std::endl;
        std::cout << "----------------------------------" << std::endl;

        return 0;
    }
}