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

 * 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

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

/**
 * @file    Rot3.cpp
 * @brief   Rotation, common code between Rotation matrix and Quaternion
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <random>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
  gtsam::print((Matrix)matrix(), s);
}

/* ************************************************************************* */
Rot3 Rot3::Random(std::mt19937& rng) {
  // TODO allow any engine without including all of boost :-(
  Unit3 axis = Unit3::Random(rng);
  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
  double angle = randomAngle(rng);
  return AxisAngle(axis, angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
  // if a_p is already aligned with b_p, return the identity rotation
  if (std::abs(a_p.dot(b_p)) > 0.999999999) {
    return Rot3();
  }

  // Check axis was not degenerate cross product
  const Vector3 z = axis.unitVector();
  if (z.hasNaN())
    throw std::runtime_error("AlignSinglePair: axis has Nans");

  // Now, calculate rotation that takes b_p to a_p
  const Matrix3 P = I_3x3 - z * z.transpose();  // orthogonal projector
  const Vector3 a_po = P * a_p.unitVector();    // point in a orthogonal to axis
  const Vector3 b_po = P * b_p.unitVector();    // point in b orthogonal to axis
  const Vector3 x = a_po.normalized();          // x-axis in axis-orthogonal plane, along a_p vector
  const Vector3 y = z.cross(x);                 // y-axis in axis-orthogonal plane
  const double u = x.dot(b_po);                 // x-coordinate for b_po
  const double v = y.dot(b_po);                 // y-coordinate for b_po
  double angle = std::atan2(v, u);
  return Rot3::AxisAngle(z, -angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                         const Unit3& a_q, const Unit3& b_q) {
  // there are three frames in play:
  // a: the first frame in which p and q are measured
  // b: the second frame in which p and q are measured
  // i: intermediate, after aligning first pair

  // First, find rotation around that aligns a_p and b_p
  Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);

  // Rotate points in frame b to the intermediate frame,
  // in which we expect the point p to be aligned now
  Unit3 i_q = i_R_b * b_q;
  assert(assert_equal(a_p, i_R_b * b_p, 1e-6));

  // Now align second pair: we need to align i_q to a_q
  Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
  assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
  assert(assert_equal(a_q, a_R_i * i_q, 1e-6));

  // The desired rotation is the product of both
  Rot3 a_R_b = a_R_i * i_R_b;
  return a_R_b;
}

/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
  return equal_with_abs_tol(matrix(), R.matrix(), tol);
}

/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
  if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(unrotate(p.point3(Dp)));
  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
  if (HR) *HR = q.basis().transpose() * q.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::operator*(const Unit3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
    OptionalJacobian<3,3> H2) const {
  const Matrix3& Rt = transpose();
  Point3 q(Rt * p); // q = Rt*p
  const double wx = q.x(), wy = q.y(), wz = q.z();
  if (H1)
    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
  if (H2)
    *H2 = Rt;
  return q;
}

/* ************************************************************************* */
Point3 Rot3::column(int index) const{
  if(index == 3)
    return r3();
  else if(index == 2)
    return r2();
  else if(index == 1)
    return r1(); // default returns r1
  else
    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}

/* ************************************************************************* */
Vector3 Rot3::xyz() const {
  Matrix3 I;Vector3 q;
  boost::tie(I,q)=RQ(matrix());
  return q;
}

/* ************************************************************************* */
Vector3 Rot3::ypr() const {
  Vector3 q = xyz();
  return Vector3(q(2),q(1),q(0));
}

/* ************************************************************************* */
Vector3 Rot3::rpy() const {
  return xyz();
}

/* ************************************************************************* */
Vector Rot3::quaternion() const {
  gtsam::Quaternion q = toQuaternion();
  Vector v(4);
  v(0) = q.w();
  v(1) = q.x();
  v(2) = q.y();
  v(3) = q.z();
  return v;
}

/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
  return SO3::ExpmapDerivative(x);
}

/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
  return SO3::LogmapDerivative(x);
}

/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {

  double x = -atan2(-A(2, 1), A(2, 2));
  Rot3 Qx = Rot3::Rx(-x);
  Matrix3 B = A * Qx.matrix();

  double y = -atan2(B(2, 0), B(2, 2));
  Rot3 Qy = Rot3::Ry(-y);
  Matrix3 C = B * Qy.matrix();

  double z = -atan2(-C(1, 0), C(1, 1));
  Rot3 Qz = Rot3::Rz(-z);
  Matrix3 R = C * Qz.matrix();

  Vector xyz = Vector3(x, y, z);
  return make_pair(R, xyz);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
  os << "\n";
  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
  return os;
}

/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
  return interpolate(*this, other, t);
}

/* ************************************************************************* */

} // namespace gtsam


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * 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 <iostream>
#include <memory>
#include <ctime>
#include <cmath>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "MatrixIO.h"

using namespace gpr;

template<typename T>
void Test1(){
    /*
     * Test 1.1: perform regression
     * Test 1.2: save/load product kernel
     */
    std::cout << "Test 1.1: regression test with product kernel... " << std::flush;

    // typedefs
    typedef GaussianKernel<T>                   GaussianKernelType;
    typedef std::shared_ptr<GaussianKernelType> GaussianKernelTypePointer;
    typedef PeriodicKernel<T>                   PeriodicKernelType;
    typedef std::shared_ptr<PeriodicKernelType> PeriodicKernelTypePointer;
    typedef ProductKernel<T>                    ProductKernelType;
    typedef std::shared_ptr<ProductKernelType>  ProductKernelTypePointer;

    typedef GaussianProcess<T>                  GaussianProcessType;
    typedef std::shared_ptr<GaussianProcessType>GaussianProcessTypePointer;

    typedef typename GaussianProcessType::VectorType     VectorType;
    typedef typename GaussianProcessType::MatrixType     MatrixType;

    // ground truth function
    auto f = [](double x)->double { return x/2.0 * std::sin(x)*std::cos(2.2*std::sin(x)); };

    double interval_start = 0;
    double interval_end = 5 * 2*M_PI; // full interval
    double interval_step = 0.1;

    //--------------------------------------------------------------------------------
    // generating ground truth
    unsigned gt_size = (interval_end-interval_start) / interval_step;
    VectorType y(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        y[i] = f(interval_start + i*interval_step);
    }

    //--------------------------------------------------------------------------------
    // perform training
    double noise = 0.04;
    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    double interval_training_end = 3 * 2*M_PI; // interval to train
    unsigned number_of_samples = 50;

    PeriodicKernelTypePointer   pk(new PeriodicKernelType(5, 0.5, 0.4));
    GaussianKernelTypePointer   gk(new GaussianKernelType(65, 10));
    ProductKernelTypePointer    cpk(new ProductKernelType(pk, gk));

    GaussianProcessTypePointer gp(new GaussianProcessType(cpk));
    gp->SetSigma(noise);

    // add samples
    double training_step_size = (interval_training_end - interval_start) / number_of_samples;
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(1);
        x(0) = interval_start + i*training_step_size;

        VectorType y(1);
        y(0) = f(x(0)) + r();

        gp->AddSample(x, y);
    }
    gp->Initialize();

    //--------------------------------------------------------------------------------
    // predict full intervall
    VectorType y_predict(gt_size);
    for(unsigned i=0; i<gt_size; i++){
        VectorType x(1);
        x(0) = interval_start + i*interval_step;
        y_predict[i] = gp->Predict(x)(0);
    }

    double err = (y-y_predict).norm() / gt_size;
    if(err>0.02){
        std::stringstream ss; ss<<err; throw ss.str();
    }
    else{
        std::cout << " [passed]." << std::endl;
    }

    std::cout << "Test 1.2: save/load product kernel... " << std::flush;

    gp->Save("/tmp/gp_io_test-");

    GaussianKernelTypePointer k_dummy(new GaussianKernelType(1, 1));
    GaussianProcessTypePointer gp_read(new GaussianProcessType(k_dummy));
    gp_read->Load("/tmp/gp_io_test-");

    if(*gp.get() == *gp_read.get()){
        std::cout << " [passed]." << std::endl;
    }
    else{
        throw std::string("gps are not equal");
    }
}

