/*
 * 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.Models.Transform;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;

namespace CartographerSharp.Mapping;

/// <summary>
/// Base class for pose graph implementations.
/// </summary>
public abstract class PoseGraph : IPoseGraph
{
    /// <summary>
    /// Initial trajectory pose information.
    /// </summary>
    public struct InitialTrajectoryPose(int toTrajectoryId, Rigid3d relativePose, long time)
    {
        public int ToTrajectoryId { get; set; } = toTrajectoryId;
        public Rigid3d RelativePose { get; set; } = relativePose;
        public long Time { get; set; } = time;
    }

    protected PoseGraph()
    {
    }

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

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

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

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

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

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

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

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

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

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

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

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

    /// <summary>
    /// Inserts landmarks observations.
    /// </summary>
    public abstract 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.
    /// Default implementation does nothing (for pose graphs without work queues).
    /// </summary>
    public virtual void DrainWorkQueue()
    {
        // Default implementation: do nothing (for pose graphs without work queues)
    }

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

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

    /// <summary>
    /// Adds a 'submap' from a proto with the given 'global_pose' to the
    /// appropriate trajectory.
    /// </summary>
    public abstract 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>
    public abstract void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);

    /// <summary>
    /// Sets the trajectory data from a proto.
    /// </summary>
    public abstract 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>
    public abstract 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>
    public abstract void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints);

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

    /// <summary>
    /// Gets the current trajectory clusters.
    /// </summary>
    public abstract List<List<int>> GetConnectedTrajectories();

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

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

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

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

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

    /// <summary>
    /// Sets localization initial poses for relocalizing against the map.
    /// Match C++: SetLocalizationInitialPoses (pose_graph.h:139)
    /// C++ signature: const std::vector&lt;transform::Rigid3d&gt; &amp; (const reference)
    /// C# equivalent: IReadOnlyList&lt;Rigid3d&gt; (read-only collection)
    /// </summary>
    public abstract void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses);

    public abstract void RunFinalOptimization();
    public abstract MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData();
    public abstract IPoseGraph.SubmapData GetSubmapData(SubmapId submapId);
    public abstract MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses();
    public abstract Rigid3d GetLocalToGlobalTransform(int trajectoryId);
    public abstract MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
    public abstract MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
    public abstract Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates();
    public abstract Dictionary<string, Rigid3d> GetLandmarkPoses();
    public abstract void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);
    public abstract void DeleteTrajectory(int trajectoryId);
    public abstract bool IsTrajectoryFinished(int trajectoryId);
    public abstract bool IsTrajectoryFrozen(int trajectoryId);
    public abstract Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData();
    public abstract List<IPoseGraph.Constraint> Constraints();
    public abstract Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);
    public abstract void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
    public abstract void SetTransformToMap(Rigid3d transform);
    public abstract Rigid3d GetTransformToMap();

    // Manual compute methods (match C++ PoseGraphInterface)
    public abstract (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);
    public abstract double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);
    public abstract double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);
    public abstract bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
}

/// <summary>
/// Pose graph trimmer interface.
/// </summary>
public abstract class PoseGraphTrimmer
{
    /// <summary>
    /// Trims the pose graph.
    /// </summary>
    public abstract void Trim(ITrimmable trimmable);
}

/// <summary>
/// Interface for trimmable pose graph operations.
/// </summary>
public interface ITrimmable
{
    int NumSubmaps(int trajectoryId);
    List<SubmapId> GetSubmapIds(int trajectoryId);
    MapById<SubmapId, IPoseGraph.SubmapData> GetOptimizedSubmapData();
    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
    List<IPoseGraph.Constraint> GetConstraints();
    void TrimSubmap(SubmapId submapId);
    bool IsFinished(int trajectoryId);
    void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state);
}

/// <summary>
/// Conversion utilities for constraints.
/// </summary>
public static class ConstraintOperations
{
    /// <summary>
    /// Converts constraint to proto.
    /// Match C++: ToProto in pose_graph.cc:147-169
    /// </summary>
    public static Models.Mapping.PoseGraph.Constraint ToProto(IPoseGraph.Constraint constraint)
    {
        // Match C++: convert tag (line 161 in pose_graph.cc)
        var tag = constraint.ConstraintTag == IPoseGraph.Constraint.Tag.IntraSubmap
            ? Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap
            : Models.Mapping.PoseGraph.Constraint.Tag.InterSubmap;

        // Match C++: convert state (line 166 in pose_graph.cc)
        var state = constraint.ConstraintState == IPoseGraph.Constraint.State.Enabled
            ? Models.Mapping.PoseGraph.Constraint.State.Enabled
            : Models.Mapping.PoseGraph.Constraint.State.Disabled;

        // Match C++: create constraint proto with all fields (lines 147-168 in pose_graph.cc)
        return new Models.Mapping.PoseGraph.Constraint(
            new Models.Mapping.PoseGraph.SubmapId(constraint.SubmapId.TrajectoryId, constraint.SubmapId.SubmapIndex),
            new Models.Mapping.PoseGraph.NodeId(constraint.NodeId.TrajectoryId, constraint.NodeId.NodeIndex),
            (Rigid3dProto)constraint.ConstraintPose.ZbarIj,
            constraint.ConstraintPose.TranslationWeight,
            constraint.ConstraintPose.RotationWeight,
            tag,
            constraint.Score,          // CRITICAL FIX: include score field (line 162 in pose_graph.cc)
            state                      // CRITICAL FIX: include state field (line 166 in pose_graph.cc)
        );
    }

    /// <summary>
    /// Creates constraint from proto.
    /// Match C++: FromProto in pose_graph.cc:77-97
    /// </summary>
    public static List<IPoseGraph.Constraint> FromProto(List<Models.Mapping.PoseGraph.Constraint> constraintProtos)
    {
        var constraints = new List<IPoseGraph.Constraint>();
        foreach (var constraintProto in constraintProtos)
        {
            // Match C++: convert tag
            IPoseGraph.Constraint.Tag tag;
            if (constraintProto.ConstraintTag == Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap)
            {
                tag = IPoseGraph.Constraint.Tag.IntraSubmap;
            }
            else
            {
                tag = IPoseGraph.Constraint.Tag.InterSubmap;
            }

            // Match C++: convert state (lines 93 in pose_graph.cc)
            IPoseGraph.Constraint.State state;
            if (constraintProto.ConstraintState == Models.Mapping.PoseGraph.Constraint.State.Enabled)
            {
                state = IPoseGraph.Constraint.State.Enabled;
            }
            else
            {
                state = IPoseGraph.Constraint.State.Disabled;
            }

            // Match C++: extract score (line 92 in pose_graph.cc)
            var score = constraintProto.Score;

            // Match C++: create constraint with all fields (line 94 in pose_graph.cc)
            var constraint = new IPoseGraph.Constraint(
                new SubmapId(constraintProto.SubmapId.TrajectoryId, constraintProto.SubmapId.SubmapIndex),
                new NodeId(constraintProto.NodeId.TrajectoryId, constraintProto.NodeId.NodeIndex),
                new IPoseGraph.Constraint.Pose(
                    (Rigid3d)constraintProto.RelativePose,
                    constraintProto.TranslationWeight,
                    constraintProto.RotationWeight),
                tag,
                score,   // CRITICAL FIX: include score field
                state    // CRITICAL FIX: include state field
            );
            constraints.Add(constraint);
        }
        return constraints;
    }
}