/* Copyright (C) 2021 Steffen Urban
 * All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero 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 Affero General Public License for more details.
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#pragma once

#include "OpenCameraCalibrator/utils/types.h"

namespace OpenICC {
namespace core {

class ImuToCameraRotationEstimator {
 public:
  ImuToCameraRotationEstimator() {}
  ImuToCameraRotationEstimator(const quat_map& visual_rotations,
                               const vec3_map& imu_angular_vel)
      : visual_rotations_(visual_rotations),
        imu_angular_vel_(imu_angular_vel) {}

  void SetVisualRotations(const quat_map& visual_rotations) {
    visual_rotations_ = visual_rotations;
  }

  void SetAngularVelocities(const vec3_map& imu_angular_vel) {
    imu_angular_vel_ = imu_angular_vel;
  }

  bool EstimateCameraImuRotation(const double dt_imu,
                                 Eigen::Matrix3d& R_imu_to_camera,
                                 double& time_offset_imu_to_camera,
                                 Eigen::Vector3d& gyro_bias,
                                 vec3_vector& smoothed_ang_imu,
                                 vec3_vector& smoothed_vis_vel);

  double SolveClosedForm(const vec3_vector& angVis,
                         const vec3_vector& angImu,
                         const std::vector<double> timestamps_s,
                         const double td,
                         const double dt_imu,
                         Eigen::Matrix3d& Rs,
                         Eigen::Vector3d& bias);

  void EnableGyroBiasEstimation() { estimate_gyro_bias_ = true; }

 private:
  //! visual rotations
  quat_map visual_rotations_;

  //! imu angular velocities
  vec3_map imu_angular_vel_;

  //! estimate bias
  bool estimate_gyro_bias_ = false;
};

}  // namespace core
}  // namespace OpenICC