template<typename T>
void Test2(){
    // typedefs
    typedef GaussianKernel<T>                   GaussianKernelType;
    typedef std::shared_ptr<GaussianKernelType>         GaussianKernelTypePointer;
    typedef PeriodicKernel<T>                   PeriodicKernelType;
    typedef std::shared_ptr<PeriodicKernelType>         PeriodicKernelTypePointer;
    typedef ProductKernel<T>                        ProductKernelType;
    typedef std::shared_ptr<ProductKernelType>      ProductKernelTypePointer;

    PeriodicKernelTypePointer   pk(new PeriodicKernelType(0.59, 0.5, 0.4));
    GaussianKernelTypePointer   gk(new GaussianKernelType(132, M_PI));
    ProductKernelTypePointer        sk(new ProductKernelType(pk, gk));

    ProductKernelTypePointer sk2(new ProductKernelType(PeriodicKernelTypePointer(new PeriodicKernelType(1, 1, 1)),
                                               GaussianKernelTypePointer(new GaussianKernelType(1, 1))));

    sk2->SetParameters(sk->GetParameters());

    if((*sk) != (*sk2)){
        throw std::string("kernels are not equal");
    }
    else{
        std::cout << " [passed]." << std::endl;
    }

}

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

    try{
        std::cout << "Test 1: Product kernel test (float): " << std::endl;
        Test1<float>();
        std::cout << "Test 1: Product kernel test (double): " << std::endl;
        Test1<double>();

        std::cout << "Test 2: parameter test with product kernel (float) " << std::flush;
        Test2<float>();
        std::cout << "Test 2: parameter test with product kernel (double) " << std::flush;
        Test2<double>();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


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

 * amsi Copyright 2019, Positioning and Navigation Laboratory,
 * Hong Kong Polytechnic University
 * All Rights Reserved
 * Authors: Weisong Wen, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file gps_imu_loose_ekf.cpp
 * @brief fuse gps (ENU) and imu (ENU) using ekf (loosely coupling)
 * @author Weisong Wen (weisong.wen@connect.polyu.hk)
 */

/**
 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS

 *  - we read IMU and GPS data from rosbag, with the following format:
 *  A topic with "/imu/data" is an imu measurement
 *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
 *  A topic with "/ublox_gps_node/fix" is a gps correction formatted with
 *  lat, lon, altitude
 */
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <vector>
#include <iostream>

#include <Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/Geometry>
// fstream
#include <fstream>
#include<sstream>
#include <stdlib.h>
#include <iomanip>

#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

// math
#include <math.h>
//time
#include <time.h>
//algorithm
#include <algorithm>
// Define Infinite (Using INT_MAX caused overflow problems)

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/NavSatFix.h>
#include <novatel_msgs/BESTPOS.h> // novatel_msgs/INSPVAX

#include <amsi/gnss_tools.hpp>

#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/PointCloud2.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <visualization_msgs/MarkerArray.h>

#include <amsi/gnss_tools.hpp>

#define INF 10000
#define pi 3.1415926

using namespace Eigen;

using namespace std;

FILE* fp_out_loose_ekf = fopen("/home/husai/gps_imu_loose_ekf.csv", "w+");

// FILE* generateData = fopen("/home/wenws/libRSF/datasets/smartLoc/data_kowloon_rang3.txt", "w+");

// FILE* generateData_odom = fopen("/home/wenws/libRSF/datasets/smartLoc/data_kowloon_odom.txt", "w+");

// FILE* generateData_gt3 = fopen("/home/wenws/libRSF/datasets/smartLoc/data_kowloon_gt3.txt", "w+");

// FILE* generateData_imu = fopen("/home/wenws/libRSF/datasets/smartLoc/data_kowloon_imu.txt", "w+");

class ekf_loose
{
  ros::NodeHandle nh;

public:

  typedef struct
  {
    double wx;
    double wy;
    double wz;
    double ax;
    double ay;
    double az;
  }imu_bias; // imu bias

  typedef struct
  {
    double wx;
    double wy;
    double wz;
    double ax;
    double ay;
    double az;
  }imu_noise; // imu bias

  typedef struct
  {
    imu_bias bias;
    imu_noise noise;
  }imu_para; // imu bias

  typedef struct
  {
    double mean;
    double std;
  }statistical_para; // imu bias || positioning evaluation

  typedef struct
  {
    double x;
    double y;
    double z;
    double ID;
  }bp; // building point

  ros::Subscriber imu_sub;
  ros::Subscriber gps_sub;
  ros::Subscriber gps_raw_sub;
  ros::Subscriber span_BP_sub, dgps_nmea_sub;

  ros::Publisher ekf_loose_Pose_pub;
  ros::Publisher ekf_GNSS_odom_pub;
  ros::Publisher ekf_span_odom_pub;
  ros::Publisher gnss_navsat_pose_pub;

  sensor_msgs::Imu imu_track;
  bool imu_up =0 ;
  int imu_co=0;

  bool gps_up = 0;
  nav_msgs::Odometry odom_track; // GNSS SPP in ENU
  nav_msgs::Odometry span_odom; // span_cpt in ENU
  double span_gps_week_sec = 0;
  nav_msgs::Odometry ekf_pose_odom; // ekf pose in ENU
  // gnss_tools
  GNSS_Tools gnss_tools_;
  Eigen::MatrixXd originllh,originllh_span; // origin llh
  Eigen::MatrixXd referencellh; // origin llh
  sensor_msgs::NavSatFix ini_navf,ini_navf_span ; // initial sensor msg

  imu_bias offline_cal= {-0.00231128, 0.0019349, -0.000309033,
                        -0.0000563799, -0.0004587, 0.0979159}; // offiline calibrated imu bias and noise

  imu_para imu_parameter; // online calibrated imu parameters using begin 2000 frames of imu raw measurements
  bool use_online_imu_cal = 1;
  bool online_cal_success = 0;
  vector<sensor_msgs::Imu> imu_queue;
  int imu_queue_size = 100;

  // EKF related parameters
  Eigen::MatrixXd imu_noise_matrix;

  Eigen::MatrixXd Q_matrix, G_matrix, sigma_matrix, R_matrix, K_matrix, H_matrix, orientation_matrix, acceleration_rotation;

  VectorXd ekf_state; // state:  px, py, pz, vx, vy, vz, bax, bzy, baz
  VectorXd ekf_u_t; // ax, ay, az
  VectorXd ekf_z_t; // px, py, pz

  double prediction_pre_t=0;

  vector<double> posi_err, posi_err_gps;

  Eigen::MatrixXd span_ecef; //

  double nstamps_ = 0;

  double x,y,z,w;

public:
  /**
   * @brief constructor
   * @param imu_msg
   */
  ekf_loose(bool state)
  {
    std::cout<<"----------------constructor-----------------"<<std::endl;
    // imu_sub = nh.subscribe("/imu/data", 50, &ekf_loose::imu_callback,this); // imu_rt
    imu_sub = nh.subscribe("/imu_raw", 50, &ekf_loose::imu_callback,this); // imu_rt
    gps_sub = nh.subscribe("/ublox_gps_node/fix", 50, &ekf_loose::ubloxFix_callback,this);
    dgps_nmea_sub =nh.subscribe("/nmea_sentence", 50, &ekf_loose::nmea_callback,this);

    gnss_navsat_pose_pub = nh.advertise<sensor_msgs::NavSatFix>("/ublox_gps_node/fix2", 10);

    ekf_loose_Pose_pub = nh.advertise<nav_msgs::Odometry>("/ekf_loose_Pose", 10);
    ekf_GNSS_odom_pub = nh.advertise<nav_msgs::Odometry>("/ekf_gnss_odom", 10);
    ekf_span_odom_pub = nh.advertise<nav_msgs::Odometry>("/ekf_span_odom", 10);

    onInitEKF(); // initialize EKF related parameters
    // fprintf(fp_out_loose_ekf, "%s ,%s ,%s ,%s ,%s ,%s, %s, %s, %s \n", "epoch", "GPS_Eror", "GPS_IMU_loose__Eror", "gps_E", "gps_N", "loose_E", "loose_N", "Gt_E", "Gt_N");
    // fprintf(generateData, "%s %s %s %s %s %s %s %s %s \n", "epoch", "GPS_Eror", "GPS_IMU_loose__Eror", "gps_E", "gps_N", "loose_E", "loose_N", "Gt_E", "Gt_N");

  }

  ~ekf_loose()
  {
  }

  /**
   * @brief imu callback
   * @param imu msg
   * @return void
   @
   */
  void onInitEKF(void)
  {
    ekf_state.resize(9);
    ekf_state << 0,0,0,
                 0,0,0,
                 0,0,0;

    ekf_u_t.resize(3);
    ekf_u_t << 0,0,0;

    ekf_z_t.resize(3);
    ekf_z_t << 0,0,0;

    Q_matrix.resize(9,9);
    Q_matrix << 0.0002, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0.0002, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0.0002, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0.0001, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0.0001, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0.0001, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0.05, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0.05, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0.05;
    Q_matrix = Q_matrix * 1;

    sigma_matrix.resize(9,9);
    sigma_matrix << 3.0, 0, 0, 0, 0, 0, 0, 0, 0,
                    0, 3.0, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 3.2, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0.3, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0.3, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0.3, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0.07, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0.07, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0.07;

    G_matrix.resize(9,9);

    R_matrix.resize(3,3);
    R_matrix << 50,0,0,
                0,50,0,
                0,0,50;

    K_matrix.resize(9,3);
    H_matrix.resize(3,9);
    H_matrix << 1, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 1, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 1, 0, 0, 0, 0, 0, 0;
  }

  /**
   * @brief imu callback
   * @param imu msg
   * @return void
   @
   */
  void imu_callback(const sensor_msgs::Imu::Ptr& input)
  {
    // std::cout << " IMU data call back" << input->angular_velocity.x << std::endl;
    imu_track = * input;
    x = imu_track.orientation.x;
    y = imu_track.orientation.y;
    z = imu_track.orientation.z;
    w = imu_track.orientation.w;

    double imu_roll, imu_pitch, imu_yaw;
    tf::Quaternion imu_orientation;
    tf::quaternionMsgToTF(input->orientation, imu_orientation);
    tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
    cout<<"yaw : = " <<imu_yaw*(180/3.14)<<endl;
    // cout<<imu_track.orientation.x<<endl;
    cout<<"imu_track.linear_acceleration.x : = " <<endl<<imu_track.linear_acceleration.x<<endl;

    Eigen::Quaterniond q(w,x,y,z);
    q.normalized();
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix=q.toRotationMatrix();
    orientation_matrix.resize(3,1);
    orientation_matrix<<imu_track.linear_acceleration.x,
                        imu_track.linear_acceleration.y,
                        imu_track.linear_acceleration.z;
    acceleration_rotation = rotation_matrix.transpose() * orientation_matrix;

    cout<<"acceleration_rotation : = " <<endl<<acceleration_rotation<<endl;
    imu_track.linear_acceleration.x = acceleration_rotation(0,0);
    imu_track.linear_acceleration.y = acceleration_rotation(1,0);
    imu_track.linear_acceleration.z = acceleration_rotation(2,0);
    cout<<"imu_track.linear_acceleration.x after rotation: = " <<endl<<imu_track.linear_acceleration.x<<endl;
    cout<<"imu_track.linear_acceleration.y after rotation: = " <<endl<<imu_track.linear_acceleration.y<<endl;
    cout<<"imu_track.linear_acceleration.z after rotation: = " <<endl<<imu_track.linear_acceleration.z<<endl;
    cout<<"-----------------------------" <<endl;

    imu_queue.push_back(imu_track);
    // cout<<"imu_queue_size -> "<<imu_queue.size()<<endl;
    if( (imu_queue.size() > imu_queue_size) && (online_cal_success ==0) && (use_online_imu_cal ==1))
    {
      online_cal_success = 1;
      const clock_t begin_time = clock();
      cout<<"-------------------start calibrate imu------------------------ "<<endl;
      imu_parameter = imu_calibtation(imu_queue);
      std::cout << "imu Calibration used  time -> " << double(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";
      // cout<<"bias ax ->" <<imu_parameter.bias.ax<<endl;
      // cout<<"bias ay ->" <<imu_parameter.bias.ay<<endl;
      // cout<<"bias az ->" <<imu_parameter.bias.az<<endl;

      // cout<<"bias wx ->" <<imu_parameter.bias.wx<<endl;
      // cout<<"bias wy ->" <<imu_parameter.bias.wy<<endl;
      // cout<<"bias wz ->" <<imu_parameter.bias.wz<<endl;
    }
    if(use_online_imu_cal)
    {
        // decrease the bias
      imu_track.angular_velocity.x = imu_track.angular_velocity.x - (imu_parameter.bias.wx);
      imu_track.angular_velocity.y = imu_track.angular_velocity.y - (imu_parameter.bias.wy);
      imu_track.angular_velocity.z = imu_track.angular_velocity.z - (imu_parameter.bias.wz);

      imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (imu_parameter.bias.ax);
      imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (imu_parameter.bias.ay);
      imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (imu_parameter.bias.az);
    }
    else if(!use_online_imu_cal) // if do not online calibrate, use offiline calibration (1h statistical static data)
    {
        // decrease the bias
      imu_track.angular_velocity.x = imu_track.angular_velocity.x - (-0.00231128);
      imu_track.angular_velocity.y = imu_track.angular_velocity.y - (0.0019349);
      imu_track.angular_velocity.z = imu_track.angular_velocity.z - (-0.000309033);

      imu_track.linear_acceleration.x = imu_track.linear_acceleration.x - (-0.0000563799);
      imu_track.linear_acceleration.y = imu_track.linear_acceleration.y - (-0.0004587);
      imu_track.linear_acceleration.z = imu_track.linear_acceleration.z - (0.0979159);
    }

    imu_up =1;
    imu_co++;

    ekf_u_t(0) = imu_track.linear_acceleration.x;
    ekf_u_t(1) = imu_track.linear_acceleration.y;
    ekf_u_t(2) = imu_track.linear_acceleration.z;

    if(prediction_pre_t == 0)
    {
      prediction_pre_t = imu_track.header.stamp.toSec();
    }

    double delta_t = imu_track.header.stamp.toSec() -  prediction_pre_t;

    if(online_cal_success)
    {
      // cout<< "delta_t-> "<<delta_t;
        // position prediction
      ekf_state(0) = ekf_state(0) + ekf_state(3) * delta_t + 1/2 * (ekf_u_t(0) -
        ekf_state(6)) * pow(delta_t, 2);
      ekf_state(1) = ekf_state(1) + ekf_state(4) * delta_t + 1/2 * (ekf_u_t(1) -
        ekf_state(7)) * pow(delta_t, 2);
      ekf_state(2) = ekf_state(2) + ekf_state(5) * delta_t + 1/2 * (ekf_u_t(2) -
        ekf_state(8)) * pow(delta_t, 2);

      // velocity estimation
      ekf_state(3) = ekf_state(3) + (ekf_u_t(0) - ekf_state(6)) * delta_t;
      ekf_state(4) = ekf_state(4) + (ekf_u_t(1) - ekf_state(7)) * delta_t;
      ekf_state(5) = ekf_state(5) + (ekf_u_t(2) - ekf_state(8)) * delta_t;

      // bias prediction
      ekf_state(6) = ekf_state(6);
      ekf_state(7) = ekf_state(7);
      ekf_state(8) = ekf_state(8);

      G_matrix << 1, 0, 0, delta_t, 0, 0, -1/2 * pow(delta_t,2), 0, 0,
                  0, 1, 0, 0, delta_t, 0, 0, -1/2 * pow(delta_t,2), 0,
                  0, 0, 1, 0, 0, delta_t, 0, 0, -1/2 * pow(delta_t,2),
                  0, 0, 0, 1, 0, 0, -delta_t, 0, 0,
                  0, 0, 0, 0, 1, 0, 0, -delta_t, 0,
                  0, 0, 0, 0, 0, 1, 0, 0, -delta_t,
                  0, 0, 0, 0, 0, 0, 1, 0, 0,
                  0, 0, 0, 0, 0, 0, 0, 1, 0,
                  0, 0, 0, 0, 0, 0, 0, 0, 1;

      // sigma matrix prediction
      sigma_matrix = G_matrix * sigma_matrix * G_matrix.transpose() + Q_matrix;
      // cout<< "sigma_matrix-> "<<sigma_matrix<<endl;

      ekf_pose_odom.header = imu_track.header;
      ekf_pose_odom.header.frame_id = "odom";
      ekf_pose_odom.pose.pose.position.x = ekf_state(0);
      ekf_pose_odom.pose.pose.position.y = ekf_state(1);
      ekf_pose_odom.pose.pose.position.z = ekf_state(2);
      // ekf_loose_Pose_pub.publish(ekf_pose_odom);
    }

    prediction_pre_t = imu_track.header.stamp.toSec();
  }

   /**
   * @brief imu bias, noise calculation (calibration)
   * @param imu queue
   * @return imu_para
   @
   */
  imu_para imu_calibtation(vector<sensor_msgs::Imu> input)
  {
    imu_para para; // bias + noise
    imu_bias bias; // bias of imu
    imu_noise noise; // noise of imu

    // cout<<"imu bias calculation (calibration)"<<endl;
    vector<double> wx_queue;
    double wx_sum = 0, wx_mean = 0;

    vector<double> wy_queue;
    double wy_sum = 0, wy_mean = 0;

    vector<double> wz_queue;
    double wz_sum = 0, wz_mean = 0;

    vector<double> ax_queue;
    double ax_sum = 0, ax_mean = 0;

    vector<double> ay_queue;
    double ay_sum = 0, ay_mean = 0;

    vector<double> az_queue;
    double az_sum = 0, az_mean = 0;

    for(int i = 0; i < input.size()-1; i ++)
    {
      wx_queue.push_back(input[i].angular_velocity.x);
      wy_queue.push_back(input[i].angular_velocity.y);
      wz_queue.push_back(input[i].angular_velocity.z);

      ax_queue.push_back(input[i].linear_acceleration.x);
      ay_queue.push_back(input[i].linear_acceleration.y);
      az_queue.push_back(input[i].linear_acceleration.z);
    }

    statistical_para result = statis_cal(wx_queue);
    para.bias.wx = result.mean;
    para.noise.wx = result.std;

    result = statis_cal(wy_queue);
    para.bias.wy = result.mean;
    para.noise.wy = result.std;

    result = statis_cal(wz_queue);
    para.bias.wz = result.mean;
    para.noise.wz = result.std;

    result = statis_cal(ax_queue);
    para.bias.ax = result.mean;
    para.noise.ax = result.std;

    result = statis_cal(ay_queue);
    para.bias.ay = result.mean;
    para.noise.ay = result.std;

    result = statis_cal(az_queue);
    para.bias.az = result.mean;
    para.noise.az = result.std;

    return para;
  }

  /**
   * @brief statistical parameters inference
   * @param vector<double>
   * @return statistical_para
   @
   */
  statistical_para statis_cal(vector<double> input)
  {
    statistical_para result;
    double sum = std::accumulate(std::begin(input), std::end(input), 0.0);
    double mean =  sum / input.size();

    double accum  = 0.0;
    std::for_each (std::begin(input), std::end(input), [&](const double d) {
      accum  += (d-mean)*(d-mean);
    });

    double stdev = sqrt(accum/(input.size()-1));

    result.mean = mean;
    result.std = stdev;
    return result;
  }

void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) {
    std::vector<std::string> str_vec_ptr;
    std::string token;
    std::stringstream ss(nmea_msg->sentence);
    bool find_SOL_COMPUTED =0;
    while (getline(ss, token, ' '))
    {
      if(token == "SOL_COMPUTED") // solutions are computed
      {
        // std::cout<<"message obtained"<<std::endl;
        find_SOL_COMPUTED = true;
      }
      if( find_SOL_COMPUTED ) // find flag SOL_COMPUTED
      {
        str_vec_ptr.push_back(token);
      }
    }
        if(find_SOL_COMPUTED)
    {
      sensor_msgs::NavSatFix navfix_ ;
      navfix_.header = nmea_msg->header;
      std::cout << std::setprecision(17);
      double lat = strtod((str_vec_ptr[2]).c_str(), NULL);
      double lon = strtod((str_vec_ptr[3]).c_str(), NULL);
      double alt = strtod((str_vec_ptr[4]).c_str(), NULL);
      std::cout << std::setprecision(17);

      navfix_.latitude = lat;
      navfix_.longitude = lon;
      navfix_.altitude = alt;
      if(ini_navf.latitude == NULL)
      {
        ini_navf = navfix_;
        std::cout<<"ini_navf.header  -> "<<ini_navf.header<<std::endl;
        originllh_span.resize(3, 1);
        originllh_span(0) = navfix_.longitude;
        originllh_span(1) = navfix_.latitude;
        originllh_span(2) = navfix_.altitude;
        std::cout<<"reference longitude: "<<navfix_.longitude<<std::endl;
        std::cout<<"reference latitude: "<<navfix_.latitude<<std::endl;
      }
      Eigen::MatrixXd curLLh; //
      curLLh.resize(3, 1);
      curLLh(0) = navfix_.longitude;
      curLLh(1) = navfix_.latitude;
      curLLh(2) = navfix_.altitude;

      Eigen::MatrixXd ecef; //
      ecef.resize(3, 1);
      ecef = gnss_tools_.llh2ecef(curLLh);
      Eigen::MatrixXd eigenENU;; //
      eigenENU.resize(3, 1);
      eigenENU = gnss_tools_.ecef2enu(originllh_span,ecef);

      span_odom.header.frame_id = "odom";
      span_odom.pose.pose.position.x = eigenENU(0);
      span_odom.pose.pose.position.y = eigenENU(1);
      span_odom.pose.pose.position.z = eigenENU(2);
      span_odom.pose.pose.orientation.x = 0;
      span_odom.pose.pose.orientation.y = 0;
      span_odom.pose.pose.orientation.z = 0;
      span_odom.pose.pose.orientation.w = 0;
      ekf_span_odom_pub.publish(span_odom);

      std::cout<<"push back message to gps_queue..."<<std::endl;
    }
  }

  /**
   * @brief gps callback
   * @param gps fix msg
   * @return void
   @
   */
  void ubloxFix_callback(const sensor_msgs::NavSatFixConstPtr& fix_msg) // update
  {
    cout<<"ubloxFix_callback received "<<endl;
    sensor_msgs::NavSatFix navfix_ ;
    navfix_.header = fix_msg->header;
    navfix_.latitude = fix_msg->latitude;
    navfix_.longitude = fix_msg->longitude;
    navfix_.altitude = fix_msg->altitude;

    if(ini_navf.latitude == NULL)
      {
        ini_navf = navfix_;
        // std::cout<<"ini_navf.header  -> "<<ini_navf.header<<std::endl;
        originllh.resize(3, 1);
        originllh(0) = navfix_.longitude;
        originllh(1) = navfix_.latitude;
        originllh(2) = navfix_.altitude;
        // std::cout<<"reference longitude: "<<navfix_.longitude<<std::endl;
        // std::cout<<"reference latitude: "<<navfix_.latitude<<std::endl;
      }
      Eigen::MatrixXd curLLh; //
      curLLh.resize(3, 1);
      curLLh(0) = navfix_.longitude;
      curLLh(1) = navfix_.latitude;
      curLLh(2) = navfix_.altitude;

      Eigen::MatrixXd ecef; //
      ecef.resize(3, 1);
      ecef = gnss_tools_.llh2ecef(curLLh);
      Eigen::MatrixXd eigenENU;; //
      eigenENU.resize(3, 1);
      eigenENU = gnss_tools_.ecef2enu(originllh_span,ecef);
      /*
      gps(0) = odom_track.pose.pose.position.x;
      gps(1) = odom_track.pose.pose.position.y;
      gps(2) = odom_track.pose.pose.position.z;
      gps(3) = odom_track.pose.pose.orientation.x;
      gps(4) = odom_track.pose.pose.orientation.y;
      gps(5) = odom_track.pose.pose.orientation.z;
      gps(6) = 1;
      */
      odom_track.header.frame_id = "odom";
      odom_track.pose.pose.position.x = eigenENU(0);
      odom_track.pose.pose.position.y = eigenENU(1);
      odom_track.pose.pose.position.z = 0;
      odom_track.pose.pose.orientation.x = 0;
      odom_track.pose.pose.orientation.y = 0;
      odom_track.pose.pose.orientation.z = 0;
      odom_track.pose.pose.orientation.w = 0;
      if(originllh_span.size()) // initial llh from span-cpt is available
      {
        gps_up = 1;
      }

      ekf_GNSS_odom_pub.publish(odom_track);

      ekf_z_t(0) = odom_track.pose.pose.position.x;
      ekf_z_t(1) = odom_track.pose.pose.position.y;
      ekf_z_t(2) = odom_track.pose.pose.position.z;

      if(online_cal_success) // imu calibration ready
      {
        // update

        // update K_matrix
        Eigen::MatrixXd I_matrix;
        I_matrix.resize(9,9);
        I_matrix.setIdentity();
        K_matrix = sigma_matrix * H_matrix.transpose() * (H_matrix * sigma_matrix *
          H_matrix.transpose() + R_matrix).inverse();
        ekf_state = ekf_state + K_matrix * (ekf_z_t - H_matrix * ekf_state);
        sigma_matrix = (I_matrix - K_matrix * H_matrix) * sigma_matrix;
        cout<<"ekf_state-> " <<ekf_state<<endl;

        ekf_pose_odom.header = navfix_.header;
        ekf_pose_odom.header.frame_id = "odom";
        ekf_pose_odom.pose.pose.position.x = ekf_state(0);
        ekf_pose_odom.pose.pose.position.y = ekf_state(1);
        ekf_pose_odom.pose.pose.position.z = ekf_state(2);
        ekf_loose_Pose_pub.publish(ekf_pose_odom);

        //2D position error between loose ekf and span
        double error_ekf = sqrt(pow(ekf_pose_odom.pose.pose.position.x - span_odom.pose.pose.position.x,2) + pow(ekf_pose_odom.pose.pose.position.y - span_odom.pose.pose.position.y,2));
        posi_err.push_back(error_ekf);
        statistical_para result = statis_cal(posi_err);
        cout<<" positioning error (loose ekf) mean:->" << result.mean
          << " positioning error (loose ekf) std: ->" << result.std;
        //2D position error between gps and span
        double error_gps = sqrt(pow(odom_track.pose.pose.position.x - span_odom.pose.pose.position.x,2) + pow(odom_track.pose.pose.position.y - span_odom.pose.pose.position.y,2));
        posi_err_gps.push_back(error_gps);
        result = statis_cal(posi_err_gps);
        cout<<" positioning error (gps) mean:->" << result.mean <<
         " positioning error (gps) std: ->" << result.std;

        fprintf(fp_out_loose_ekf, "%d ,%3.2f ,%3.2f ,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f \n", posi_err.size(), error_gps, error_ekf,
          odom_track.pose.pose.position.x, odom_track.pose.pose.position.y, ekf_pose_odom.pose.pose.position.x, ekf_pose_odom.pose.pose.position.y,
          span_odom.pose.pose.position.x , span_odom.pose.pose.position.y);

      }

  }

  /**
   * @brief  least square for signle point positioning
   * @param eAllSVPositions ((n,4) prn, sx, sy, sz, )     eAllSVPositions ((n,3) PRN CNO Pseudorange)
   * @return eWLSSolution 5 unknowns with two clock bias variables
   @
  */
  Eigen::MatrixXd LeastSquare(Eigen::MatrixXd eAllSVPositions, Eigen::MatrixXd eAllMeasurement){

    Eigen::MatrixXd eWLSSolution;
    eWLSSolution.resize(5, 1);

    /**after read the obs file, one measure is not right**/
    int validNumMeasure=0;
    std::vector<int> validMeasure;
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          validNumMeasure++;
          validMeasure.push_back(int(eAllMeasurement(idx, 0)));
        }
      }
    }

    Eigen::MatrixXd validMeasurement; // for WLS
    validMeasurement.resize(validNumMeasure,eAllMeasurement.cols());
    for (int idx = 0; idx < eAllMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(eAllMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllMeasurement.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            validMeasurement(idx, kdx) = eAllMeasurement(idx, kdx);

          }
        }
      }
    }

    int iNumSV = validMeasurement.rows();

    /*Find the received SV and Sort based on the order of Measurement matrix*/
    Eigen::MatrixXd eExistingSVPositions; // for WLS
    eExistingSVPositions.resize(iNumSV, eAllSVPositions.cols());

    for (int idx = 0; idx < validMeasurement.rows(); idx++){
      for (int jdx = 0; jdx < eAllSVPositions.rows(); jdx++){
        if (int(validMeasurement(idx, 0)) == int(eAllSVPositions(jdx, 0))){
          for (int kdx = 0; kdx < eAllSVPositions.cols(); kdx++){
            // std::cout<<"satellite prn -> "<<eAllMeasurement(idx, 0)<<"\n"<<std::endl;
            eExistingSVPositions(idx, kdx) = eAllSVPositions(jdx, kdx);

          }
        }
      }
    }
    //for (int idx = 0; idx < eExistingSVPositions.rows(); idx++){
    //  printf("%2d-[%3d] - (%10.2f,%10.2f,%10.2f) %f\n", idx, int(eExistingSVPositions(idx, 0)), eExistingSVPositions(idx, 1), eExistingSVPositions(idx, 2), eExistingSVPositions(idx, 3), eExistingSVPositions(idx, 4)*CLIGHT);
    //}

    //Intialize the result by guessing.
    for (int idx = 0; idx < eWLSSolution.rows(); idx++){
      eWLSSolution(idx, 0) = 0;
    }

    // for the case of insufficient satellite
    if (iNumSV < 5){
      return eWLSSolution;
    }

    bool bWLSConverge = false;

    int count = 0;
    while (!bWLSConverge)
    {
      Eigen::MatrixXd eH_Matrix;
      eH_Matrix.resize(iNumSV, eWLSSolution.rows());

      Eigen::MatrixXd eDeltaPr;
      eDeltaPr.resize(iNumSV, 1);

      Eigen::MatrixXd eDeltaPos;
      eDeltaPos.resize(eWLSSolution.rows(), 1);

      for (int idx = 0; idx < iNumSV; idx++){

        int prn = int(validMeasurement(idx, 0));
        double pr = validMeasurement(idx, 2);

        // Calculating Geometric Distance
        double rs[3], rr[3], e[3];
        double dGeoDistance;

        rs[0] = eExistingSVPositions(idx, 1);
        rs[1] = eExistingSVPositions(idx, 2);
        rs[2] = eExistingSVPositions(idx, 3);

        rr[0] = eWLSSolution(0);
        rr[1] = eWLSSolution(1);
        rr[2] = eWLSSolution(2);

        // dGeoDistance = geodist(rs, rr, e);
        dGeoDistance = sqrt(pow((rs[0] - rr[0]),2) + pow((rs[1] - rr[1]),2) +pow((rs[2] - rr[2]),2));

        // Making H matrix
        eH_Matrix(idx, 0) = -(rs[0] - rr[0]) / dGeoDistance;
        eH_Matrix(idx, 1) = -(rs[1] - rr[1]) / dGeoDistance;
        eH_Matrix(idx, 2) = -(rs[2] - rr[2]) / dGeoDistance;

        if (PRNisGPS(prn)){
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 0;
        }
        else if (PRNisBeidou(prn))
        {
          eH_Matrix(idx, 3) = 1;
          eH_Matrix(idx, 4) = 1;
        }

        // Making delta pseudorange
        double rcv_clk_bias;
        if (PRNisGPS(prn)){
          rcv_clk_bias = eWLSSolution(3);
        }
        else if (PRNisBeidou(prn))
        {
          rcv_clk_bias = eWLSSolution(4);
        }
        // double sv_clk_bias = eExistingSVPositions(idx, 4) * CLIGHT;
        eDeltaPr(idx, 0) = pr - dGeoDistance + rcv_clk_bias;
        //printf("%2d - %f %f %f %f \n", prn, pr, dGeoDistance, eDeltaPr(idx, 0), rcv_clk_bias);
      }

      // Least Square Estimation
      eDeltaPos = (eH_Matrix.transpose() * eH_Matrix).ldlt().solve(eH_Matrix.transpose() *  eDeltaPr);
      //eDeltaPos = (eH_Matrix.transpose() * eH_Matrix).inverse() * eH_Matrix.transpose() *  eDeltaPr;
      //eDeltaPos = eH_Matrix.householderQr().solve(eDeltaPr);

      //for (int idx = 0; idx < eDeltaPos.rows(); idx++)
      //  printf("%f ", eDeltaPos(idx));
      //printf("\n");

      eWLSSolution(0) += eDeltaPos(0);
      eWLSSolution(1) += eDeltaPos(1);
      eWLSSolution(2) += eDeltaPos(2);
      eWLSSolution(3) += eDeltaPos(3);
      eWLSSolution(4) += eDeltaPos(4);

      for (int i = 0; i < 3; ++i){
        //printf("%f\n", fabs(eDeltaPos(i)));
        if (fabs(eDeltaPos(i)) >1e-4)
        {
          bWLSConverge = false;
        }
        else {
          bWLSConverge = true;
        };

      }
      count += 1;
      if (count > 6)
        bWLSConverge = true;
    }
    // printf("WLS -> (%11.2f,%11.2f,%11.2f)\n\n", eWLSSolution(0), eWLSSolution(1), eWLSSolution(2));
    std::cout << std::setprecision(12);
    // cout<< "---------------WLS (ECEF) x, y, z, bias_gps, bias_beidou-----------------  \n"<<eWLSSolution<<endl;

    return eWLSSolution;
  }

  /**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisGPS(int prn)
  {
    if (prn <= 32 || prn == 84)
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisGLONASS(int prn)
  {
    if (prn > 32 && prn <= 56)
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief satellite set validation
   * @param prn
   * @return ture/false
   @
   */
  bool PRNisBeidou(int prn)
  {
    if ((prn <= 121) && (prn >= 87))
      return true;
    else{
      return false;
    }
  }

  /**
   * @brief covariance estimation
   * @param nlosExclusion::GNSS_Raw_Array GNSS_data
   * @return weight_matrix
   @
   */
  double cofactorMatrixCal_single_satellite(double ele, double snr)
  {
    double cofactor_ = 0;
    double snr_1 = 50.0; // T = 50
    double snr_A = 30.0; // A = 30
    double snr_a = 30.0;// a = 30
    double snr_0 = 10.0; // F = 10

    double snr_R = snr;
    double elR = ele;
    double q_R_1 = 1 / (pow(( sin(elR * pi/180.0 )),2));
    double q_R_2 = pow(10,(-(snr_R - snr_1) / snr_a));
    double q_R_3 = (((snr_A / (pow(10,(-(snr_0 - snr_1) / snr_a))) - 1) / (snr_0 - snr_1)) * (snr_R - snr_1) + 1);
    double q_R = q_R_1* (q_R_2 * q_R_3);
    cofactor_ = (float(q_R)); // uncertainty: cofactor_[i] larger, larger uncertainty

    return cofactor_;
  }

  /**
   * @brief delay function
   * @param seconds for delay
   * @return void
   @
   */
  void wait(int seconds) // delay function
  {
    clock_t endwait,start;
    start = clock();
    endwait = clock() + seconds * CLOCKS_PER_SEC;
    while (clock() < endwait) {
      if(clock() - start > CLOCKS_PER_SEC)
      {
        start = clock();
        std::cout<<".......1 s"<<std::endl;
      }
    }
  }

