/*
 * Copyright 2016 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.
 */

using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping;

/// <summary>
/// Interface for pose graph operations.
/// </summary>
public interface IPoseGraph
{
    /// <summary>
    /// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
    /// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
    /// 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
    /// </summary>
    public struct Constraint(SubmapId submapId, NodeId nodeId, Constraint.Pose pose, Constraint.Tag tag, double score = 0.0, Constraint.State state = Constraint.State.Enabled)
    {
        /// <summary>
        /// Constraint pose information.
        /// </summary>
        public struct Pose(Rigid3d zbarIj, double translationWeight, double rotationWeight)
        {
            public Rigid3d ZbarIj { get; set; } = zbarIj;
            public double TranslationWeight { get; set; } = translationWeight;
            public double RotationWeight { get; set; } = rotationWeight;
        }

        public SubmapId SubmapId { get; set; } = submapId;
        public NodeId NodeId { get; set; } = nodeId;

        /// <summary>
        /// Pose of the node 'j' relative to submap 'i'.
        /// </summary>
        public Pose ConstraintPose { get; set; } = pose;

        /// <summary>
        /// Differentiates between intra-submap (where node 'j' was inserted into
        /// submap 'i') and inter-submap constraints (where node 'j' was not inserted
        /// into submap 'i').
        /// </summary>
        public enum Tag
        {
            IntraSubmap,
            InterSubmap
        }

        public Tag ConstraintTag { get; set; } = tag;

        /// <summary>
        /// Match C++: score field for constraint quality.
        /// </summary>
        public double Score { get; set; } = score;

        /// <summary>
        /// Match C++: state enum for enabled/disabled constraints.
        /// </summary>
        public enum State
        {
            Enabled,
            Disabled
        }

        public State ConstraintState { get; set; } = state;
    }

    /// <summary>
    /// Landmark node information.
    /// </summary>
    public struct LandmarkNode(
        List<LandmarkNode.LandmarkObservation>? landmarkObservations = null,
        Rigid3d? globalLandmarkPose = null,
        bool frozen = false)
    {
        /// <summary>
        /// Landmark observation.
        /// </summary>
        public struct LandmarkObservation(
            int trajectoryId,
            long time,
            Rigid3d landmarkToTrackingTransform,
            double translationWeight,
            double rotationWeight)
        {
            public int TrajectoryId { get; set; } = trajectoryId;
            public long Time { get; set; } = time;
            public Rigid3d LandmarkToTrackingTransform { get; set; } = landmarkToTrackingTransform;
            public double TranslationWeight { get; set; } = translationWeight;
            public double RotationWeight { get; set; } = rotationWeight;
        }

        public List<LandmarkObservation> LandmarkObservations { get; set; } = landmarkObservations ?? [];
        public Rigid3d? GlobalLandmarkPose { get; set; } = globalLandmarkPose;
        public bool Frozen { get; set; } = frozen;
    }

    /// <summary>
    /// Submap pose information.
    /// </summary>
    public struct SubmapPose(int version, Rigid3d pose)
    {
        public int Version { get; set; } = version;
        public Rigid3d Pose { get; set; } = pose;
    }

    /// <summary>
    /// Submap data with pose.
    /// </summary>
    public struct SubmapData(Submap? submap, Rigid3d pose)
    {
        public Submap? Submap { get; set; } = submap;
        public Rigid3d Pose { get; set; } = pose;
    }

    /// <summary>
    /// Trajectory data.
    /// </summary>
    public struct TrajectoryData(
        double gravityConstant = 9.8,
        Quaternion? imuCalibration = null,
        Rigid3d? fixedFrameOriginInMap = null)
    {
        public double GravityConstant { get; set; } = gravityConstant;
        public Quaternion ImuCalibration { get; set; } = imuCalibration ?? Quaternion.Identity;
        public Rigid3d? FixedFrameOriginInMap { get; set; } = fixedFrameOriginInMap;
    }

    /// <summary>
    /// Trajectory state enumeration.
    /// </summary>
    public enum TrajectoryState
    {
        Active,
        Finished,
        Frozen,
        Deleted
    }

    /// <summary>
    /// Gets the total number of work items added to the work queue.
    /// </summary>
    public int WorkItemsAdded { get; }

    /// <summary>
    /// Gets the total number of work items completed by the work queue.
    /// </summary>
    public int WorkItemsCompleted { get; }

