using System;
using System.Globalization;
using System.Linq;

namespace ImportUnityPackage;

/// <summary>
/// Column-vector 4x4 transforms and the conversion from Unity's left-handed, Y-up meters to s&box's right-handed,
/// Z-up inches (+X forward, +Y left): Unity (x, y, z) is s&box (z, -x, y) × 39.37.
/// </summary>
internal sealed class UnityTransform
{
	readonly double[] m; // row-major, column vectors: p' = M p
	UnityTransform( double[] values ) { m = values; }
	double this[int row, int column] => m[row * 4 + column];

	public static readonly UnityTransform Identity = new( new double[] { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 } );
	public const double InchesPerMeter = 1 / 0.0254;

	public static UnityTransform Translation( double x, double y, double z ) => new( new double[] { 1, 0, 0, x, 0, 1, 0, y, 0, 0, 1, z, 0, 0, 0, 1 } );
	public static UnityTransform Scaling( double x, double y, double z ) => new( new double[] { x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1 } );
	public static UnityTransform Linear( double[,] r ) => new( new double[] { r[0, 0], r[0, 1], r[0, 2], 0, r[1, 0], r[1, 1], r[1, 2], 0, r[2, 0], r[2, 1], r[2, 2], 0, 0, 0, 0, 1 } );

	static UnityTransform Axis( int axis, double degrees )
	{
		var a = degrees * Math.PI / 180; double c = Math.Cos( a ), s = Math.Sin( a );
		return axis switch
		{
			0 => Linear( new[,] { { 1, 0, 0 }, { 0, c, -s }, { 0, s, c } } ),
			1 => Linear( new[,] { { c, 0, s }, { 0, 1, 0 }, { -s, 0, c } } ),
			_ => Linear( new[,] { { c, -s, 0 }, { s, c, 0 }, { 0, 0, 1 } } ),
		};
	}

	/// <summary>FBX Euler rotation in degrees. Order 0-5 is XYZ, XZY, YZX, YXZ, ZXY, ZYX: the first axis is applied first.</summary>
	public static UnityTransform Euler( double x, double y, double z, int order = 0 )
	{
		var angles = new[] { x, y, z };
		var sequence = order switch { 1 => "XZY", 2 => "YZX", 3 => "YXZ", 4 => "ZXY", 5 => "ZYX", _ => "XYZ" };
		var result = Identity;
		foreach ( var letter in sequence ) result = Axis( letter - 'X', angles[letter - 'X'] ) * result;
		return result;
	}