private:
  int reserve1;

private:
  ros::Publisher pub_debug_marker_; // marker publisher
  ros::Publisher marker_pub;
  visualization_msgs::MarkerArray markers; // markers for building models

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ekf_loose");
  std::cout<<"ekf_loose......"<<std::endl;

  // printf("obss.n = %d\n", obss.n);
  ekf_loose ekf_loose_(1);
  ros::spin();
  while (ros::ok()) {
  }
  return 0;
}


#include "test_data.hpp"
#include <boost/test/unit_test.hpp>
#include "libs/ml_models/K_nearest.h"
#include <iostream>

BOOST_AUTO_TEST_CASE(empty_data_knear_test)
{
    mlmodels::K_nearest model;
    auto m = model.train(mlmodels::training_data{},
            mlmodels::class_data{},
            mlmodels::K_nearest::args{}
        );
    BOOST_TEST((m.get() == nullptr), "the model that we should get back from empty data set should be empty as well");
    mlmodels::testing_result out;
    auto r = model.predict(m, mlmodels::testing_data{}, out);
    BOOST_TEST(r == false, "should failed because training did not run");
    BOOST_TEST(out.empty(), "there should not be any data from the test");
}

BOOST_AUTO_TEST_CASE(test_model_create_knear)
{
    const auto& samples = ut_data::unittest_data();
    const auto& classes = ut_data::classes4ut();
    BOOST_TEST_REQUIRE(mlmodels::rows(samples) == classes.size());

    mlmodels::K_nearest knear;
    auto m = knear.train(samples,
                classes,
                mlmodels::K_nearest::args{}
        );
    BOOST_TEST_REQUIRE((m.get() != nullptr), "we are expecting that this train would succeed");
    const auto& test_samples = ut_data::samples4test();
    BOOST_TEST_REQUIRE(mlmodels::rows(test_samples) < mlmodels::rows(samples));
    BOOST_TEST_REQUIRE(mlmodels::columns(test_samples) == mlmodels::columns(samples));
    auto p = knear.test(m, test_samples, ut_data::NUMBER_OF_CLASSES);
    BOOST_TEST_REQUIRE(!p.empty(), "we are expecting to get entries from the test");
    BOOST_TEST_REQUIRE(p.size() == mlmodels::rows(test_samples));
    //mlmodels::save_model(m);
#ifdef PRINT_MODEL_RESULTS
    std::cout<<"model prediction for k nearest\n";
    for (auto f : p) {
        std::cout<<"["<<f<<"]";
    }
    std::cout<<std::endl;
#endif
}
#if 0
BOOST_AUTO_TEST_CASE(test_model_create_knear_to_remove)
{
    const auto& samples = ut_data::unittest_data();
    const auto& classes = ut_data::classes4ut();
    BOOST_TEST_REQUIRE(mlmodels::rows(samples) == classes.size());

    mlmodels::K_nearest knear;
    auto m = knear.train(samples,
                    classes, mlmodels::K_nearest::args{}
        );
    BOOST_TEST_REQUIRE((m.get() != nullptr), "we are expecting that this train would succeed");
    const auto& test_samples = ut_data::samples4test();
    BOOST_TEST_REQUIRE(mlmodels::rows(test_samples) < mlmodels::rows(samples));
    BOOST_TEST_REQUIRE(mlmodels::columns(test_samples) == mlmodels::columns(samples));
    //auto p = knear.test(m, test_samples, ut_data::NUMBER_OF_CLASSES);
    mlmodels::class_data res, dist, nr;
    BOOST_TEST_REQUIRE(knear.test(m, test_samples, res, dist, nr));
    BOOST_TEST_REQUIRE(!res.empty(), "we are expecting to get entries from the test");
    BOOST_TEST_REQUIRE(res.size() == mlmodels::rows(test_samples));
    //mlmodels::save_model(m);
    std::cout<<"model prediction for k nearest\n";
    for (auto f : res) {
        std::cout<<"["<<f<<"]";
    }
    std::cout<<std::endl;
    std::cout<<"distance array of size "<<dist.size()<<std::endl;
    auto i = 0u;
    for (auto d : dist) {
        std::cout<<"["<<d<<"]";
        if ((++i % res.size()) == 0) {
            std::cout<<"\n";
        }
    }
    std::cout<<std::endl;
    std::cout<<"neighbor responses array of size "<<dist.size()<<std::endl;
    i = 0u;
    for (auto r : nr) {
        std::cout<<"["<<r<<"]";
        if ((++i % res.size()) == 0) {
            std::cout<<"\n";
        }
    }
}
#endif


