using System;

namespace SquishyTerry.Sim;

/// <summary>
/// Minimal float vector used by the face simulation. Kept independent of s&amp;box types so the
/// simulation can be unit-tested outside the engine against the original wasm build.
/// </summary>
public struct Vec3
{
	public float X, Y, Z;

	public Vec3( float x, float y, float z ) { X = x; Y = y; Z = z; }

	public static readonly Vec3 Zero = new( 0, 0, 0 );

	public static Vec3 operator +( Vec3 a, Vec3 b ) => new( a.X + b.X, a.Y + b.Y, a.Z + b.Z );
	public static Vec3 operator -( Vec3 a, Vec3 b ) => new( a.X - b.X, a.Y - b.Y, a.Z - b.Z );
	public static Vec3 operator -( Vec3 a ) => new( -a.X, -a.Y, -a.Z );
	public static Vec3 operator *( Vec3 a, float s ) => new( a.X * s, a.Y * s, a.Z * s );
	public static Vec3 operator *( float s, Vec3 a ) => new( a.X * s, a.Y * s, a.Z * s );
	public static Vec3 operator /( Vec3 a, float s ) => new( a.X / s, a.Y / s, a.Z / s );

	public static float Dot( Vec3 a, Vec3 b ) => a.X * b.X + a.Y * b.Y + a.Z * b.Z;

	public static Vec3 Cross( Vec3 a, Vec3 b ) =>
		new( a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X );

	public static Vec3 Mul( Vec3 a, Vec3 b ) => new( a.X * b.X, a.Y * b.Y, a.Z * b.Z );

	public readonly float Length => MathF.Sqrt( X * X + Y * Y + Z * Z );
	public readonly float LengthSquared => X * X + Y * Y + Z * Z;

	public readonly Vec3 Normalized
	{
		get
		{
			float inv = 1.0f / MathF.Sqrt( X * X + Y * Y + Z * Z );
			return new Vec3( X * inv, Y * inv, Z * inv );
		}
	}

	public static Vec3 Lerp( Vec3 a, Vec3 b, float t ) => a * (1.0f - t) + b * t;

	public override readonly string ToString() => $"({X:0.#####}, {Y:0.#####}, {Z:0.#####})";
}

/// <summary>Quaternion (x, y, z, w), same convention as the original gl-matrix/C++ code.</summary>
public struct Quat
{
	public float X, Y, Z, W;

	public Quat( float x, float y, float z, float w ) { X = x; Y = y; Z = z; W = w; }

	public static readonly Quat Identity = new( 0, 0, 0, 1 );

	public static Quat FromAxisAngle( Vec3 axis, float angle )
	{
		float s = MathF.Sin( angle * 0.5f );
		return new Quat( axis.X * s, axis.Y * s, axis.Z * s, MathF.Cos( angle * 0.5f ) );
	}

	/// <summary>Hamilton product a*b (apply b first, then a).</summary>
	public static Quat operator *( Quat a, Quat b ) => new(
		a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
		a.W * b.Y - a.X * b.Z + a.Y * b.W + a.Z * b.X,
		a.W * b.Z + a.X * b.Y - a.Y * b.X + a.Z * b.W,
		a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z );

	public readonly Quat Inverse => new( -X, -Y, -Z, W );

	public readonly Quat Normalized
	{
		get
		{
			float inv = 1.0f / MathF.Sqrt( X * X + Y * Y + Z * Z + W * W );
			return new Quat( X * inv, Y * inv, Z * inv, W * inv );
		}
	}

	public readonly Vec3 Rotate( Vec3 v )
	{
		// v' = v + 2w(q x v) + 2 q x (q x v)
		float tx = 2f * (Y * v.Z - Z * v.Y);
		float ty = 2f * (Z * v.X - X * v.Z);
		float tz = 2f * (X * v.Y - Y * v.X);
		return new Vec3(
			v.X + W * tx + (Y * tz - Z * ty),
			v.Y + W * ty + (Z * tx - X * tz),
			v.Z + W * tz + (X * ty - Y * tx) );
	}

	/// <summary>gl-matrix quat.slerp.</summary>
	public static Quat Slerp( Quat a, Quat b, float t )
	{
		float bx = b.X, by = b.Y, bz = b.Z, bw = b.W;
		float cosom = a.X * bx + a.Y * by + a.Z * bz + a.W * bw;
		if ( cosom < 0 ) { cosom = -cosom; bx = -bx; by = -by; bz = -bz; bw = -bw; }
		float s0, s1;
		if ( 1.0f - cosom > 0.000001f )
		{
			float omega = MathF.Acos( cosom );
			float sinom = MathF.Sin( omega );
			s0 = MathF.Sin( (1 - t) * omega ) / sinom;
			s1 = MathF.Sin( t * omega ) / sinom;
		}
		else
		{
			s0 = 1 - t;
			s1 = t;
		}
		return new Quat( s0 * a.X + s1 * bx, s0 * a.Y + s1 * by, s0 * a.Z + s1 * bz, s0 * a.W + s1 * bw );
	}

	/// <summary>Column-major 3x3 rotation matrix.</summary>
	public readonly void ToMatrix( Span<float> m )
	{
		double x = X, y = Y, z = Z, w = W;
		m[0] = (float)(1 - 2 * y * y - 2 * z * z);
		m[1] = (float)(2 * x * y + 2 * w * z);
		m[2] = (float)(2 * x * z - 2 * w * y);
		m[3] = (float)(2 * x * y - 2 * w * z);
		m[4] = (float)(1 - 2 * x * x - 2 * z * z);
		m[5] = (float)(2 * y * z + 2 * w * x);
		m[6] = (float)(2 * x * z + 2 * w * y);
		m[7] = (float)(2 * y * z - 2 * w * x);
		m[8] = (float)(1 - 2 * x * x - 2 * y * y);
	}
}