using System;
using System.Collections.Generic;

namespace SquishyTerry.Sim;

/// <summary>Distance constraint (original: 12-byte struct {u32 a, u32 b, float rest}).</summary>
public struct Edge
{
	public int A, B;
	public float Rest;
}

/// <summary>Dihedral bending constraint across edge P1-P2 (original: 20-byte struct).</summary>
public struct Bend
{
	public int P1, P2, P3, P4;
	public float RestAngle;
}

/// <summary>
/// Distance constraints flattened into arrays for the solver hot loop (List indexers and struct
/// copies are expensive in the unoptimized editor build).
/// </summary>
public sealed class DistanceConstraints
{
	public readonly int[] A, B;
	public readonly float[] Rest;
	public readonly int Count;

	/// <param name="edges">Constraints in solve order.</param>
	public DistanceConstraints( List<Edge> edges )
	{
		Count = edges.Count;
		A = new int[Count];
		B = new int[Count];
		Rest = new float[Count];
		for ( int i = 0; i < Count; i++ )
		{
			var e = edges[i];
			A[i] = e.A;
			B[i] = e.B;
			Rest[i] = e.Rest;
		}
	}
}

/// <summary>Bending constraints flattened into arrays, see <see cref="DistanceConstraints"/>.</summary>
public sealed class BendConstraints
{
	public readonly int[] P1, P2, P3, P4;
	public readonly float[] RestAngle;
	public readonly int Count;

	/// <param name="bends">Constraints in solve order.</param>
	public BendConstraints( List<Bend> bends )
	{
		Count = bends.Count;
		P1 = new int[Count];
		P2 = new int[Count];
		P3 = new int[Count];
		P4 = new int[Count];
		RestAngle = new float[Count];
		for ( int i = 0; i < Count; i++ )
		{
			var b = bends[i];
			P1[i] = b.P1;
			P2[i] = b.P2;
			P3[i] = b.P3;
			P4[i] = b.P4;
			RestAngle[i] = b.RestAngle;
		}
	}
}

/// <summary>Mesh generation and PBD constraint helpers recovered from the wasm build.</summary>
public static class Geometry
{
	/// <summary>Array copy (Array.Clone is not on the s&amp;box whitelist).</summary>
	public static T[] Copy<T>( T[] src )
	{
		var dst = new T[src.Length];
		for ( int i = 0; i < src.Length; i++ ) dst[i] = src[i];
		return dst;
	}

	static readonly int[] IcoTris =
	{
		0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11,
		1, 5, 9, 5, 11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8,
		3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8, 3, 8, 9,
		4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1,
	};

	/// <summary>Unit icosphere (f_om / f_caa). Vertex order matches the original.</summary>
	public static void Icosphere( int levels, List<Vec3> pos, List<int> idx )
	{
		const float a = 0.525731f, b = 0.850651f;
		pos.Clear();
		pos.Add( new Vec3( -a, b, 0 ) ); pos.Add( new Vec3( a, b, 0 ) );
		pos.Add( new Vec3( -a, -b, 0 ) ); pos.Add( new Vec3( a, -b, 0 ) );
		pos.Add( new Vec3( 0, -a, b ) ); pos.Add( new Vec3( 0, a, b ) );
		pos.Add( new Vec3( 0, -a, -b ) ); pos.Add( new Vec3( 0, a, -b ) );
		pos.Add( new Vec3( b, 0, -a ) ); pos.Add( new Vec3( b, 0, a ) );
		pos.Add( new Vec3( -b, 0, -a ) ); pos.Add( new Vec3( -b, 0, a ) );

		idx.Clear();
		idx.AddRange( IcoTris );

		// The original dedupes midpoints with a linear search (tolerance 1e-4). On a sphere that is
		// equivalent to keying by edge, and the append order is identical.
		var mids = new Dictionary<long, int>();
		int Mid( int i, int j )
		{
			long key = i < j ? ((long)i << 32) | (uint)j : ((long)j << 32) | (uint)i;
			if ( mids.TryGetValue( key, out int m ) ) return m;
			var p = ((pos[i] + pos[j]) * 0.5f).Normalized;
			pos.Add( p );
			mids[key] = pos.Count - 1;
			return pos.Count - 1;
		}

		for ( int l = 0; l < levels; l++ )
		{
			var next = new List<int>( idx.Count * 4 );
			for ( int t = 0; t < idx.Count; t += 3 )
			{
				int k = idx[t], m = idx[t + 1], j = idx[t + 2];
				int g = Mid( k, m ), h = Mid( m, j ), i = Mid( j, k );
				next.Add( k ); next.Add( g ); next.Add( i );
				next.Add( m ); next.Add( h ); next.Add( g );
				next.Add( j ); next.Add( i ); next.Add( h );
				next.Add( g ); next.Add( h ); next.Add( i );
			}
			idx.Clear();
			idx.AddRange( next );
		}
	}

