/*
 * Copyright 2018 The Cartographer Authors
 *
 * 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.
 */

#ifndef CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_
#define CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_

#include <deque>
#include <memory>
#include <vector>

#include "cartographer/common/internal/ceres_solver_options.h"
#include "cartographer/common/histogram.h"
#include "cartographer/mapping/pose_extrapolator_interface.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/transform/timestamped_transform.h"

namespace cartographer {
namespace mapping {

// Uses the linear acceleration and rotational velocities to estimate a pose.
class ImuBasedPoseExtrapolator : public PoseExtrapolatorInterface {
 public:
  explicit ImuBasedPoseExtrapolator(
      const proto::ImuBasedPoseExtrapolatorOptions& options);
  ~ImuBasedPoseExtrapolator() override;

  static std::unique_ptr<PoseExtrapolatorInterface> InitializeWithImu(
      const proto::ImuBasedPoseExtrapolatorOptions& options,
      const std::vector<sensor::ImuData>& imu_data,
      const std::vector<transform::TimestampedTransform>& initial_poses);

  // Returns the time of the last added pose or Time::min() if no pose was added
  // yet.
  common::Time GetLastPoseTime() const override;
  common::Time GetLastExtrapolatedTime() const override;

  void AddPose(common::Time time, const transform::Rigid3d& pose) override;
  void AddImuData(const sensor::ImuData& imu_data) override;
  void AddOdometryData(const sensor::OdometryData& odometry_data) override;

  transform::Rigid3d ExtrapolatePose(common::Time time) override;

  ExtrapolationResult ExtrapolatePosesWithGravity(
      const std::vector<common::Time>& times) override;

  // Gravity alignment estimate.
  Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

 private:
  template <typename T>
  void TrimDequeData(std::deque<T>* data);

  void TrimImuData();
  void TrimOdometryData();

  // Odometry methods.
  bool HasOdometryData() const;
  bool HasOdometryDataForTime(const common::Time& first_time) const;
  transform::Rigid3d InterpolateOdometry(const common::Time& first_time) const;
  transform::Rigid3d CalculateOdometryBetweenNodes(
      const transform::Rigid3d& first_node_odometry,
      const transform::Rigid3d& second_node_odometry) const;

  std::vector<transform::Rigid3f> InterpolatePoses(
      const ::cartographer::transform::TimestampedTransform& start,
      const ::cartographer::transform::TimestampedTransform& end,
      const std::vector<common::Time>::const_iterator times_begin,
      const std::vector<common::Time>::const_iterator times_end);

  std::deque<::cartographer::transform::TimestampedTransform> timed_pose_queue_;
  std::deque<::cartographer::transform::TimestampedTransform>
      previous_solution_;

  std::deque<sensor::ImuData> imu_data_;
  std::deque<sensor::OdometryData> odometry_data_;
  common::Time last_extrapolated_time_ = common::Time::min();

  transform::Rigid3d gravity_from_local_ = transform::Rigid3d::Identity();

  const proto::ImuBasedPoseExtrapolatorOptions options_;
  const ceres::Solver::Options solver_options_;

  common::Histogram num_iterations_hist_;
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_