    /// <summary>
    /// Gets the number of work items currently pending in the work queue.
    /// </summary>
    public int WorkItemsPending { get; }

    /// <summary>
    /// Gets the current number of items in the work queue.
    /// </summary>
    public int WorkQueueCount { get; }

    /// <summary>
    /// Gets the number of nodes started in the constraint builder.
    /// </summary>
    public int ConstraintBuilderNodesStarted { get; }

    /// <summary>
    /// Gets the number of nodes finished in the constraint builder.
    /// </summary>
    public int ConstraintBuilderNodesFinished { get; }

    /// <summary>
    /// Gets the total number of trajectory nodes in the pose graph.
    /// Used for progress tracking during optimization.
    /// </summary>
    public int TrajectoryNodesCount { get; }

    /// <summary>
    /// Gets the total number of constraint tasks dispatched for scan matching.
    /// </summary>
    public int ConstraintTasksTotal { get; }

    /// <summary>
    /// Gets the number of constraint tasks that have finished scan matching.
    /// </summary>
    public int ConstraintTasksFinished { get; }

    /// <summary>
    /// Inserts an IMU measurement.
    /// </summary>
    void AddImuData(int trajectoryId, ImuData imuData);

    /// <summary>
    /// Inserts an odometry measurement.
    /// </summary>
    void AddOdometryData(int trajectoryId, OdometryData odometryData);