	/// <summary>
	/// Hop distance from <paramref name="center"/> through shared triangles, -1 beyond
	/// <paramref name="maxRing"/> (f_wl).
	/// </summary>
	public static int[] RingDistances( int vertexCount, IReadOnlyList<int> idx, int center, int maxRing )
	{
		var ring = new int[vertexCount];
		Array.Fill( ring, -1 );
		ring[center] = 0;
		for ( int b = 0; b < maxRing; b++ )
		{
			int a = b + 1;
			for ( int t = 0; t + 2 < idx.Count; t += 3 )
			{
				int i0 = idx[t], i1 = idx[t + 1], i2 = idx[t + 2];
				if ( ring[i0] == b ) { if ( ring[i1] == -1 ) ring[i1] = a; if ( ring[i2] == -1 ) ring[i2] = a; }
				if ( ring[i1] == b ) { if ( ring[i0] == -1 ) ring[i0] = a; if ( ring[i2] == -1 ) ring[i2] = a; }
				if ( ring[i2] == b ) { if ( ring[i0] == -1 ) ring[i0] = a; if ( ring[i1] == -1 ) ring[i1] = a; }
			}
		}
		return ring;
	}

	/// <summary>Area-weighted vertex normals (f_zb).</summary>
	public static void ComputeNormals( IReadOnlyList<Vec3> pos, IReadOnlyList<int> idx, Vec3[] normals )
	{
		Array.Clear( normals );
		for ( int t = 0; t + 2 < idx.Count; t += 3 )
		{
			int a = idx[t], b = idx[t + 1], c = idx[t + 2];
			var n = Vec3.Cross( pos[b] - pos[a], pos[c] - pos[a] );
			normals[a] += n;
			normals[b] += n;
			normals[c] += n;
		}
		for ( int i = 0; i < normals.Length; i++ )
			normals[i] = normals[i].Normalized;
	}

	/// <summary>
	/// Area-weighted vertex normals (f_zb), array fast path. The Vec3 math is inlined by hand
	/// because the editor build is unoptimized; the arithmetic order matches the overload above.
	/// </summary>
	public static void ComputeNormals( Vec3[] pos, int[] idx, Vec3[] normals )
	{
		Array.Clear( normals );
		int count = idx.Length - 2;
		for ( int t = 0; t < count; t += 3 )
		{
			int a = idx[t], b = idx[t + 1], c = idx[t + 2];
			ref Vec3 pa = ref pos[a];
			ref Vec3 pb = ref pos[b];
			ref Vec3 pc = ref pos[c];
			float ux = pb.X - pa.X, uy = pb.Y - pa.Y, uz = pb.Z - pa.Z;
			float vx = pc.X - pa.X, vy = pc.Y - pa.Y, vz = pc.Z - pa.Z;
			float nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx;
			ref Vec3 na = ref normals[a];
			na.X += nx; na.Y += ny; na.Z += nz;
			ref Vec3 nb = ref normals[b];
			nb.X += nx; nb.Y += ny; nb.Z += nz;
			ref Vec3 nc = ref normals[c];
			nc.X += nx; nc.Y += ny; nc.Z += nz;
		}
		for ( int i = 0; i < normals.Length; i++ )
		{
			ref Vec3 n = ref normals[i];
			float inv = 1.0f / MathF.Sqrt( n.X * n.X + n.Y * n.Y + n.Z * n.Z );
			n.X *= inv; n.Y *= inv; n.Z *= inv;
		}
	}

	/// <summary>Unique edges in first-seen order, rest length scaled (f_il / f_ed).</summary>
	public static List<Edge> BuildEdges( IReadOnlyList<Vec3> pos, IReadOnlyList<int> idx, float scale )
	{
		var edges = new List<Edge>();
		var seen = new HashSet<long>();
		void Add( int a, int b )
		{
			long key = a < b ? ((long)a << 32) | (uint)b : ((long)b << 32) | (uint)a;
			if ( !seen.Add( key ) ) return;
			edges.Add( new Edge { A = a, B = b, Rest = (pos[a] - pos[b]).Length * scale } );
		}
		for ( int t = 0; t + 2 < idx.Count; t += 3 )
		{
			int i = idx[t], a = idx[t + 1], h = idx[t + 2];
			Add( i, a );
			Add( a, h );
			Add( h, i );
		}
		return edges;
	}