//  (C) Copyright Eric Niebler 2008.
//  Use, modification and distribution are 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 <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_sum.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::rolling_sum> > acc(tag::rolling_window::window_size = 3);

    BOOST_CHECK_EQUAL(0, rolling_sum(acc));

    acc(1);
    BOOST_CHECK_EQUAL(1, rolling_sum(acc));

    acc(2);
    BOOST_CHECK_EQUAL(3, rolling_sum(acc));

    acc(3);
    BOOST_CHECK_EQUAL(6, rolling_sum(acc));

    acc(4);
    BOOST_CHECK_EQUAL(9, rolling_sum(acc));

    acc(5);
    BOOST_CHECK_EQUAL(12, rolling_sum(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("rolling sum test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/orientation.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <CGAL/Polygon_mesh_processing/transform.h>
#include <boost/core/ref.hpp>

#include <iostream>
#include <fstream>
#include <sstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3> Surface_mesh;
typedef Kernel::Aff_transformation_3 Trsfrm;

namespace PMP = CGAL::Polygon_mesh_processing;
namespace params = CGAL::parameters;

int main()
{
  Surface_mesh base_cube;
  std::ifstream input("data-coref/cube.off");
  input >> base_cube;

  Surface_mesh input_mesh;
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> vol_id_map =
    input_mesh.add_property_map<Surface_mesh::Face_index, std::size_t>().first;

  // add large cube
  input_mesh.join(base_cube);

  // add middle cube
  Surface_mesh middle_cube = base_cube;
  PMP::transform(Trsfrm(CGAL::SCALING, 0.5), middle_cube);
  PMP::transform(Trsfrm(CGAL::TRANSLATION, Kernel::Vector_3(0.25,0.25,0.25)), middle_cube);
  input_mesh.join(middle_cube);

  // add central cube
  Surface_mesh small_cube = base_cube;
  PMP::transform(Trsfrm(CGAL::SCALING, 0.1), small_cube);
  PMP::transform(Trsfrm(CGAL::TRANSLATION, Kernel::Vector_3(0.5,0.5,0.5)), small_cube);
  input_mesh.join(small_cube);

  // test using orientation
  std::vector<PMP::Volume_error_code> expected_result;
  expected_result.push_back(PMP::VALID_VOLUME);
  expected_result.push_back(PMP::INCOMPATIBLE_ORIENTATION);
  expected_result.push_back(PMP::INCOMPATIBLE_ORIENTATION);
  std::vector<PMP::Volume_error_code>  error_codes;
  std::size_t nb_vol = PMP::volume_connected_components(input_mesh, vol_id_map,
                                                        params::error_codes(boost::ref(error_codes)));
  assert(nb_vol==3);
  std::sort(error_codes.begin(), error_codes.end());
  assert( error_codes==expected_result );

  // test ignoring orientation
  nb_vol = PMP::volume_connected_components(input_mesh, vol_id_map,
                                            params::do_orientation_tests(false)
                                            .error_codes(boost::ref(error_codes)));
  assert(nb_vol==2);
  expected_result.clear();
  expected_result.resize(2, PMP::VALID_VOLUME);
  assert( error_codes==expected_result );

  // test surface component self-intersection
  {
  Surface_mesh tmp = input_mesh;
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> tmp_vol_id_map =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>().first;
  // create a self-intersection
  Surface_mesh::Halfedge_index h = *tmp.halfedges().begin();
  h = CGAL::Euler::split_edge(h, tmp);
  tmp.point( target(h, tmp) ) = tmp.point( target( tmp.next(h), tmp) );
  CGAL::Euler::split_face(h, next( next(h, tmp), tmp), tmp);
  h = opposite(h, tmp);
  CGAL::Euler::split_face(h, next( next(h, tmp), tmp), tmp);

  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(false)
                                            .error_codes(boost::ref(error_codes))
                                            .do_self_intersection_tests(true));

  assert(nb_vol==2);
  expected_result.clear();
  expected_result.push_back(PMP::VALID_VOLUME);
  expected_result.push_back(PMP::SURFACE_WITH_SELF_INTERSECTIONS);
  std::sort(error_codes.begin(), error_codes.end());
  assert( error_codes==expected_result );
  }

  // test single cc
  {
  Surface_mesh tmp = base_cube;
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> tmp_vol_id_map =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>().first;

  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(false)
                                            .error_codes(boost::ref(error_codes))
                                            .do_self_intersection_tests(true));

  assert(nb_vol==1);
  expected_result.clear();
  expected_result.push_back(PMP::VALID_VOLUME);
  assert( error_codes==expected_result );
  }

  // test intersection between cc
  {
  Surface_mesh tmp = input_mesh;
  // create surface intersection
  tmp.join(base_cube);
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> tmp_vol_id_map =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>().first;

  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(false)
                                            .error_codes(boost::ref(error_codes))
                                            .do_self_intersection_tests(true));

  assert(nb_vol==4);
  expected_result.clear();
  expected_result.resize(4, PMP::VOLUME_INTERSECTION);
  assert( error_codes==expected_result );
  }
  {
  Surface_mesh tmp = input_mesh;
  // create surface intersection
  tmp.join(middle_cube);
  tmp.join(middle_cube);
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> fccmap =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>("f:CC").first;

  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> tmp_vol_id_map =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>().first;
  std::vector< std::vector<std::size_t> > nested_cc_per_cc;

  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(false)
                                            .error_codes(boost::ref(error_codes))
                                            .face_connected_component_map(fccmap)
                                            .do_self_intersection_tests(true)
                                            .volume_inclusions(boost::ref(nested_cc_per_cc)));

  assert(nb_vol==5);
  expected_result.clear();
  expected_result.push_back(PMP::VALID_VOLUME);
  expected_result.resize(5, PMP::VOLUME_INTERSECTION);
  std::sort(error_codes.begin(), error_codes.end());
  assert( error_codes==expected_result );
  std::size_t sum=0;
  for (int i=0; i<5; ++i)
  {
    sum+=nested_cc_per_cc[i].size();
    assert(nested_cc_per_cc[i].size()==1 ||
           nested_cc_per_cc[i].size()==3 ||
           nested_cc_per_cc[i].size()==0);
  }
  assert(sum == 0+3*1+3);
  }

  // test level 0 orientation
  {
  Surface_mesh tmp = base_cube;
  PMP::transform(Trsfrm(CGAL::TRANSLATION, Kernel::Vector_3(2,0,0)), tmp);
  tmp.join(base_cube);
  PMP::reverse_face_orientations(tmp);
  Surface_mesh::Property_map<Surface_mesh::Face_index, std::size_t> tmp_vol_id_map =
    tmp.add_property_map<Surface_mesh::Face_index, std::size_t>().first;
  // create a self-intersection
  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(false));

  assert(nb_vol==2);
  nb_vol = PMP::volume_connected_components(tmp, tmp_vol_id_map,
                                            params::do_orientation_tests(true));

  assert(nb_vol==1);
  }
  // debug code
  /*
    std::cout << "  found " << nb_vol << " volumes\n";
    typedef CGAL::Face_filtered_graph<Surface_mesh> Filtered_graph;
    Filtered_graph vol_mesh(sm, 0, vol_id_map);
    for(std::size_t id = 0; id < nb_vol; ++id)
    {
      if(id > 0)
        vol_mesh.set_selected_faces(id, vol_id_map);
      Surface_mesh out;
      CGAL::copy_face_graph(vol_mesh, out);
      std::ostringstream oss;
      oss << "vol_" << id <<".off";
      std::ofstream os(oss.str().data());
      os << out;
    }
  */
}


