/*
 * 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.Common.Time;
using CartographerSharp.Models.GroundTruth;
using CartographerSharp.Models.Transform;
using CartographerSharp.Transform;
using System.Globalization;
using  RobotNet10.Shared.Numbers;
using GroundTruthProto = CartographerSharp.Models.GroundTruth.GroundTruth;
using QuaternionUtils = CartographerSharp.Transform.QuaternionUtils;

namespace CartographerSharp.GroundTruth
{
/// <summary>
/// Reads a text file and converts it to a GroundTruth proto. Each line contains:
/// time1 time2 x y z roll pitch yaw
/// using Unix epoch timestamps.
/// 
/// This is the format used in the relations files provided for:
/// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
/// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
/// Robots, vol. 27, no. 4, pp. 387–407, 2009.
/// </summary>
public static class RelationsTextFile
{
    /// <summary>
    /// Reads relations from a text file.
    /// </summary>
    /// <param name="relationsFilename">Path to the relations text file.</param>
    /// <returns>GroundTruth proto with relations.</returns>
    public static GroundTruthProto ReadRelationsTextFile(string relationsFilename)
    {
        if (string.IsNullOrEmpty(relationsFilename))
            throw new ArgumentException("Relations filename cannot be null or empty", nameof(relationsFilename));
        if (!File.Exists(relationsFilename))
            throw new FileNotFoundException("Relations file not found", relationsFilename);

        var groundTruth = new GroundTruthProto
        {
            Relations = []
        };

        var lines = File.ReadAllLines(relationsFilename);
        foreach (var line in lines)
        {
            if (string.IsNullOrWhiteSpace(line))
                continue;

            var parts = line.Split([' ', '\t'], StringSplitOptions.RemoveEmptyEntries);
            if (parts.Length < 8)
                continue;

            if (!double.TryParse(parts[0], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime1) ||
                !double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime2) ||
                !double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out var x) ||
                !double.TryParse(parts[3], NumberStyles.Float, CultureInfo.InvariantCulture, out var y) ||
                !double.TryParse(parts[4], NumberStyles.Float, CultureInfo.InvariantCulture, out var z) ||
                !double.TryParse(parts[5], NumberStyles.Float, CultureInfo.InvariantCulture, out var roll) ||
                !double.TryParse(parts[6], NumberStyles.Float, CultureInfo.InvariantCulture, out var pitch) ||
                !double.TryParse(parts[7], NumberStyles.Float, CultureInfo.InvariantCulture, out var yaw))
            {
                continue;
            }

            var commonTime1 = UnixToCommonTime(unixTime1);
            var commonTime2 = UnixToCommonTime(unixTime2);

            // Create expected transform from translation and Euler angles
            var expected = new Rigid3d( new Vector3(x, y, z), QuaternionUtils.RollPitchYaw(roll, pitch, yaw));

            // Convert TimeSpan to universal ticks (same as ToUniversal for DateTime)
            var relation = new Relation
            {
                Timestamp1 = commonTime1.Ticks,
                Timestamp2 = commonTime2.Ticks,
                Expected = (Rigid3dProto)expected, // Explicit conversion from Rigid3d to Rigid3dProto
                CoveredDistance = 0.0 // Will be computed later if needed
            };

            groundTruth.Relations.Add(relation);
        }

        return groundTruth;
    }

    /// <summary>
    /// Converts Unix timestamp to Common time (TimeSpan since epoch).
    /// </summary>
    private static TimeSpan UnixToCommonTime(double unixTime)
    {
        const long kUtsTicksPerSecond = 10000000;
        var utsEpochOffsetTicks = TimeUtils.UtsEpochOffsetFromUnixEpochInSeconds * kUtsTicksPerSecond;
        var unixTimeTicks = (long)(unixTime * kUtsTicksPerSecond);
        return TimeSpan.FromTicks(utsEpochOffsetTicks + unixTimeTicks);
    }
}
}