	/// <summary>
	/// One bending constraint per interior edge p1&lt;=p2, found by searching for the triangle that
	/// contains the reversed edge (f_zk / f_dd).
	/// </summary>
	public static List<Bend> BuildBends( IReadOnlyList<Vec3> pos, IReadOnlyList<int> idx )
	{
		var bends = new List<Bend>();

		// Directed edge -> opposite vertex, so the O(n^2) search in the original becomes a lookup.
		// Iteration order is unaffected because each directed edge occurs once in a manifold mesh.
		var opposite = new Dictionary<long, int>();
		static long Key( int from, int to ) => ((long)from << 32) | (uint)to;
		for ( int t = 0; t + 2 < idx.Count; t += 3 )
		{
			int a = idx[t], b = idx[t + 1], c = idx[t + 2];
			opposite[Key( a, b )] = c;
			opposite[Key( b, c )] = a;
			opposite[Key( c, a )] = b;
		}

		void Try( int p1, int p2, int p3 )
		{
			if ( p2 < p1 ) return;
			if ( !opposite.TryGetValue( Key( p2, p1 ), out int p4 ) ) return;
			var e = pos[p2] - pos[p1];
			var n1 = Vec3.Cross( e, pos[p3] - pos[p1] );
			var n2 = Vec3.Cross( e, pos[p4] - pos[p1] );
			float d = Vec3.Dot( n1, n2 ) * (1.0f / n1.Length) * (1.0f / n2.Length);
			// Clamped: nearly coplanar triangles can round to |d| > 1 (never happens on the original sphere).
			bends.Add( new Bend { P1 = p1, P2 = p2, P3 = p3, P4 = p4, RestAngle = MathF.Acos( Math.Clamp( d, -1.0f, 1.0f ) ) } );
		}

		for ( int t = 0; t + 2 < idx.Count; t += 3 )
		{
			int e = idx[t], f = idx[t + 1], a = idx[t + 2];
			Try( e, f, a );
			Try( f, a, e );
			Try( a, e, f );
		}
		return bends;
	}

	/// <summary>Gauss-Seidel PBD distance projection, stiffness 1 (f_vh).</summary>
	public static void SolveDistance( DistanceConstraints edges, Vec3[] p, float[] w )
	{
		// Vec3 math inlined by hand (operators cost a call each in the unoptimized editor build).
		// Every expression keeps the original evaluation order so results stay bit-identical.
		int[] ea = edges.A, eb = edges.B;
		float[] rest = edges.Rest;
		int count = edges.Count;
		for ( int i = 0; i < count; i++ )
		{
			int ia = ea[i], ib = eb[i];
			ref Vec3 pa = ref p[ia];
			ref Vec3 pb = ref p[ib];
			float dx = pb.X - pa.X, dy = pb.Y - pa.Y, dz = pb.Z - pa.Z;
			float len = MathF.Sqrt( dx * dx + dy * dy + dz * dz );
			if ( len < 0.000001f ) continue;
			float wa = w[ia], wb = w[ib];
			if ( wa == 0.0f && wb == 0.0f ) continue;
			float q = len - rest[i];
			float cx = q * dx / len, cy = q * dy / len, cz = q * dz / len;
			float sum = wb + wa;
			float sa = wa / sum, sb = wb / sum;
			pa.X += cx * sa; pa.Y += cy * sa; pa.Z += cz * sa;
			pb.X -= cx * sb; pb.Y -= cy * sb; pb.Z -= cz * sb;
		}
	}