/*
Copyright (c) 2016 Bastien Durix

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 *  \brief 2D shape evaluation
 *  \author Bastien Durix
 */

#include <boost/program_options.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include <shape/DiscreteShape.h>
#include <boundary/DiscreteBoundary.h>

#include <algorithm/extractboundary/NaiveBoundary.h>
#include <algorithm/evaluation/ShapeError.h>

int main(int argc, char** argv)
{
	std::string imgref, imgcmp;

	boost::program_options::options_description desc("OPTIONS");

	desc.add_options()
		("help", "Help message")
		("imgref", boost::program_options::value<std::string>(&imgref)->default_value("img1.png"), "Reference binary image file")
		("imgcmp", boost::program_options::value<std::string>(&imgcmp)->default_value("img2.png"), "Compaired binary image file")
		;

	boost::program_options::variables_map vm;
	boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
	boost::program_options::notify(vm);

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

	cv::Mat shprefgray = cv::imread(imgref,cv::ImreadModes::IMREAD_GRAYSCALE);
	cv::Mat shpref;
	cv::threshold(shprefgray,shpref,1,255,cv::THRESH_BINARY);
	shape::DiscreteShape<2>::Ptr disshref = shape::DiscreteShape<2>::Ptr(new shape::DiscreteShape<2>(shpref.cols,shpref.rows));

	cv::Mat shpcmpgray = cv::imread(imgcmp,cv::ImreadModes::IMREAD_GRAYSCALE);
	cv::Mat shpcmp;
	cv::threshold(shpcmpgray,shpcmp,1,255,cv::THRESH_BINARY);
	shape::DiscreteShape<2>::Ptr disshcmp = shape::DiscreteShape<2>::Ptr(new shape::DiscreteShape<2>(shpcmp.cols,shpcmp.rows));

	double symdiff = algorithm::evaluation::SymDiffArea(disshref,disshcmp);

	std::cout << "Symmetric area difference :  " << symdiff << std::endl;

	boundary::DiscreteBoundary<2>::Ptr bndref = algorithm::extractboundary::NaiveBoundary(disshref);
	boundary::DiscreteBoundary<2>::Ptr bndcmp = algorithm::extractboundary::NaiveBoundary(disshcmp);

	double hausdist = algorithm::evaluation::HausDist(bndref,bndcmp);

	std::cout << "Hausdorff Distance :  " << hausdist << std::endl;

	return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

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

#include <tdp/eigen/std_vector.h>

namespace tdp {
/// This is a simple version of the DPvMFmeansSimple algorithm in dpmeans.hpp
/// without inheritance or the use of CLData structures which can make
/// it a bit hard to read the other algorithm.
///
/// This implementation is ment as a lightweight alternative for small
/// number of datapoints or if you just want to have a look at how the
/// algorithm works.
template<class T, int D, int Options>
class DPvMFmeansSimple
{
public:
  /// Constructor
  ///
  /// lambda = cos(lambda_in_degree * M_PI/180.) - 1.
  DPvMFmeansSimple(T lambda);
  virtual ~DPvMFmeansSimple();

  /// Adds an observation (adds obs, computes label, and potentially
  /// adds new cluster depending on label assignment).
  virtual void addObservation(Eigen::Matrix<T,D,1,Options>* x, uint16_t* z);
  /// Updates all labels of all data currently stored with the object.
  virtual void updateLabels();
  /// Updates all centers based on the current data and label
  /// assignments.
  virtual void updateCenters();

  /// Iterate updates for centers and labels until cost function
  /// convergence.
  virtual bool iterateToConvergence(uint32_t maxIter, T eps);
  /// Compuyte the current cost function value.
  virtual T cost();

  uint32_t GetK() const {return K_;};
  const std::vector<int32_t>& GetNs() const {return Ns_;};
  bool GetCenter(uint32_t k, Eigen::Matrix<T,D,1,Options>& mu) const {
    if (k<K_) {mu = mus_[k]; return true; } else { return false; } };
  const Eigen::Matrix<T,D,1,Options>& GetCenter(uint32_t k) const {
    if (k<K_) {return mus_[k]; } else { return Eigen::Matrix<T,D,1,Options>::Zero(); } };
  const std::vector<uint16_t*>& GetZs() const { return zs_;};
  bool GetX(uint32_t i, Eigen::Matrix<T,D,1,Options>& x) const {
    if (i<xs_.size()) {x=*xs_[i]; return true; } else { return false; } };

protected:
  T lambda_;
  uint32_t K_;
  std::vector<Eigen::Matrix<T,D,1,Options>*> xs_;
  std::vector<uint16_t*> zs_;
  eigen_vector<Eigen::Matrix<T,D,1,Options>> mus_;
  eigen_vector<Eigen::Matrix<T,D,1,Options>> xSums_;
  std::vector<int32_t> Ns_;

  /// resets all clusters (mus_ and Ks_) and resizes them to K_
  void resetClusters();
  /// Removes all empty clusters.
  void removeEmptyClusters();
  /// Computes the index of the closest cluster (may be K_ in which
  /// case a new cluster has to be added).
  uint16_t indOfClosestCluster(const Eigen::Matrix<T,D,1,Options>& x, T& sim_closest,
      uint16_t* zExclude=nullptr);
};

typedef DPvMFmeansSimple<float,3,Eigen::DontAlign> DPvMFmeansSimple3fda;
typedef DPvMFmeansSimple<float,4,Eigen::DontAlign> DPvMFmeansSimple4fda;

// -------------------------------- impl ----------------------------------
template<class T, int D, int Options>
DPvMFmeansSimple<T,D,Options>::DPvMFmeansSimple(T lambda)
  : lambda_(lambda), K_(0)
{}
template<class T, int D, int Options>
DPvMFmeansSimple<T,D,Options>::~DPvMFmeansSimple()
{}

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::addObservation(Eigen::Matrix<T,D,1,Options>* x, uint16_t* z) {
  xs_.push_back(x);
  T sim_closest = 0;
  zs_.push_back(z);
//  if (*z == 0xFFFF) return;
  *z = indOfClosestCluster(*x, sim_closest);
  if (*z == K_) {
    mus_.push_back(*x);
    xSums_.push_back(*x);
    Ns_.push_back(0);
    ++K_;
//    std::cout << "adding cluster " << mus_.size() << " "
//      << xSums_.size() << " " << K_ << " "
//      << x.transpose() << std::endl;
  }
  Ns_[*z] ++;
};

template<class T, int D, int Options>
uint16_t DPvMFmeansSimple<T,D,Options>::indOfClosestCluster(const
    Eigen::Matrix<T,D,1,Options>& x, T& sim_closest, uint16_t* zExclude)
{
  uint16_t z_i = K_;
  sim_closest = lambda_;
  for (uint32_t k=0; k<K_; ++k) {
    if (zExclude && k == *zExclude)
      continue;
    T sim_k = mus_[k].dot(x);
    if(sim_k > sim_closest) {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::updateLabels()
{
  if (xs_.size() == 0) return;
  for(uint32_t i=0; i<xs_.size(); ++i) {
    T sim_closest = 0;
    uint16_t zPrev = *zs_[i];
//    if (zPrev == 0xFFFF) continue;
    uint16_t z = indOfClosestCluster(*xs_[i], sim_closest);
    if (z==zPrev && Ns_[z] == 1) {
      z = indOfClosestCluster(*xs_[i], sim_closest, &z);
//      std::cout << "single cluster " << z << " " << zPrev << std::endl;
    }
    if (z == K_) {
//      std::cout << "adding cluster " << mus_.size() << " "
//        << xSums_.size() << " " << K_ << " "
//        << xs_[i].transpose() << std::endl;
      mus_.push_back(*xs_[i]);
      xSums_.push_back(Eigen::Matrix<T,D,1,Options>::Zero());
      Ns_.push_back(0);
      ++K_;
    }
    if (z != zPrev) {
      Ns_[zPrev] --;
      xSums_[zPrev] -= *xs_[i];
      Ns_[z] ++;
      xSums_[z] += *xs_[i];
    }
    *zs_[i] = z;
  }
};

// General update centers assumes Euclidean
template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::updateCenters()
{
  if (xs_.size() == 0) return;
//  resetClusters();
//  for(uint32_t i=0; i<xs_.size(); ++i) {
//    ++Ns_[zs_[i]];
//    mus_[zs_[i]] += xs_[i];
//  }
  // Spherical mean computation
  for(uint32_t k=0; k<K_; ++k) {
//    mus_[k] /= mus_[k].norm();
    mus_[k] = xSums_[k].normalized();
  }
  removeEmptyClusters();
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::resetClusters() {
  Ns_.resize(K_, 0);
  for(uint32_t k=0; k<K_; ++k) {
    mus_[k].fill(0);
    Ns_[k] = 0;
  }
};

template<class T, int D, int Options>
void DPvMFmeansSimple<T,D,Options>::removeEmptyClusters() {
  if (K_ < 1) return;
  uint32_t kNew = K_;
  std::vector<bool> toDelete(K_,false);
  for(int32_t k=K_-1; k>-1; --k)
    if(Ns_[k] == 0) {
      toDelete[k] = true;
//      std::cout<<"cluster k "<<k<<" empty"<<std::endl;
//#pragma omp parallel for
      for(uint32_t i=0; i<xs_.size(); ++i)
//        if(static_cast<int32_t>(*zs_[i]) >= k && *zs_[i] != 0xFFFF) *zs_[i] -= 1;
        if(static_cast<int32_t>(*zs_[i]) >= k) *zs_[i] -= 1;
      kNew --;
    }
  uint32_t j=0;
  for(uint32_t k=0; k<K_; ++k) {
    mus_[j] = mus_[k];
    xSums_[j] = xSums_[k];
    Ns_[j] = Ns_[k];
    if(!toDelete[k]) {
      ++j;
    }
  }
//  std::cout << "K " << K_ << " -> " << kNew << std::endl;
  K_ = kNew;
  Ns_.resize(K_);
  mus_.resize(K_);
  xSums_.resize(K_);
//  for(uint32_t k=0; k<K_; ++k)
//    std::cout << mus_[k].transpose() << std::endl;
};

template<class T, int D, int Options>
T DPvMFmeansSimple<T,D,Options>::cost() {
  T f = lambda_*K_;
//  std::cout << "f="<<f<< std::endl;
  for(uint32_t i=0; i<xs_.size(); ++i)  {
//    if (*zs_[i] == 0xFFFF) continue;
    f += mus_[*zs_[i]].dot(*xs_[i]);
//    std::cout << zs_[i] << ", " << xs_[i].transpose() << ", "
//      << mus_[zs_[i]].transpose();
//    std::cout << " f="<<f<< std::endl;
  }
  return f;
}

template<class T, int D, int Options>
bool DPvMFmeansSimple<T,D,Options>::iterateToConvergence(uint32_t maxIter, T eps) {
  uint32_t iter = 0;
  T fPrev = 1e9;
  T f = cost();
  updateCenters();
//  std::cout << "f=" << f << " fPrev=" << fPrev << std::endl;
  while (iter < maxIter && fabs(fPrev - f)/f > eps) {
    updateLabels();
    updateCenters();
    fPrev = f;
    f = cost();
    ++iter;
//    std::cout << iter << ": f=" << f << " fPrev=" << fPrev << ": ";
//    int32_t Nall = 0;
//    for (const auto& N : Ns_) {
//      std::cout << N << " ";
//      Nall += N;
//    } std::cout << " sum= " << Nall << std::endl;
  }
//  if (f != f || fPrev != fPrev || f > 1e9 || iter == maxIter-1) {
  std::cout << iter << ": f=" << f << " fPrev=" << fPrev << ": ";
  int32_t Nall = 0;
  for (const auto& N : Ns_) {
    std::cout << N << " ";
    Nall += N;
  } std::cout << " sum= " << Nall << std::endl;
//  }
  return iter < maxIter;
}

}


// This file is part of PolyMPC, a lightweight C++ template library
// for real-time nonlinear optimization and optimal control.
//
// Copyright (C) 2020 Listov Petr <petr.listov@epfl.ch>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

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

using namespace Eigen;

/*

Source : https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix/39146048
Usage:
MatrixXd A = load_csv<MatrixXd>("C:/Users/.../A.csv");
Matrix3d B = load_csv<Matrix3d>("C:/Users/.../B.csv");
VectorXd v = load_csv<VectorXd>("C:/Users/.../v.csv");
*/

//const double inf = std::numeric_limits<double>::infinity();

template<typename M>
M load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    //const char *exp[] = { "", "inf", "NaN" };
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ',')) {
            bool is_inf = false;
            /* for (int i=0; i < 3; i++)
            {
                if (exp[i] == cell){values.push_back(inf); is_inf=true; break;}
            }*/
            if (!is_inf) {values.push_back(std::stod(cell));}
        }
        ++rows;
    }
    //std::cout << rows << " " << values.size() << std::endl;

    return Eigen::Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, RowMajor>>(values.data(), rows, values.size()/rows);

}


/**
 * @file function_test.cpp
 * @author Ryan Curtin
 * @author Shikhar Bhardwaj
 *
 * Test the Function<> class to see that it properly adds functionality.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/function.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression_function.hpp>
#include <mlpack/core/optimizers/sdp/sdp.hpp>
#include <mlpack/core/optimizers/sdp/lrsdp.hpp>
#include <mlpack/core/optimizers/aug_lagrangian/aug_lagrangian.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::optimization::traits; // For some SFINAE checks.
using namespace mlpack::regression;

/**
 * Utility class with no functions.
 */
class EmptyTestFunction { };

/**
 * Utility class with Evaluate() but no Evaluate().
 */
class EvaluateTestFunction
{
 public:
  double Evaluate(const arma::mat& coordinates)
  {
    return arma::accu(coordinates);
  }

  double Evaluate(const arma::mat& coordinates,
                  const size_t begin,
                  const size_t batchSize)
  {
    return arma::accu(coordinates) + begin + batchSize;
  }
};

/**
 * Utility class with Gradient() but no Evaluate().
 */
class GradientTestFunction
{
 public:
  void Gradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }

  void Gradient(const arma::mat& coordinates,
                const size_t /* begin */,
                arma::mat& gradient,
                const size_t /* batchSize */)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }
};