	public static UnityTransform Rotation( double x, double y, double z, double w )
	{
		var n = Math.Sqrt( x * x + y * y + z * z + w * w );
		if ( n < 1e-12 ) return Identity;
		x /= n; y /= n; z /= n; w /= n;
		return Linear( new[,]
		{
			{ 1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w) },
			{ 2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w) },
			{ 2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y) },
		} );
	}

	public static UnityTransform operator *( UnityTransform a, UnityTransform b )
	{
		var r = new double[16];
		for ( var i = 0; i < 4; i++ )
			for ( var j = 0; j < 4; j++ )
				r[i * 4 + j] = a[i, 0] * b[0, j] + a[i, 1] * b[1, j] + a[i, 2] * b[2, j] + a[i, 3] * b[3, j];
		return new( r );
	}

	public double[] Point( double x, double y, double z ) => new[]
	{
		m[0] * x + m[1] * y + m[2] * z + m[3], m[4] * x + m[5] * y + m[6] * z + m[7], m[8] * x + m[9] * y + m[10] * z + m[11],
	};

	public UnityTransform Inverse()
	{
		// General affine inverse: invert the 3x3 linear part, then the translation.
		double a = this[0, 0], b = this[0, 1], c = this[0, 2], d = this[1, 0], e = this[1, 1], f = this[1, 2], g = this[2, 0], h = this[2, 1], k = this[2, 2];
		var det = a * (e * k - f * h) - b * (d * k - f * g) + c * (d * h - e * g);
		if ( Math.Abs( det ) < 1e-18 ) return Identity;
		var inv = new[,]
		{
			{ (e * k - f * h) / det, (c * h - b * k) / det, (b * f - c * e) / det },
			{ (f * g - d * k) / det, (a * k - c * g) / det, (c * d - a * f) / det },
			{ (d * h - e * g) / det, (b * g - a * h) / det, (a * e - b * d) / det },
		};
		var t = new[] { this[0, 3], this[1, 3], this[2, 3] };
		var r = new double[16];
		for ( var i = 0; i < 3; i++ )
		{
			for ( var j = 0; j < 3; j++ ) r[i * 4 + j] = inv[i, j];
			r[i * 4 + 3] = -(inv[i, 0] * t[0] + inv[i, 1] * t[1] + inv[i, 2] * t[2]);
		}
		r[15] = 1;
		return new( r );
	}

	public bool IsIdentity( double tolerance = 1e-4 ) => Enumerable.Range( 0, 16 ).All( i => Math.Abs( m[i] - Identity.m[i] ) <= tolerance * (i % 4 == 3 && i < 12 ? 100 : 1) );

	/// <summary>Translation, rotation quaternion (x, y, z, w) and scale. Shear is discarded; a mirror goes into the X scale.</summary>
	public (double[] Position, double[] Rotation, double[] Scale) Decompose()
	{
		var columns = Enumerable.Range( 0, 3 ).Select( j => new[] { this[0, j], this[1, j], this[2, j] } ).ToArray();
		var scale = columns.Select( c => Math.Sqrt( c[0] * c[0] + c[1] * c[1] + c[2] * c[2] ) ).ToArray();
		var det = this[0, 0] * (this[1, 1] * this[2, 2] - this[1, 2] * this[2, 1]) - this[0, 1] * (this[1, 0] * this[2, 2] - this[1, 2] * this[2, 0]) + this[0, 2] * (this[1, 0] * this[2, 1] - this[1, 1] * this[2, 0]);
		if ( det < 0 ) scale[0] = -scale[0];
		var r = new double[3, 3];
		for ( var j = 0; j < 3; j++ )
			for ( var i = 0; i < 3; i++ ) r[i, j] = scale[j] == 0 ? (i == j ? 1 : 0) : columns[j][i] / scale[j];
		return (new[] { this[0, 3], this[1, 3], this[2, 3] }, Quaternion( r ), scale);
	}

	static double[] Quaternion( double[,] r )
	{
		double trace = r[0, 0] + r[1, 1] + r[2, 2], x, y, z, w;
		if ( trace > 0 )
		{
			var s = Math.Sqrt( trace + 1 ) * 2;
			w = s / 4; x = (r[2, 1] - r[1, 2]) / s; y = (r[0, 2] - r[2, 0]) / s; z = (r[1, 0] - r[0, 1]) / s;
		}
		else if ( r[0, 0] > r[1, 1] && r[0, 0] > r[2, 2] )
		{
			var s = Math.Sqrt( 1 + r[0, 0] - r[1, 1] - r[2, 2] ) * 2;
			w = (r[2, 1] - r[1, 2]) / s; x = s / 4; y = (r[0, 1] + r[1, 0]) / s; z = (r[0, 2] + r[2, 0]) / s;
		}
		else if ( r[1, 1] > r[2, 2] )
		{
			var s = Math.Sqrt( 1 + r[1, 1] - r[0, 0] - r[2, 2] ) * 2;
			w = (r[0, 2] - r[2, 0]) / s; x = (r[0, 1] + r[1, 0]) / s; y = s / 4; z = (r[1, 2] + r[2, 1]) / s;
		}
		else
		{
			var s = Math.Sqrt( 1 + r[2, 2] - r[0, 0] - r[1, 1] ) * 2;
			w = (r[1, 0] - r[0, 1]) / s; x = (r[0, 2] + r[2, 0]) / s; y = (r[1, 2] + r[2, 1]) / s; z = s / 4;
		}
		return new[] { x, y, z, w };
	}

	// Unity axes to s&box axes: s&box x = Unity z, y = -Unity x, z = Unity y. Improper (it changes handedness).
	static readonly UnityTransform UnityAxes = Linear( new double[,] { { 0, 0, 1 }, { -1, 0, 0 }, { 0, 1, 0 } } );

	/// <summary>A Unity local transform (meters) as the equivalent s&box local transform (inches).</summary>
	public static (double[] Position, double[] Rotation, double[] Scale) FromUnity( double[] position, double[] rotation, double[] scale )
	{
		var r = UnityAxes * Rotation( rotation[0], rotation[1], rotation[2], rotation[3] ) * UnityAxes.Inverse();
		var (_, quaternion, _) = r.Decompose();
		return (new[] { position[2] * InchesPerMeter, -position[0] * InchesPerMeter, position[1] * InchesPerMeter }, quaternion, new[] { scale[2], scale[0], scale[1] });
	}

	/// <summary>An s&amp;box local transform (inches) as the equivalent Unity local transform (meters): the inverse of <see cref="FromUnity"/>.</summary>
	public static (double[] Position, double[] Rotation, double[] Scale) ToUnity( (double[] Position, double[] Rotation, double[] Scale) sbox )
	{
		var (position, rotation, scale) = sbox;
		var r = UnityAxes.Inverse() * Rotation( rotation[0], rotation[1], rotation[2], rotation[3] ) * UnityAxes;
		return (new[] { -position[1] / InchesPerMeter, position[2] / InchesPerMeter, position[0] / InchesPerMeter }, r.Decompose().Rotation, new[] { scale[1], scale[2], scale[0] });
	}

	/// <summary>Conjugates this transform by <paramref name="basis"/>: the same motion expressed in the basis's target space.</summary>
	public UnityTransform In( UnityTransform basis ) => basis * this * basis.Inverse();

	public static string Format( double[] values ) => string.Join( ",", values.Select( v => (Math.Abs( v ) < 1e-9 ? 0 : v).ToString( "0.######", CultureInfo.InvariantCulture ) ) );
}