/*
 * 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.Internal.D3D;

/// <summary>
/// Result of IMU integration.
/// </summary>
public struct IntegrateImuResult
{
    public Vector3 DeltaVelocity { get; set; }
    public Vector3 DeltaTranslation { get; set; }
    public Quaternion DeltaRotation { get; set; }

    public IntegrateImuResult(Vector3 deltaVelocity, Vector3 deltaTranslation, Quaternion deltaRotation)
    {
        DeltaVelocity = deltaVelocity;
        DeltaTranslation = deltaTranslation;
        DeltaRotation = deltaRotation;
    }
}

/// <summary>
/// IMU integration utilities.
/// </summary>
public static class ImuIntegration
{
    /// <summary>
    /// Integrates IMU data between start_time and end_time.
    /// Returns delta_velocity, delta_translation, and delta_rotation.
    /// </summary>
    public static IntegrateImuResult IntegrateImu(
        List<ImuData> imuData,
        long startTime,
        long endTime,
        ref int imuIndex)
    {
        if (startTime > endTime)
            throw new ArgumentException("startTime must be <= endTime");
        
        if (imuIndex < 0 || imuIndex >= imuData.Count)
            throw new ArgumentOutOfRangeException(nameof(imuIndex));
        
        if (imuData[imuIndex].Time > startTime)
            throw new ArgumentException("imuData[imuIndex].Time must be <= startTime");
        
        if (imuIndex + 1 < imuData.Count && imuData[imuIndex + 1].Time <= startTime)
            throw new ArgumentException("imuData[imuIndex+1].Time must be > startTime");

        var result = new IntegrateImuResult(
            Vector3.Zero,
            Vector3.Zero,
            Quaternion.Identity);

        long currentTime = startTime;

        while (currentTime < endTime)
        {
            long nextImuTime = long.MaxValue;
            if (imuIndex + 1 < imuData.Count)
            {
                nextImuTime = imuData[imuIndex + 1].Time;
            }
            
            long nextTime = Math.Min(nextImuTime, endTime);
            double deltaT = (nextTime - currentTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)

            var currentImu = imuData[imuIndex];
            
            // Compute delta angle from angular velocity
            var deltaAngle = currentImu.AngularVelocity * deltaT;
            
            // Convert angle-axis to quaternion (simplified - assumes small angles)
            // For small angles: q ≈ [1, 0.5*angle.x, 0.5*angle.y, 0.5*angle.z]
            var angleLength = deltaAngle.Length();
            Quaternion deltaRotation;
            if (angleLength < 1e-6)
            {
                deltaRotation = Quaternion.Identity;
            }
            else
            {
                var axis = Vector3.Normalize(deltaAngle);
                deltaRotation = Quaternion.CreateFromAxisAngle(axis, angleLength);
            }
            
            // Update cumulative rotation
            result.DeltaRotation = Quaternion.Multiply(result.DeltaRotation, deltaRotation);
            
            // Integrate linear acceleration
            // Rotate acceleration to current orientation frame
            var rotatedAcceleration = Vector3.Transform(currentImu.LinearAcceleration, result.DeltaRotation);
            var deltaVelocity = rotatedAcceleration * deltaT;
            result.DeltaVelocity += deltaVelocity;
            
            // Integrate velocity to get translation
            result.DeltaTranslation += result.DeltaVelocity * deltaT;

            currentTime = nextTime;
            if (currentTime == nextImuTime)
            {
                imuIndex++;
            }
        }

        return result;
    }
}