/**
 * Utility class with Gradient() and Evaluate().
 */
class EvaluateGradientTestFunction
{
 public:
  double Evaluate(const arma::mat& coordinates)
  {
    return arma::accu(coordinates);
  }

  double Evaluate(const arma::mat& coordinates,
                  const size_t /* begin */,
                  const size_t /* batchSize */)
  {
    return arma::accu(coordinates);
  }

  void Gradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }

  void Gradient(const arma::mat& coordinates,
                const size_t /* begin */,
                arma::mat& gradient,
                const size_t /* batchSize */)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }
};

/**
 * Utility class with EvaluateWithGradient().
 */
class EvaluateWithGradientTestFunction
{
 public:
  double EvaluateWithGradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
    return arma::accu(coordinates);
  }

  double EvaluateWithGradient(const arma::mat& coordinates,
                              const size_t /* begin */,
                              arma::mat& gradient,
                              const size_t /* batchSize */)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
    return arma::accu(coordinates);
  }
};

/**
 * Utility class with all three functions.
 */
class EvaluateAndWithGradientTestFunction
{
 public:
  double Evaluate(const arma::mat& coordinates)
  {
    return arma::accu(coordinates);
  }

  double Evaluate(const arma::mat& coordinates,
                  const size_t begin,
                  const size_t batchSize)
  {
    return arma::accu(coordinates) + batchSize + begin;
  }

