/*
 * 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 RobotNet10.Shared.Numbers;
using SysNum = System.Numerics;

namespace CartographerSharp.Common.Math;

/// <summary>
/// Common mathematical utilities for Cartographer.
/// </summary>
public static class MathUtils
{
    /// <summary>
    /// Clamps 'value' to be in the range ['min', 'max'].
    /// </summary>
    public static T Clamp<T>(T value, T min, T max) where T : IComparable<T>
    {
        if (value.CompareTo(max) > 0)
        {
            return max;
        }
        if (value.CompareTo(min) < 0)
        {
            return min;
        }
        return value;
    }

    /// <summary>
    /// Calculates 'base'^'exponent'.
    /// </summary>
    public static T Power<T>(T baseValue, int exponent) where T : SysNum.IMultiplyOperators<T, T, T>, SysNum.IMultiplicativeIdentity<T, T>
    {
        if (exponent == 0)
        {
            return T.MultiplicativeIdentity;
        }
        if (exponent < 0)
        {
            throw new ArgumentException("Exponent must be non-negative", nameof(exponent));
        }
        
        T result = baseValue;
        for (int i = 1; i < exponent; i++)
        {
            result *= baseValue;
        }
        return result;
    }

    /// <summary>
    /// Calculates a^2.
    /// </summary>
    public static T Pow2<T>(T a) where T : SysNum.IMultiplyOperators<T, T, T>
    {
        return a * a;
    }

    /// <summary>
    /// Converts from degrees to radians.
    /// </summary>
    public static double DegToRad(double deg)
    {
        return System.Math.PI * deg / 180.0;
    }

    /// <summary>
    /// Converts from radians to degrees.
    /// </summary>
    public static double RadToDeg(double rad)
    {
        return 180.0 * rad / System.Math.PI;
    }

    /// <summary>
    /// Bring the 'difference' between two angles into [-pi; pi].
    /// </summary>
    public static T NormalizeAngleDifference<T>(T difference) where T : SysNum.IFloatingPoint<T>
    {
        var kPi = T.CreateChecked(System.Math.PI);
        var twoPi = T.CreateChecked(2.0 * System.Math.PI);
        
        while (difference > kPi)
        {
            difference -= twoPi;
        }
        while (difference < -kPi)
        {
            difference += twoPi;
        }
        return difference;
    }

    /// <summary>
    /// Calculates atan2 for a 2D vector.
    /// </summary>
    public static double Atan2(RobotNet10.Shared.Numbers.Vector2 vector)
    {
        return System.Math.Atan2(vector.Y, vector.X);
    }

    /// <summary>
    /// Calculates quaternion product: z * w.
    /// </summary>
    /// <param name="z">First quaternion as array [w, x, y, z]</param>
    /// <param name="w">Second quaternion as array [w, x, y, z]</param>
    /// <param name="zw">Output quaternion as array [w, x, y, z]</param>
    public static void QuaternionProduct(ReadOnlySpan<double> z, ReadOnlySpan<double> w, Span<double> zw)
    {
        if (z.Length < 4 || w.Length < 4 || zw.Length < 4)
        {
            throw new ArgumentException("Quaternion arrays must have at least 4 elements");
        }

        // z = [w, x, y, z] in array, but Quaternion uses [x, y, z, w] order
        // So z[0] = w, z[1] = x, z[2] = y, z[3] = z
        zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; // w component
        zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; // x component
        zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; // y component
        zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; // z component
    }
}