	/// <summary>Müller et al. 2007 PBD dihedral bending (f_hjb).</summary>
	public static void SolveBending( BendConstraints bends, Vec3[] p, float[] w, float stiffness )
	{
		// Inlined like SolveDistance. n1 x p2 is exactly -(p2 x n1) in IEEE float (same products,
		// negated difference), so q3 and q4 share the two cross products A and B. The four points
		// are read by value and written back once (faster than ref access in the unoptimized build);
		// P1..P4 are always distinct vertices, so this equals updating them in place.
		int[] b1 = bends.P1, b2 = bends.P2, b3 = bends.P3, b4 = bends.P4;
		float[] restAngle = bends.RestAngle;
		int count = bends.Count;
		for ( int i = 0; i < count; i++ )
		{
			int i1 = b1[i], i2 = b2[i], i3 = b3[i], i4 = b4[i];
			float w1 = w[i1], w2 = w[i2], w3 = w[i3], w4 = w[i4];
			if ( w1 == 0 || w2 == 0 || w3 == 0 || w4 == 0 ) continue;

			Vec3 r1 = p[i1], r2 = p[i2], r3 = p[i3], r4 = p[i4];
			float ox = r1.X, oy = r1.Y, oz = r1.Z;
			float p2x = r2.X - ox, p2y = r2.Y - oy, p2z = r2.Z - oz;
			float p3x = r3.X - ox, p3y = r3.Y - oy, p3z = r3.Z - oz;
			float p4x = r4.X - ox, p4y = r4.Y - oy, p4z = r4.Z - oz;

			float c23x = p2y * p3z - p2z * p3y, c23y = p2z * p3x - p2x * p3z, c23z = p2x * p3y - p2y * p3x;
			float l23 = MathF.Sqrt( c23x * c23x + c23y * c23y + c23z * c23z );
			if ( l23 < 0.00001f ) continue;
			float n1x = c23x / l23, n1y = c23y / l23, n1z = c23z / l23;

			float c24x = p2y * p4z - p2z * p4y, c24y = p2z * p4x - p2x * p4z, c24z = p2x * p4y - p2y * p4x;
			float l24 = MathF.Sqrt( c24x * c24x + c24y * c24y + c24z * c24z );
			if ( l24 < 0.00001f ) continue;
			float n2x = c24x / l24, n2y = c24y / l24, n2z = c24z / l24;

			float d = n1x * n2x + n1y * n2y + n1z * n2z;
			if ( d >= 1.0f || d <= -1.0f ) continue;

			// A = p2 x n2, B = p2 x n1
			float ax = p2y * n2z - p2z * n2y, ay = p2z * n2x - p2x * n2z, az = p2x * n2y - p2y * n2x;
			float bx = p2y * n1z - p2z * n1y, by = p2z * n1x - p2x * n1z, bz = p2x * n1y - p2y * n1x;
			// q3 = (A + (n1 x p2) d) / l23 ; q4 = (B + (n2 x p2) d) / l24
			float q3x = (ax - bx * d) / l23, q3y = (ay - by * d) / l23, q3z = (az - bz * d) / l23;
			float q4x = (bx - ax * d) / l24, q4y = (by - ay * d) / l24, q4z = (bz - az * d) / l24;

			// q2 = -((p3 x n2 + (n1 x p3) d) / l23) - (p4 x n1 + (n2 x p4) d) / l24
			float ex = p3y * n2z - p3z * n2y, ey = p3z * n2x - p3x * n2z, ez = p3x * n2y - p3y * n2x;
			float fx = n1y * p3z - n1z * p3y, fy = n1z * p3x - n1x * p3z, fz = n1x * p3y - n1y * p3x;
			float gx = p4y * n1z - p4z * n1y, gy = p4z * n1x - p4x * n1z, gz = p4x * n1y - p4y * n1x;
			float hx = n2y * p4z - n2z * p4y, hy = n2z * p4x - n2x * p4z, hz = n2x * p4y - n2y * p4x;
			float q2x = -((ex + fx * d) / l23) - (gx + hx * d) / l24;
			float q2y = -((ey + fy * d) / l23) - (gy + hy * d) / l24;
			float q2z = -((ez + fz * d) / l23) - (gz + hz * d) / l24;
			float q1x = -q2x - q3x - q4x, q1y = -q2y - q3y - q4y, q1z = -q2z - q3z - q4z;

			float denom = w1 * (q1x * q1x + q1y * q1y + q1z * q1z) + w2 * (q2x * q2x + q2y * q2y + q2z * q2z)
				+ w3 * (q3x * q3x + q3y * q3y + q3z * q3z) + w4 * (q4x * q4x + q4y * q4y + q4z * q4z);
			if ( denom < 0.00001f ) continue;

			float lambda = MathF.Sqrt( 1.0f - d * d ) * stiffness * (MathF.Acos( d ) - restAngle[i]) / denom;
			float s1 = lambda * w1, s2 = lambda * w2, s3 = lambda * w3, s4 = lambda * w4;
			p[i1] = new Vec3( r1.X - q1x * s1, r1.Y - q1y * s1, r1.Z - q1z * s1 );
			p[i2] = new Vec3( r2.X - q2x * s2, r2.Y - q2y * s2, r2.Z - q2z * s2 );
			p[i3] = new Vec3( r3.X - q3x * s3, r3.Y - q3y * s3, r3.Z - q3z * s3 );
			p[i4] = new Vec3( r4.X - q4x * s4, r4.Y - q4y * s4, r4.Z - q4z * s4 );
		}
	}

	/// <summary>Möller–Trumbore; like the original it does not reject hits behind the origin (f_kc).</summary>
	public static bool RayTriangle( Vec3 origin, Vec3 dir, Vec3 a, Vec3 b, Vec3 c, out float t )
	{
		t = 0;
		var e1 = b - a;
		var e2 = c - a;
		var pv = Vec3.Cross( dir, e2 );
		float det = Vec3.Dot( e1, pv );
		if ( det > -1e-8f && det < 1e-8f ) return false;
		float inv = 1.0f / det;
		var tv = origin - a;
		float u = Vec3.Dot( tv, pv ) * inv;
		if ( u < 0.0f || u > 1.0f ) return false;
		var qv = Vec3.Cross( tv, e1 );
		float v = Vec3.Dot( dir, qv ) * inv;
		if ( v < 0.0f || u + v > 1.0f ) return false;
		t = Vec3.Dot( e2, qv ) * inv;
		return true;
	}
}