  void Gradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }

  void Gradient(const arma::mat& coordinates,
                const size_t /* begin */,
                arma::mat& gradient,
                const size_t /* batchSize */)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }

  double EvaluateWithGradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
    return arma::accu(coordinates);
  }

  double EvaluateWithGradient(const arma::mat& coordinates,
                              const size_t /* begin */,
                              arma::mat& gradient,
                              const size_t /* batchSize */)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
    return arma::accu(coordinates);
  }
};

/**
 * Utility class with const Evaluate() and non-const Gradient().
 */
class EvaluateAndNonConstGradientTestFunction
{
 public:
  double Evaluate(const arma::mat& coordinates) const
  {
    return arma::accu(coordinates);
  }

  void Gradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }
};

/**
 * Utility class with const Evaluate() and non-const Gradient().
 */
class EvaluateAndStaticGradientTestFunction
{
 public:
  double Evaluate(const arma::mat& coordinates) const
  {
    return arma::accu(coordinates);
  }

  static void Gradient(const arma::mat& coordinates, arma::mat& gradient)
  {
    gradient.ones(coordinates.n_rows, coordinates.n_cols);
  }
};

BOOST_AUTO_TEST_SUITE(FunctionTest);

/**
 * Make sure that an empty class doesn't have any methods added to it.
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientEmptyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<EmptyTestFunction>,
                                       EvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<EmptyTestFunction>,
                                       GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EmptyTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, false);
  BOOST_REQUIRE_EQUAL(hasGradient, false);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we don't add any functions if we only have Evaluate().
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientEvaluateOnlyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<EvaluateTestFunction>,
                                       EvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<EvaluateTestFunction>,
                                       GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, false);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we don't add any functions if we only have Gradient().
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientGradientOnlyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<GradientTestFunction>,
                                       EvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<GradientTestFunction>,
                                       GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<GradientTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, false);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we add EvaluateWithGradient() when we have both Evaluate() and
 * Gradient().
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientBothTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateGradientTestFunction>,
                           EvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateGradientTestFunction>,
                           GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateGradientTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we add Evaluate() and Gradient() when we have only
 * EvaluateWithGradient().
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientEvaluateWithGradientTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateWithGradientTestFunction>,
                           EvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateWithGradientTestFunction>,
                           GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateWithGradientTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we add no methods when we already have all three.
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientAllThreeTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateAndWithGradientTestFunction>,
                           EvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateAndWithGradientTestFunction>,
                           GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateAndWithGradientTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

BOOST_AUTO_TEST_CASE(LogisticRegressionEvaluateWithGradientTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<LogisticRegressionFunction<>>,
                           EvaluateConstForm>::value;
  const bool hasGradient =
      HasGradient<Function<LogisticRegressionFunction<>>,
                           GradientConstForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<LogisticRegressionFunction<>>,
                              EvaluateWithGradientConstForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

BOOST_AUTO_TEST_CASE(SDPTest)
{
  typedef AugLagrangianFunction<LRSDPFunction<SDP<arma::mat>>> FunctionType;

  const bool hasEvaluate =
      HasEvaluate<Function<FunctionType>, EvaluateConstForm>::value;
  const bool hasGradient =
      HasGradient<Function<FunctionType>, GradientConstForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<FunctionType>,
                              EvaluateWithGradientConstForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure that an empty class doesn't have any methods added to it.
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWithGradientEmptyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<EmptyTestFunction>,
                                       DecomposableEvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<EmptyTestFunction>,
                                       DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EmptyTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, false);
  BOOST_REQUIRE_EQUAL(hasGradient, false);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we don't add any functions if we only have Evaluate().
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWithGradientEvaluateOnlyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<EvaluateTestFunction>,
                                       DecomposableEvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<EvaluateTestFunction>,
                                       DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, false);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we don't add any functions if we only have Gradient().
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWithGradientGradientOnlyTest)
{
  const bool hasEvaluate = HasEvaluate<Function<GradientTestFunction>,
                                       DecomposableEvaluateForm>::value;
  const bool hasGradient = HasGradient<Function<GradientTestFunction>,
                                       DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<GradientTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, false);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, false);
}

/**
 * Make sure we add EvaluateWithGradient() when we have both Evaluate() and
 * Gradient().
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWithGradientBothTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateGradientTestFunction>,
                           DecomposableEvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateGradientTestFunction>,
                           DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateGradientTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we add Evaluate() and Gradient() when we have only
 * EvaluateWithGradient().
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWGradientEvaluateWithGradientTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateWithGradientTestFunction>,
                           DecomposableEvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateWithGradientTestFunction>,
                           DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateWithGradientTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  Function<EvaluateWithGradientTestFunction> f;
  arma::mat coordinates(10, 10, arma::fill::ones);
  arma::mat gradient;
  f.Gradient(coordinates, 0, gradient, 5);

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we add no methods when we already have all three.
 */