    /// <summary>
    /// Inserts a fixed frame pose measurement.
    /// </summary>
    void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData);

    /// <summary>
    /// Inserts landmarks observations.
    /// </summary>
    void AddLandmarkData(int trajectoryId, LandmarkData landmarkData);

    /// <summary>
    /// Drains the work queue to ensure all pending operations are completed.
    /// This should be called before finishing a trajectory to avoid race conditions.
    /// </summary>
    void DrainWorkQueue();

    /// <summary>
    /// Finishes the given trajectory.
    /// </summary>
    void FinishTrajectory(int trajectoryId);

    /// <summary>
    /// Freezes a trajectory. Poses in this trajectory will not be optimized.
    /// </summary>
    void FreezeTrajectory(int trajectoryId);

    /// <summary>
    /// Adds a 'submap' from a proto with the given 'global_pose' to the
    /// appropriate trajectory.
    /// </summary>
    void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap);

    /// <summary>
    /// Adds a 'node' from a proto with the given 'global_pose' to the
    /// appropriate trajectory.
    /// </summary>
    void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);

    /// <summary>
    /// Sets the trajectory data from a proto.
    /// </summary>
    void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data);

    /// <summary>
    /// Adds information that 'node_id' was inserted into 'submap_id'. The submap
    /// has to be deserialized first.
    /// </summary>
    void AddNodeToSubmap(NodeId nodeId, SubmapId submapId);

    /// <summary>
    /// Adds serialized constraints. The corresponding trajectory nodes and submaps
    /// have to be deserialized before calling this function.
    /// </summary>
    void AddSerializedConstraints(List<Constraint> constraints);

    /// <summary>
    /// Adds a 'trimmer'. It will be used after all data added before it has been
    /// included in the pose graph.
    /// </summary>
    void AddTrimmer(PoseGraphTrimmer trimmer);

    /// <summary>
    /// Returns the current trajectory clusters.
    /// </summary>
    List<List<int>> GetConnectedTrajectories();

    /// <summary>
    /// Returns the IMU data.
    /// </summary>
    Dictionary<int, List<ImuData>> GetImuData();

    /// <summary>
    /// Returns the odometry data.
    /// </summary>
    Dictionary<int, List<OdometryData>> GetOdometryData();

    /// <summary>
    /// Returns the fixed frame pose data.
    /// </summary>
    Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData();

    /// <summary>
    /// Returns the landmark data.
    /// </summary>
    Dictionary<string, LandmarkNode> GetLandmarkNodes();

    /// <summary>
    /// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
    /// respect to 'to_trajectory_id' at time 'time'.
    /// </summary>
    void SetInitialTrajectoryPose(
        int fromTrajectoryId,
        int toTrajectoryId,
        Rigid3d pose,
        long time);

    /// <summary>
    /// Waits for all computations to finish and computes optimized poses.
    /// </summary>
    void RunFinalOptimization();

    /// <summary>
    /// Returns data for all submaps.
    /// </summary>
    MapById<SubmapId, SubmapData> GetAllSubmapData();

    /// <summary>
    /// Returns the current optimized transform and submap itself for the given
    /// 'submap_id'. Returns 'null' for the 'submap' member if the submap does
    /// not exist (anymore).
    /// </summary>
    SubmapData GetSubmapData(SubmapId submapId);

    /// <summary>
    /// Returns the global poses for all submaps.
    /// </summary>
    MapById<SubmapId, SubmapPose> GetAllSubmapPoses();

    /// <summary>
    /// Returns the transform converting data in the local map frame (i.e. the
    /// continuous, non-loop-closed frame) into the global map frame (i.e. the
    /// discontinuous, loop-closed frame).
    /// </summary>
    Rigid3d GetLocalToGlobalTransform(int trajectoryId);

    /// <summary>
    /// Returns the current optimized trajectories.
    /// </summary>
    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();

    /// <summary>
    /// Returns the current optimized trajectory poses.
    /// </summary>
    MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();

    /// <summary>
    /// Returns the states of trajectories.
    /// </summary>
    Dictionary<int, TrajectoryState> GetTrajectoryStates();

    /// <summary>
    /// Returns the current optimized landmark poses.
    /// </summary>
    Dictionary<string, Rigid3d> GetLandmarkPoses();

    /// <summary>
    /// Sets global pose of landmark 'landmark_id' to given 'global_pose'.
    /// </summary>
    void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);

    /// <summary>
    /// Deletes a trajectory asynchronously.
    /// </summary>
    void DeleteTrajectory(int trajectoryId);

    /// <summary>
    /// Checks if the given trajectory is finished.
    /// </summary>
    bool IsTrajectoryFinished(int trajectoryId);

    /// <summary>
    /// Checks if the given trajectory is frozen.
    /// </summary>
    bool IsTrajectoryFrozen(int trajectoryId);

    /// <summary>
    /// Returns the trajectory data.
    /// </summary>
    Dictionary<int, TrajectoryData> GetTrajectoryData();

    /// <summary>
    /// Returns the collection of constraints.
    /// </summary>
    List<Constraint> Constraints();

    /// <summary>
    /// Serializes the constraints and trajectories. If
    /// 'include_unfinished_submaps' is set to 'true', unfinished submaps, i.e.
    /// submaps that have not yet received all rangefinder data insertions, will
    /// be included, otherwise not.
    /// </summary>
    Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);

    /// <summary>
    /// Sets the callback function that is invoked whenever the global optimization
    /// problem is solved.
    /// </summary>
    void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);

    /// <summary>
    /// Sets the transform from local map frame to global map frame (map origin).
    /// Used when loading state from pbstream; matches C++ SetTransformToMap.
    /// </summary>
    void SetTransformToMap(Rigid3d transform);

    /// <summary>
    /// Returns the transform from local map frame to global map frame.
    /// Matches C++ GetTransformToMap.
    /// </summary>
    Rigid3d GetTransformToMap();

    /// <summary>
    /// Manually compute a constraint between a node and submap using global scan matching.
    /// Match C++: manualComputeConstraint (pose_graph_interface.h:181-192)
    /// </summary>
    (double Score, Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);

    /// <summary>
    /// Manually compute constraint score from an initial pose estimate.
    /// Match C++: manualComputeConstraintScore (pose_graph_interface.h:194-197)
    /// </summary>
    double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);

    /// <summary>
    /// Manually compute scan matcher score with refined pose output.
    /// Match C++: manualComputeScanMatcher (pose_graph_interface.h:199-202)
    /// </summary>
    double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);

    /// <summary>
    /// Manually relocalize a trajectory against finished submaps.
    /// Match C++: ManualRelocalization (pose_graph_interface.h:212-216)
    /// </summary>
    bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
}

/// <summary>
/// Callback for global SLAM optimization.
/// </summary>
public delegate void GlobalSlamOptimizationCallback(
    Dictionary<int, SubmapId> submapIds,
    Dictionary<int, NodeId> nodeIds);

/// <summary>
/// Result of manual relocalization operation.
/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
/// </summary>
public struct LocalizationResult
{
    public NodeId NodeId { get; init; }
    public SubmapId SubmapId { get; init; }
    public Rigid3d GlobalPose { get; init; }
    public double Score { get; init; }
}

/// <summary>
/// Callback for localization/relocalization results.
/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
/// </summary>
public delegate void LocalizationResultCallback(LocalizationResult result);