/*
 * OpenVINS: An Open Platform for Visual-Inertial Research
 * Copyright (C) 2018-2022 Patrick Geneva
 * Copyright (C) 2018-2022 Guoquan Huang
 * Copyright (C) 2018-2022 OpenVINS Contributors
 * Copyright (C) 2018-2019 Kevin Eckenhoff
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#include <cmath>
#include <csignal>
#include <deque>
#include <iomanip>
#include <sstream>
#include <unistd.h>
#include <vector>

#include <ceres/ceres.h>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem.hpp>

#if ROS_AVAILABLE == 1
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#endif

#include "utils/colors.h"
#include "utils/sensor_data.h"

#include "ceres/Factor_GenericPrior.h"
#include "ceres/Factor_ImageReprojCalib.h"
#include "ceres/Factor_ImuCPIv1.h"
#include "ceres/State_JPLQuatLocal.h"
#include "init/InertialInitializerOptions.h"
#include "sim/Simulator.h"
#include "utils/helper.h"

using namespace ov_init;

// Define the function to be called when ctrl-c (SIGINT) is sent to process
void signal_callback_handler(int signum) { std::exit(signum); }

// Main function
int main(int argc, char **argv) {

  // Ensure we have a path, if the user passes it then we should use it
  std::string config_path = "unset_path_to_config.yaml";
  if (argc > 1) {
    config_path = argv[1];
  }

#if ROS_AVAILABLE == 1
  // Launch our ros node
  ros::init(argc, argv, "test_dynamic_mle");
  auto nh = std::make_shared<ros::NodeHandle>("~");
  nh->param<std::string>("config_path", config_path, config_path);

  // Topics to publish
  auto pub_pathimu = nh->advertise<nav_msgs::Path>("/ov_msckf/pathimu", 2);
  PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str());
  auto pub_pathgt = nh->advertise<nav_msgs::Path>("/ov_msckf/pathgt", 2);
  PRINT_DEBUG("Publishing: %s\n", pub_pathgt.getTopic().c_str());
  auto pub_loop_point = nh->advertise<sensor_msgs::PointCloud>("/ov_msckf/loop_feats", 2);
  PRINT_DEBUG("Publishing: %s\n", pub_loop_point.getTopic().c_str());
  auto pub_points_sim = nh->advertise<sensor_msgs::PointCloud2>("/ov_msckf/points_sim", 2);
  PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str());
#endif

  // Load the config
  auto parser = std::make_shared<ov_core::YamlParser>(config_path);
#if ROS_AVAILABLE == 1
  parser->set_node_handler(nh);
#endif

  // Verbosity
  std::string verbosity = "INFO";
  parser->parse_config("verbosity", verbosity);
  ov_core::Printer::setPrintLevel(verbosity);

  // Create the simulator
  InertialInitializerOptions params;
  params.print_and_load(parser);
  params.print_and_load_simulation(parser);
  if (!parser->successful()) {
    PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET);
    std::exit(EXIT_FAILURE);
  }
  Simulator sim(params);

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

  // Ceres problem stuff
  // NOTE: By default the problem takes ownership of the memory
  ceres::Problem problem;

  // Our system states (map from time to index)
  std::map<double, int> map_states;
  std::vector<double *> ceres_vars_ori;
  std::vector<double *> ceres_vars_pos;
  std::vector<double *> ceres_vars_vel;
  std::vector<double *> ceres_vars_bias_g;
  std::vector<double *> ceres_vars_bias_a;

  // Feature states (3dof p_FinG)
  std::map<size_t, int> map_features;
  std::vector<double *> ceres_vars_feat;
  typedef std::vector<std::pair<Factor_ImageReprojCalib *, std::vector<double *>>> factpair;
  std::map<size_t, factpair> map_features_delayed; // only valid as long as problem is

  // Setup extrinsic calibration q_ItoC, p_IinC (map from camera id to index)
  std::map<size_t, int> map_calib_cam2imu;
  std::vector<double *> ceres_vars_calib_cam2imu_ori;
  std::vector<double *> ceres_vars_calib_cam2imu_pos;

  // Setup intrinsic calibration focal, center, distortion (map from camera id to index)
  std::map<size_t, int> map_calib_cam;
  std::vector<double *> ceres_vars_calib_cam_intrinsics;

  // Set the optimization settings
  // NOTE: We use dense schur since after eliminating features we have a dense problem
  // NOTE: http://ceres-solver.org/solving_faqs.html#solving
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.trust_region_strategy_type = ceres::DOGLEG;
  // options.linear_solver_type = ceres::SPARSE_SCHUR;
  // options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  options.num_threads = 6;
  options.max_solver_time_in_seconds = 9999;
  options.max_num_iterations = 20;
  options.minimizer_progress_to_stdout = true;
  // options.use_nonmonotonic_steps = true;
  // options.function_tolerance = 1e-5;
  // options.gradient_tolerance = 1e-4 * options.function_tolerance;

  // DEBUG: check analytical jacobians using ceres finite differences
  // options.check_gradients = true;
  // options.gradient_check_relative_precision = 1e-4;
  // options.gradient_check_numeric_derivative_relative_step_size = 1e-8;

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

  // Random number generator used to perturb the groundtruth features
  // NOTE: It seems that if this too large it can really prevent good optimization
  // NOTE: Values greater 5cm seems to fail, not sure if this is reasonable or not...
  std::mt19937 gen_state_perturb(params.sim_seed_preturb);
  double feat_noise = 0.05; // meters

  // Buffer our camera image
  double buffer_timecam = -1;
  double buffer_timecam_last = -1;
  std::vector<int> buffer_camids;
  std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> buffer_feats;
  auto imu_readings = std::make_shared<std::vector<ov_core::ImuData>>();

  // Simulate!
  signal(SIGINT, signal_callback_handler);
  while (sim.ok()) {

    // IMU: get the next simulated IMU measurement if we have it
    ov_core::ImuData message_imu;
    bool hasimu = sim.get_next_imu(message_imu.timestamp, message_imu.wm, message_imu.am);
    if (hasimu) {
      imu_readings->push_back(message_imu);
    }

    // CAM: get the next simulated camera uv measurements if we have them
    double time_cam;
    std::vector<int> camids;
    std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> feats;
    bool hascam = sim.get_next_cam(time_cam, camids, feats);
    if (hascam) {
      if (buffer_timecam != -1) {

        // Get our predicted state at the requested camera timestep
        double timestamp_k = buffer_timecam_last;
        double timestamp_k1 = buffer_timecam;
        Eigen::Matrix<double, 16, 1> state_k1;
        std::shared_ptr<ov_core::CpiV1> cpi = nullptr;
        if (map_states.empty()) {

          // Add the first ever pose to the problem
          // TODO: do not initialize from the groundtruth pose
          double time1 = timestamp_k1 + sim.get_true_parameters().calib_camimu_dt;
          Eigen::Matrix<double, 17, 1> gt_imustate;
          assert(sim.get_state(time1, gt_imustate));
          state_k1 = gt_imustate.block(1, 0, 16, 1);

        } else {

          // Get our previous state timestamp (newest time) and biases to integrate with
          assert(timestamp_k != -1);
          // timestamp_k = map_states.rbegin()->first;
          Eigen::Vector4d quat_k;
          for (int i = 0; i < 4; i++) {
            quat_k(i) = ceres_vars_ori.at(map_states.at(timestamp_k))[i];
          }
          Eigen::Vector3d pos_k, vel_k, bias_g_k, bias_a_k;
          for (int i = 0; i < 3; i++) {
            pos_k(i) = ceres_vars_pos.at(map_states.at(timestamp_k))[i];
            vel_k(i) = ceres_vars_vel.at(map_states.at(timestamp_k))[i];
            bias_g_k(i) = ceres_vars_bias_g.at(map_states.at(timestamp_k))[i];
            bias_a_k(i) = ceres_vars_bias_a.at(map_states.at(timestamp_k))[i];
          }
          Eigen::Vector3d gravity;
          gravity << 0.0, 0.0, params.gravity_mag;

          // Preintegrate from previous state
          // Then append a new state with preintegration factor
          // TODO: estimate the timeoffset? don't use the true?
          double time0 = timestamp_k + sim.get_true_parameters().calib_camimu_dt;
          double time1 = timestamp_k1 + sim.get_true_parameters().calib_camimu_dt;
          auto readings = InitializerHelper::select_imu_readings(*imu_readings, time0, time1);
          cpi = std::make_shared<ov_core::CpiV1>(params.sigma_w, params.sigma_wb, params.sigma_a, params.sigma_ab, false);
          cpi->setLinearizationPoints(bias_g_k, bias_a_k, quat_k, gravity);
          for (size_t k = 0; k < readings.size() - 1; k++) {
            auto imu0 = readings.at(k);
            auto imu1 = readings.at(k + 1);
            cpi->feed_IMU(imu0.timestamp, imu1.timestamp, imu0.wm, imu0.am, imu1.wm, imu1.am);
          }
          assert(timestamp_k1 > timestamp_k);

          // Compute the predicted state
          state_k1.block(0, 0, 4, 1) = ov_core::quat_multiply(cpi->q_k2tau, quat_k);
          state_k1.block(4, 0, 3, 1) =
              pos_k + vel_k * cpi->DT - 0.5 * cpi->grav * std::pow(cpi->DT, 2) + ov_core::quat_2_Rot(quat_k).transpose() * cpi->alpha_tau;
          state_k1.block(7, 0, 3, 1) = vel_k - cpi->grav * cpi->DT + ov_core::quat_2_Rot(quat_k).transpose() * cpi->beta_tau;
          state_k1.block(10, 0, 3, 1) = bias_g_k;
          state_k1.block(13, 0, 3, 1) = bias_a_k;
        }

        // ================================================================
        //  ADDING GRAPH STATE / ESTIMATES!
        // ================================================================

        // Load our state variables into our allocated state pointers
        auto *var_ori = new double[4];
        for (int i = 0; i < 4; i++) {
          var_ori[i] = state_k1(0 + i, 0);
        }
        auto *var_pos = new double[3];
        auto *var_vel = new double[3];
        auto *var_bias_g = new double[3];
        auto *var_bias_a = new double[3];
        for (int i = 0; i < 3; i++) {
          var_pos[i] = state_k1(4 + i, 0);
          var_vel[i] = state_k1(7 + i, 0);
          var_bias_g[i] = state_k1(10 + i, 0);
          var_bias_a[i] = state_k1(13 + i, 0);
        }

        // Now actually create the parameter block in the ceres problem
        auto ceres_jplquat = new State_JPLQuatLocal();
        problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
        problem.AddParameterBlock(var_pos, 3);
        problem.AddParameterBlock(var_vel, 3);
        problem.AddParameterBlock(var_bias_g, 3);
        problem.AddParameterBlock(var_bias_a, 3);

        // Fix this first ever pose to constrain the problem
        // On the Comparison of Gauge Freedom Handling in Optimization-Based Visual-Inertial State Estimation
        // Zichao Zhang; Guillermo Gallego; Davide Scaramuzza
        // https://ieeexplore.ieee.org/document/8354808
        if (map_states.empty()) {

          // Construct state and prior
          Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
          for (int i = 0; i < 4; i++) {
            x_lin(0 + i) = var_ori[i];
          }
          for (int i = 0; i < 3; i++) {
            x_lin(4 + i) = var_pos[i];
            // x_lin(7 + i) = var_bias_g[i];
            // x_lin(10 + i) = var_bias_a[i];
          }
          Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(4, 1);
          Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(4, 4);
          prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-8, 2); // 4dof unobservable yaw and position
          // prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(1e-1, 2); // bias_g prior
          // prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(1e-1, 2); // bias_a prior

          // Construct state type and ceres parameter pointers
          std::vector<std::string> x_types;
          std::vector<double *> factor_params;
          factor_params.push_back(var_ori);
          x_types.emplace_back("quat_yaw");
          factor_params.push_back(var_pos);
          x_types.emplace_back("vec3");
          // factor_params.push_back(var_bias_g);
          // x_types.emplace_back("vec3");
          // factor_params.push_back(var_bias_a);
          // x_types.emplace_back("vec3");

          // Append it to the problem
          auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
          problem.AddResidualBlock(factor_prior, nullptr, factor_params);
          // problem.SetParameterBlockConstant(var_ori);
          // problem.SetParameterBlockConstant(var_pos);
        }

        // Append to our historical vector of states
        map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
        ceres_vars_ori.push_back(var_ori);
        ceres_vars_pos.push_back(var_pos);
        ceres_vars_vel.push_back(var_vel);
        ceres_vars_bias_g.push_back(var_bias_g);
        ceres_vars_bias_a.push_back(var_bias_a);
        buffer_timecam_last = timestamp_k1;

        // ================================================================
        //  ADDING GRAPH FACTORS!
        // ================================================================

        // Append the new IMU factor
        if (cpi != nullptr) {
          assert(timestamp_k != -1);
          std::vector<double *> factor_params;
          factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
          factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
          factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
          factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
          factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
          factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
          factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
          factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
          factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
          factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
          auto *factor_imu = new Factor_ImuCPIv1(cpi->DT, cpi->grav, cpi->alpha_tau, cpi->beta_tau, cpi->q_k2tau, cpi->b_a_lin,
                                                 cpi->b_w_lin, cpi->J_q, cpi->J_b, cpi->J_a, cpi->H_b, cpi->H_a, cpi->P_meas);
          problem.AddResidualBlock(factor_imu, nullptr, factor_params);
        }

        // Then, append new feature observations factors seen from this frame (initialize features as needed)
        // We first loop through each camera and for each we append measurements as needed
        assert(buffer_camids.size() == buffer_feats.size());
        for (size_t n = 0; n < buffer_camids.size(); n++) {

          // First make sure we have calibration states added
          int cam_id = buffer_camids.at(n);
          if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end()) {
            auto *var_calib_ori = new double[4];
            for (int i = 0; i < 4; i++) {
              var_calib_ori[i] = params.camera_extrinsics.at(cam_id)(0 + i, 0);
            }
            auto *var_calib_pos = new double[3];
            for (int i = 0; i < 3; i++) {
              var_calib_pos[i] = params.camera_extrinsics.at(cam_id)(4 + i, 0);
            }
            auto ceres_calib_jplquat = new State_JPLQuatLocal();
            problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
            problem.AddParameterBlock(var_calib_pos, 3);
            map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
            ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
            ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

            // Construct state and prior
            Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
            for (int i = 0; i < 4; i++) {
              x_lin(0 + i) = var_calib_ori[i];
            }
            for (int i = 0; i < 3; i++) {
              x_lin(4 + i) = var_calib_pos[i];
            }
            Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
            Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
            prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
            prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

            // Construct state type and ceres parameter pointers
            std::vector<std::string> x_types;
            std::vector<double *> factor_params;
            factor_params.push_back(var_calib_ori);
            x_types.emplace_back("quat");
            factor_params.push_back(var_calib_pos);
            x_types.emplace_back("vec3");
            if (!params.init_dyn_mle_opt_calib) {
              problem.SetParameterBlockConstant(var_calib_ori);
              problem.SetParameterBlockConstant(var_calib_pos);
            } else {
              auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
              problem.AddResidualBlock(factor_prior, nullptr, factor_params);
            }
          }
          bool is_fisheye = (std::dynamic_pointer_cast<ov_core::CamEqui>(params.camera_intrinsics.at(cam_id)) != nullptr);
          if (map_calib_cam.find(cam_id) == map_calib_cam.end()) {
            auto *var_calib_cam = new double[8];
            for (int i = 0; i < 8; i++) {
              var_calib_cam[i] = params.camera_intrinsics.at(cam_id)->get_value()(i, 0);
            }
            problem.AddParameterBlock(var_calib_cam, 8);
            map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
            ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

            // Construct state and prior
            Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
            for (int i = 0; i < 8; i++) {
              x_lin(0 + i) = var_calib_cam[i];
            }
            Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
            Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
            prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
            prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

            // Construct state type and ceres parameter pointers
            std::vector<std::string> x_types;
            std::vector<double *> factor_params;
            factor_params.push_back(var_calib_cam);
            x_types.emplace_back("vec8");
            if (!params.init_dyn_mle_opt_calib) {
              problem.SetParameterBlockConstant(var_calib_cam);
            } else {
              auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
              problem.AddResidualBlock(factor_prior, nullptr, factor_params);
            }
          }

          // Now loop through each feature observed at this k+1 timestamp
          for (auto const &feat : buffer_feats.at(n)) {
            size_t featid = feat.first;
            Eigen::Vector2d uv_raw;
            uv_raw << feat.second(0), feat.second(1);

            // Next make sure that we have the feature state
            // Normally, we should need to triangulate this once we have enough measurements
            // TODO: do not initialize features from the groundtruth map
            if (map_features.find(featid) == map_features.end()) {
              assert(params.use_stereo);
              Eigen::Vector3d p_FinG = sim.get_map().at(featid);
              auto *var_feat = new double[3];
              for (int i = 0; i < 3; i++) {
                std::normal_distribution<double> w(0, 1);
                var_feat[i] = p_FinG(i) + feat_noise * w(gen_state_perturb);
              }
              map_features.insert({featid, (int)ceres_vars_feat.size()});
              map_features_delayed.insert({featid, factpair()});
              ceres_vars_feat.push_back(var_feat);
            }

            // Factor parameters it is a function of
            std::vector<double *> factor_params;
            factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
            factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
            factor_params.push_back(ceres_vars_feat.at(map_features.at(featid)));
            factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
            factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
            factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
            auto *factor_pinhole = new Factor_ImageReprojCalib(uv_raw, params.sigma_pix, is_fisheye);
            if (map_features_delayed.find(featid) != map_features_delayed.end()) {
              map_features_delayed.at(featid).push_back({factor_pinhole, factor_params});
            } else {
              ceres::LossFunction *loss_function = nullptr;
              // ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
              problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
            }
          }
        }
      }
      buffer_timecam = time_cam;
      buffer_camids = camids;
      buffer_feats = feats;

      // If a delayed feature has enough measurements we can add it to the problem!
      // We will wait for enough observations before adding the parameter
      // This should make it much more stable since the parameter / optimization is more constrained
      size_t min_num_meas_to_optimize = 10;
      auto it0 = map_features_delayed.begin();
      while (it0 != map_features_delayed.end()) {
        size_t featid = (*it0).first;
        auto factors = (*it0).second;
        if (factors.size() >= min_num_meas_to_optimize) {
          problem.AddParameterBlock(ceres_vars_feat.at(map_features.at(featid)), 3);
          for (auto const &factorpair : factors) {
            auto factor_pinhole = factorpair.first;
            auto factor_params = factorpair.second;
            ceres::LossFunction *loss_function = nullptr;
            // ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
            problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
          }
          it0 = map_features_delayed.erase(it0);
        } else {
          it0++;
        }
      }

      // ================================================================
      //  PERFORM ACTUAL OPTIMIZATION!
      // ================================================================

      // We can try to optimize every few frames, but this can cause the IMU to drift
      // Thus this can't be too large, nor too small to reduce the computation
      if (map_states.size() % 10 == 0 && map_states.size() > min_num_meas_to_optimize) {

        // COMPUTE: error before optimization
        double error_before = 0.0;
        if (!map_features.empty()) {
          for (auto &feat : map_features) {
            Eigen::Vector3d pos1, pos2;
            pos1 << ceres_vars_feat.at(feat.second)[0], ceres_vars_feat.at(feat.second)[1], ceres_vars_feat.at(feat.second)[2];
            pos2 = sim.get_map().at(feat.first);
            error_before += (pos2 - pos1).norm();
          }
          error_before /= (double)map_features.size();
        }

        // Optimize the ceres graph
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        PRINT_INFO("[CERES]: %s\n", summary.message.c_str());
        PRINT_INFO("[CERES]: %d iterations | %zu states, %zu features | %d parameters and %d residuals | cost %.4e => %.4e\n",
                   (int)summary.iterations.size(), map_states.size(), map_features.size(), summary.num_parameters, summary.num_residuals,
                   summary.initial_cost, summary.final_cost);

        // COMPUTE: error after optimization
        double error_after = 0.0;
        if (!map_features.empty()) {
          for (auto &feat : map_features) {
            Eigen::Vector3d pos1, pos2;
            pos1 << ceres_vars_feat.at(feat.second)[0], ceres_vars_feat.at(feat.second)[1], ceres_vars_feat.at(feat.second)[2];
            pos2 = sim.get_map().at(feat.first);
            error_after += (pos2 - pos1).norm();
          }
          error_after /= (double)map_features.size();
        }
        PRINT_INFO("[CERES]: map error %.4f => %.4f (meters) for %zu features\n", error_before, error_after, map_features.size());

        // Print feature positions to console
        // https://gist.github.com/goldbattle/177a6b2cccb4b4208e687b0abae4bc9f
        //      for (auto &feat : map_features) {
        //        size_t featid = feat.first;
        //        std::cout << featid << ",";
        //        std::cout << ceres_vars_feat[map_features[featid]][0] << "," << ceres_vars_feat[map_features[featid]][1] << ","
        //                  << ceres_vars_feat[map_features[featid]][2] << ",";
        //        std::cout << sim.get_map().at(featid)[0] << "," << sim.get_map().at(featid)[1] << "," << sim.get_map().at(featid)[2] <<
        //        ","; std::cout << map_features_num_meas[feat.first] << ";" << std::endl;
        //      }
      }

#if ROS_AVAILABLE == 1
      // Visualize in RVIZ our current estimates and
      if (map_states.size() % 1 == 0) {

        // Pose states
        nav_msgs::Path arrEST, arrGT;
        arrEST.header.stamp = ros::Time::now();
        arrEST.header.frame_id = "global";
        arrGT.header.stamp = ros::Time::now();
        arrGT.header.frame_id = "global";
        for (auto &statepair : map_states) {
          geometry_msgs::PoseStamped poseEST, poseGT;
          poseEST.header.stamp = ros::Time(statepair.first);
          poseEST.header.frame_id = "global";
          poseEST.pose.orientation.x = ceres_vars_ori[statepair.second][0];
          poseEST.pose.orientation.y = ceres_vars_ori[statepair.second][1];
          poseEST.pose.orientation.z = ceres_vars_ori[statepair.second][2];
          poseEST.pose.orientation.w = ceres_vars_ori[statepair.second][3];
          poseEST.pose.position.x = ceres_vars_pos[statepair.second][0];
          poseEST.pose.position.y = ceres_vars_pos[statepair.second][1];
          poseEST.pose.position.z = ceres_vars_pos[statepair.second][2];
          Eigen::Matrix<double, 17, 1> gt_imustate;
          assert(sim.get_state(statepair.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate));
          poseGT.header.stamp = ros::Time(statepair.first);
          poseGT.header.frame_id = "global";
          poseGT.pose.orientation.x = gt_imustate(1);
          poseGT.pose.orientation.y = gt_imustate(2);
          poseGT.pose.orientation.z = gt_imustate(3);
          poseGT.pose.orientation.w = gt_imustate(4);
          poseGT.pose.position.x = gt_imustate(5);
          poseGT.pose.position.y = gt_imustate(6);
          poseGT.pose.position.z = gt_imustate(7);
          arrEST.poses.push_back(poseEST);
          arrGT.poses.push_back(poseGT);
        }
        pub_pathimu.publish(arrEST);
        pub_pathgt.publish(arrGT);

        // Features ESTIMATES pointcloud
        sensor_msgs::PointCloud point_cloud;
        point_cloud.header.frame_id = "global";
        point_cloud.header.stamp = ros::Time::now();
        for (auto &featpair : map_features) {
          if (map_features_delayed.find(featpair.first) != map_features_delayed.end())
            continue;
          geometry_msgs::Point32 p;
          p.x = (float)ceres_vars_feat[map_features[featpair.first]][0];
          p.y = (float)ceres_vars_feat[map_features[featpair.first]][1];
          p.z = (float)ceres_vars_feat[map_features[featpair.first]][2];
          point_cloud.points.push_back(p);
        }
        pub_loop_point.publish(point_cloud);

        // Features GROUNDTRUTH pointcloud
        sensor_msgs::PointCloud2 cloud;
        cloud.header.frame_id = "global";
        cloud.header.stamp = ros::Time::now();
        cloud.width = 3 * map_features.size();
        cloud.height = 1;
        cloud.is_bigendian = false;
        cloud.is_dense = false; // there may be invalid points
        sensor_msgs::PointCloud2Modifier modifier(cloud);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        modifier.resize(3 * map_features.size());
        sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
        sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
        sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
        for (auto &featpair : map_features) {
          if (map_features_delayed.find(featpair.first) != map_features_delayed.end())
            continue;
          *out_x = (float)sim.get_map().at(featpair.first)(0); // no change in id since no tracker is used
          ++out_x;
          *out_y = (float)sim.get_map().at(featpair.first)(1); // no change in id since no tracker is used
          ++out_y;
          *out_z = (float)sim.get_map().at(featpair.first)(2); // no change in id since no tracker is used
          ++out_z;
        }
        pub_points_sim.publish(cloud);
      }
#endif
    }
  }

  // Done!
  return EXIT_SUCCESS;
};