BOOST_AUTO_TEST_CASE(AddDecomposableEvaluateWithGradientAllThreeTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateAndWithGradientTestFunction>,
                  DecomposableEvaluateForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateAndWithGradientTestFunction>,
                           DecomposableGradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateAndWithGradientTestFunction>,
                              DecomposableEvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we can properly create EvaluateWithGradient() even when one of the
 * functions is non-const.
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientMixedTypesTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateAndNonConstGradientTestFunction>,
                  EvaluateConstForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateAndNonConstGradientTestFunction>,
                  GradientForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateAndNonConstGradientTestFunction>,
                              EvaluateWithGradientForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

/**
 * Make sure we can properly create EvaluateWithGradient() even when one of the
 * functions is static.
 */
BOOST_AUTO_TEST_CASE(AddEvaluateWithGradientMixedTypesStaticTest)
{
  const bool hasEvaluate =
      HasEvaluate<Function<EvaluateAndStaticGradientTestFunction>,
                  EvaluateConstForm>::value;
  const bool hasGradient =
      HasGradient<Function<EvaluateAndStaticGradientTestFunction>,
                  GradientStaticForm>::value;
  const bool hasEvaluateWithGradient =
      HasEvaluateWithGradient<Function<EvaluateAndStaticGradientTestFunction>,
                              EvaluateWithGradientConstForm>::value;

  BOOST_REQUIRE_EQUAL(hasEvaluate, true);
  BOOST_REQUIRE_EQUAL(hasGradient, true);
  BOOST_REQUIRE_EQUAL(hasEvaluateWithGradient, true);
}

class A
{
 public:
  size_t NumFunctions() const;
  size_t NumFeatures() const;
  double Evaluate(const arma::mat&, const size_t, const size_t) const;
  void Gradient(const arma::mat&, const size_t, arma::mat&, const size_t) const;
  void Gradient(const arma::mat&, const size_t, arma::sp_mat&, const size_t)
      const;
  void PartialGradient(const arma::mat&, const size_t, arma::sp_mat&) const;
};

class B
{
 public:
  size_t NumFunctions();
  size_t NumFeatures();
  double Evaluate(const arma::mat&, const size_t, const size_t);
  void Gradient(const arma::mat&, const size_t, arma::mat&, const size_t);
  void Gradient(const arma::mat&, const size_t, arma::sp_mat&, const size_t);
  void PartialGradient(const arma::mat&, const size_t, arma::sp_mat&);
};

class C
{
 public:
  size_t NumConstraints() const;
  double Evaluate(const arma::mat&) const;
  void Gradient(const arma::mat&, arma::mat&) const;
  double EvaluateConstraint(const size_t, const arma::mat&) const;
  void GradientConstraint(const size_t, const arma::mat&, arma::mat&) const;
};

class D
{
 public:
  size_t NumConstraints();
  double Evaluate(const arma::mat&);
  void Gradient(const arma::mat&, arma::mat&);
  double EvaluateConstraint(const size_t, const arma::mat&);
  void GradientConstraint(const size_t, const arma::mat&, arma::mat&);
};

/**
 * Test the correctness of the static check for DecomposableFunctionType API.
 */
BOOST_AUTO_TEST_CASE(DecomposableFunctionTypeCheckTest)
{
  static_assert(CheckNumFunctions<A>::value,
      "CheckNumFunctions static check failed.");
  static_assert(CheckNumFunctions<B>::value,
      "CheckNumFunctions static check failed.");
  static_assert(!CheckNumFunctions<C>::value,
      "CheckNumFunctions static check failed.");
  static_assert(!CheckNumFunctions<D>::value,
      "CheckNumFunctions static check failed.");

  static_assert(CheckDecomposableEvaluate<A>::value,
      "CheckDecomposableEvaluate static check failed.");
  static_assert(CheckDecomposableEvaluate<B>::value,
      "CheckDecomposableEvaluate static check failed.");
  static_assert(!CheckDecomposableEvaluate<C>::value,
      "CheckDecomposableEvaluate static check failed.");
  static_assert(!CheckDecomposableEvaluate<D>::value,
      "CheckDecomposableEvaluate static check failed.");

  static_assert(CheckDecomposableGradient<A>::value,
      "CheckDecomposableGradient static check failed.");
  static_assert(CheckDecomposableGradient<B>::value,
      "CheckDecomposableGradient static check failed.");
  static_assert(!CheckDecomposableGradient<C>::value,
      "CheckDecomposableGradient static check failed.");
  static_assert(!CheckDecomposableGradient<D>::value,
      "CheckDecomposableGradient static check failed.");
}

/**
 * Test the correctness of the static check for LagrangianFunctionType API.
 */
BOOST_AUTO_TEST_CASE(LagrangianFunctionTypeCheckTest)
{
  static_assert(!CheckEvaluate<A>::value, "CheckEvaluate static check failed.");
  static_assert(!CheckEvaluate<B>::value, "CheckEvaluate static check failed.");
  static_assert(CheckEvaluate<C>::value, "CheckEvaluate static check failed.");
  static_assert(CheckEvaluate<D>::value, "CheckEvaluate static check failed.");

  static_assert(!CheckGradient<A>::value, "CheckGradient static check failed.");
  static_assert(!CheckGradient<B>::value, "CheckGradient static check failed.");
  static_assert(CheckGradient<C>::value, "CheckGradient static check failed.");
  static_assert(CheckGradient<D>::value, "CheckGradient static check failed.");

  static_assert(!CheckNumConstraints<A>::value,
      "CheckNumConstraints static check failed.");
  static_assert(!CheckNumConstraints<B>::value,
      "CheckNumConstraints static check failed.");
  static_assert(CheckNumConstraints<C>::value,
      "CheckNumConstraints static check failed.");
  static_assert(CheckNumConstraints<D>::value,
      "CheckNumConstraints static check failed.");

  static_assert(!CheckEvaluateConstraint<A>::value,
      "CheckEvaluateConstraint static check failed.");
  static_assert(!CheckEvaluateConstraint<B>::value,
      "CheckEvaluateConstraint static check failed.");
  static_assert(CheckEvaluateConstraint<C>::value,
      "CheckEvaluateConstraint static check failed.");
  static_assert(CheckEvaluateConstraint<D>::value,
      "CheckEvaluateConstraint static check failed.");

  static_assert(!CheckGradientConstraint<A>::value,
      "CheckGradientConstraint static check failed.");
  static_assert(!CheckGradientConstraint<B>::value,
      "CheckGradientConstraint static check failed.");
  static_assert(CheckGradientConstraint<C>::value,
      "CheckGradientConstraint static check failed.");
  static_assert(CheckGradientConstraint<D>::value,
      "CheckGradientConstraint static check failed.");
}

/**
 * Test the correctness of the static check for SparseFunctionType API.
 */
BOOST_AUTO_TEST_CASE(SparseFunctionTypeCheckTest)
{
  static_assert(CheckSparseGradient<A>::value,
      "CheckSparseGradient static check failed.");
  static_assert(CheckSparseGradient<B>::value,
      "CheckSparseGradient static check failed.");
  static_assert(!CheckSparseGradient<C>::value,
      "CheckSparseGradient static check failed.");
  static_assert(!CheckSparseGradient<D>::value,
      "CheckSparseGradient static check failed.");
}

/**
 * Test the correctness of the static check for SparseFunctionType API.
 */
BOOST_AUTO_TEST_CASE(ResolvableFunctionTypeCheckTest)
{
  static_assert(CheckNumFeatures<A>::value,
      "CheckNumFeatures static check failed.");
  static_assert(CheckNumFeatures<B>::value,
      "CheckNumFeatures static check failed.");
  static_assert(!CheckNumFeatures<C>::value,
      "CheckNumFeatures static check failed.");
  static_assert(!CheckNumFeatures<D>::value,
      "CheckNumFeatures static check failed.");

  static_assert(CheckPartialGradient<A>::value,
      "CheckPartialGradient static check failed.");
  static_assert(CheckPartialGradient<B>::value,
      "CheckPartialGradient static check failed.");
  static_assert(!CheckPartialGradient<C>::value,
      "CheckPartialGradient static check failed.");
  static_assert(!CheckPartialGradient<D>::value,
      "CheckPartialGradient static check failed.");
}

BOOST_AUTO_TEST_SUITE_END();


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// Distributed under 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 "ublas_heev.hpp"

#include <boost/numeric/bindings/lapack/driver/hbev.hpp>
#include <boost/numeric/bindings/lapack/driver/sbev.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/mpl/if.hpp>

#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

template <typename U>
int lower()
{
  return 0;
}

template <>
int lower<ublas::lower>()
{
  return 2;
}

template <typename U>
int upper()
{
  return 0;
}

template <>
int upper<ublas::upper>()
{
  return 2;
}

template <typename T, typename W, typename UPLO, typename Orientation>
int do_memory_uplo(int n, W& workspace)
{
  typedef typename bindings::remove_imaginary<T>::type real_type ;

  typedef ublas::banded_matrix<T, Orientation>      banded_type ;
  typedef ublas::matrix<T, ublas::column_major>     matrix_type ;
  typedef ublas::vector<real_type>                  vector_type ;

  // Set matrix
  banded_type a(n, n, lower<UPLO>(), upper<UPLO>());
  a.clear(); // Make upper triangular band matrix
  matrix_type z(n, n);
  vector_type e1(n);
  vector_type e2(n);

  fill_banded(a);
  banded_type a2(a);

  ublas::hermitian_adaptor<banded_type, UPLO> h(a), h2(a2);

  // Compute Schur decomposition.
  lapack::hbev('V',
               h, e1, z, workspace) ;

  if(check_residual(h2, e1, z)) return 255 ;

  matrix_type dummy_z(n, n);
  lapack::hbev('N',
               h2, e2, dummy_z, workspace) ;
  if(norm_2(e1 - e2) > n * norm_2(e1) * std::numeric_limits< real_type >::epsilon()) return 255 ;

  // Test for a matrix range
  fill_banded(a);
  a2.assign(a);

  typedef ublas::matrix_range< banded_type > banded_range ;

  ublas::range r(1,n-1) ;
  banded_range a_r(a, r, r);
  ublas::hermitian_adaptor< banded_range, UPLO> h_r(a_r);
  ublas::vector_range< vector_type> e_r(e1, r);
  ublas::matrix_range< matrix_type> z_r(z, r, r);

  lapack::hbev('V',
               h_r, e_r, z_r, workspace);

  banded_range a2_r(a2, r, r);
  ublas::hermitian_adaptor< banded_range, UPLO> h2_r(a2_r);
  if(check_residual(h2_r, e_r, z_r)) return 255 ;

  return 0 ;
} // do_memory_uplo()

template <typename T, typename W>
int do_memory_type(int n, W workspace)
{
  std::cout << "  upper\n" ;
  if(do_memory_uplo<T,W,ublas::upper, ublas::row_major>(n, workspace)) return 255 ;
  std::cout << "  lower\n" ;
  if(do_memory_uplo<T,W,ublas::lower, ublas::row_major>(n, workspace)) return 255 ;
  return 0 ;
}

template <typename T>
struct Workspace
{
  typedef ublas::vector<T>                         array_type ;
  typedef lapack::detail::workspace1< array_type > type ;

  Workspace(size_t n)
    : work_(3*n-2)
  {}

  type operator()()
  {
    return lapack::workspace(work_) ;
  }

  array_type work_ ;
};

template <typename T>
struct Workspace< std::complex<T> >
{
  typedef ublas::vector<T>                                                 real_array_type ;
  typedef ublas::vector< std::complex<T> >                                 complex_array_type ;
  typedef lapack::detail::workspace2< complex_array_type,real_array_type > type ;

  Workspace(size_t n)
    : work_(n)
    , rwork_(3*n-2)
  {}

  type operator()()
  {
    return lapack::workspace(work_, rwork_) ;
  }

  complex_array_type work_ ;
  real_array_type    rwork_ ;
};

template <typename T>
int do_value_type()
{
  const int n = 8 ;

  std::cout << " optimal workspace\n";
  if(do_memory_type<T,lapack::optimal_workspace>(n, lapack::optimal_workspace())) return 255 ;

  std::cout << " minimal workspace\n";
  if(do_memory_type<T,lapack::minimal_workspace>(n, lapack::minimal_workspace())) return 255 ;

  std::cout << " workspace array\n";
  Workspace<T> work(n);
  if(do_memory_type<T,typename Workspace<T>::type >(n, work())) return 255 ;
  return 0;
} // do_value_type()

int main()
{
  // Run tests for different value_types
  std::cout << "float\n" ;
  if(do_value_type<float>()) return 255;

  std::cout << "double\n" ;
  if(do_value_type<double>()) return 255;

  std::cout << "complex<float>\n" ;
  if(do_value_type< std::complex<float> >()) return 255;

  std::cout << "complex<double>\n" ;
  if(do_value_type< std::complex<double> >()) return 255;

  std::cout << "Regression test succeeded\n" ;
  return 0;
}