using System;
using System.Collections.Generic;

namespace SquishyTerry.Sim;

/// <summary>
/// Small soft body glued to the face: an eye, the nose or an ear. Built from a clipped level-3
/// icosphere and anchored to one face vertex plus that feature patch's shape-matching rotation.
/// Original: 172-byte class, ctor f_hc, update f_gc.
/// </summary>
public sealed class Part
{
	public Vec3[] Pos, PrevPos, Pred, Vel, PrevVel, Normals;
	public int[] Indices;
	/// <summary>Indices plus a fan cap over the base rim, used for volume.</summary>
	public int[] ClosedIndices;
	public float[] InvMass;
	/// <summary>Normalized height 0..1 above the cut plane (before scaling).</summary>
	public float[] Height;
	public Vec3[] RestLocal;
	/// <summary>Optional per-vertex shading mask (e.g. nostrils), or null.</summary>
	public float[] Mask;
	/// <summary>Rest shape before it was rotated onto the anchor normal (+Z out of the face, +Y up).</summary>
	public Vec3[] RestUnaligned;
	/// <summary>Rotation taking <see cref="RestUnaligned"/> to <see cref="RestLocal"/>.</summary>
	public Quat Align;
	readonly DistanceConstraints _distance;
	// Per-vertex soft blend weight f (constant, from Height) and 1 - f; goal-position scratch.
	readonly float[] _blend, _blendKeep;
	readonly Vec3[] _goal;

	public int VertexCount => Pos.Length;

	/// <param name="shape">Optional sculpt applied to the unit sphere before clipping (null in the original).</param>
	/// <param name="detail">Icosphere subdivisions (3 in the original).</param>
	/// <param name="mask">Optional per-vertex shading mask, evaluated on the sculpted unit sphere (see <see cref="Mask"/>).</param>
	public Part( Vec3 anchor, Vec3 normal, Vec3 scale, float cutZ, float shear, float rotZ, Func<Vec3, Vec3> shape = null, int detail = 3, Func<Vec3, float> mask = null )
	{
		var pos = new List<Vec3>();
		var idx = new List<int>();
		Geometry.Icosphere( detail, pos, idx );
		if ( shape is not null )
		{
			for ( int i = 0; i < pos.Count; i++ )
				pos[i] = shape( pos[i] );
		}

		// --- Clip the sphere at z >= cutZ (f_cd / f_bd / f_mc) ---
		var clipped = new List<bool>( new bool[pos.Count] );
		var cutVerts = new Dictionary<long, int>();
		var outIdx = new List<int>();

		int CutVertex( int v1, int v2, float d1, float d2 )
		{
			int lo = Math.Min( v1, v2 ), hi = Math.Max( v1, v2 );
			long key = ((long)lo << 32) | (uint)hi;
			if ( cutVerts.TryGetValue( key, out int existing ) ) return existing;
			float dLo = MathF.Abs( v1 > v2 ? d2 : d1 );
			float dHi = MathF.Abs( v1 > v2 ? d1 : d2 );
			float r = dLo / (dLo + dHi);
			pos.Add( pos[hi] * r + pos[lo] * (1.0f - r) );
			clipped.Add( true );
			cutVerts[key] = pos.Count - 1;
			return pos.Count - 1;
		}

		void OneIn( int a, int b, int c, float da, float db, float dc )
		{
			if ( !(da > 0 && db < 0 && dc < 0) ) return;
			int k = CutVertex( a, b, da, db );
			int h = CutVertex( a, c, da, dc );
			outIdx.Add( a ); outIdx.Add( k ); outIdx.Add( h );
		}

		void TwoIn( int a, int b, int c, float da, float db, float dc )
		{
			if ( !(da > 0 && db > 0 && dc < 0) ) return;
			int n = CutVertex( a, c, da, dc );
			int j = CutVertex( b, c, db, dc );
			outIdx.Add( a ); outIdx.Add( b ); outIdx.Add( j );
			outIdx.Add( a ); outIdx.Add( j ); outIdx.Add( n );
		}

		for ( int t = 0; t + 2 < idx.Count; t += 3 )
		{
			int a = idx[t], b = idx[t + 1], c = idx[t + 2];
			float da = pos[a].Z - cutZ, db = pos[b].Z - cutZ, dc = pos[c].Z - cutZ;
			if ( da >= 0 && db >= 0 && dc >= 0 )
			{
				outIdx.Add( a ); outIdx.Add( b ); outIdx.Add( c );
				continue;
			}
			OneIn( a, b, c, da, db, dc );
			OneIn( b, c, a, db, dc, da );
			OneIn( c, a, b, dc, da, db );
			TwoIn( a, b, c, da, db, dc );
			TwoIn( b, c, a, db, dc, da );
			TwoIn( c, a, b, dc, da, db );
		}

		// --- Drop unused vertices, keeping order (f_pq) ---
		var used = new bool[pos.Count];
		foreach ( int i in outIdx ) used[i] = true;
		var remap = new int[pos.Count];
		var newPos = new List<Vec3>();
		var newClipped = new List<bool>();
		for ( int i = 0; i < pos.Count; i++ )
		{
			if ( !used[i] ) continue;
			remap[i] = newPos.Count;
			newPos.Add( pos[i] );
			newClipped.Add( clipped[i] );
		}
		Indices = new int[outIdx.Count];
		for ( int i = 0; i < outIdx.Count; i++ ) Indices[i] = remap[outIdx[i]];
		int n = newPos.Count;
		var p = newPos.ToArray();

		if ( mask is not null )
		{
			Mask = new float[n];
			for ( int i = 0; i < n; i++ ) Mask[i] = mask( p[i] );
		}

		// --- Shape it ---
		for ( int i = 0; i < n; i++ )
		{
			float z = p[i].Z - cutZ;
			p[i] = new Vec3( p[i].X + z * shear, p[i].Y, z );
		}

		Height = new float[n];
		float hs = 1.0f / (1.0f - cutZ);
		for ( int i = 0; i < n; i++ ) Height[i] = p[i].Z * hs;

		float sinZ = MathF.Sin( rotZ * 0.5f ), cosZ = MathF.Cos( rotZ * 0.5f );
		float two = sinZ * 2.0f;
		for ( int i = 0; i < n; i++ )
		{
			float x = p[i].X * scale.X, y = p[i].Y * scale.Y, z = p[i].Z * scale.Z;
			p[i] = new Vec3( x - two * (y * cosZ + x * sinZ), two * (x * cosZ - y * sinZ) + y, z );
		}

		// --- Base rim loop and closed index list (f_ry) ---
		var next = new Dictionary<int, int>();
		for ( int t = 0; t + 2 < Indices.Length; t += 3 )
		{
			int a = Indices[t], b = Indices[t + 1], c = Indices[t + 2];
			if ( newClipped[a] && newClipped[b] ) next[a] = b;
			if ( newClipped[b] && newClipped[c] ) next[b] = c;
			if ( newClipped[c] && newClipped[a] ) next[c] = a;
		}
		var rim = new List<int>();
		int start = newClipped.IndexOf( true );
		rim.Add( start );
		while ( true )
		{
			next.TryGetValue( rim[^1], out int nx );
			if ( nx == rim[0] || rim.Count > n ) break;
			rim.Add( nx );
		}
		var closed = new List<int>( Indices );
		for ( int j = 1; j < rim.Count - 1; j++ )
		{
			closed.Add( rim[0] ); closed.Add( rim[j] ); closed.Add( rim[j + 1] );
		}
		ClosedIndices = closed.ToArray();

		InvMass = new float[n];
		for ( int i = 0; i < n; i++ ) InvMass[i] = p[i].Z <= 0.02f ? 0.0f : 1.0f;

		// --- Rotate +Z onto the anchor normal ---
		float xy = MathF.Sqrt( normal.X * normal.X + normal.Y * normal.Y );
		float half = MathF.Asin( xy ) * 0.5f;
		float k = 1.0f / xy * MathF.Sin( half );
		// The original divides by |n.xy|; guard the straight-ahead case it never hit.
		var align = xy > 1e-6f ? new Quat( -normal.Y * k, normal.X * k, 0, MathF.Cos( half ) ) : Quat.Identity;
		Align = align;
		RestUnaligned = Geometry.Copy( p );
		for ( int i = 0; i < n; i++ ) p[i] = align.Rotate( p[i] );

		RestLocal = Geometry.Copy( p );
		Pos = new Vec3[n];
		for ( int i = 0; i < n; i++ ) Pos[i] = p[i] + anchor;
		PrevPos = Geometry.Copy( Pos );
		Pred = new Vec3[n];
		Vel = new Vec3[n];
		PrevVel = new Vec3[n];
		Normals = new Vec3[n];
		Geometry.ComputeNormals( Pos, Indices, Normals );

		_distance = new DistanceConstraints( Geometry.BuildEdges( Pos, Indices, 0.85f ) );
		_goal = new Vec3[n];
		_blend = new float[n];
		_blendKeep = new float[n];
		for ( int i = 0; i < n; i++ )
		{
			float s = Math.Clamp( Height[i] * 2.0f, 0.0f, 1.0f );
			float f = s * s * 0.7f * (3.0f - s * 2.0f);
			_blend[i] = f;
			_blendKeep[i] = 1.0f - f;
		}
		// The original also builds bending constraints here but never solves them.
	}

	/// <param name="anchor">Face vertex position the part sits on.</param>
	/// <param name="normal">Face vertex normal.</param>
	/// <param name="rotation">Rotation of the underlying shape-matching patch.</param>
	/// <param name="anchorVelocity">Face vertex velocity; above 0.05 the part goes nearly rigid.</param>
	public void Update( Vec3 anchor, Vec3 normal, Quat rotation, Vec3 anchorVelocity )
	{
		// Vec3 math written out by hand (the editor build is unoptimized), in the original evaluation
		// order. Loops that only touch vertex i are fused; goal = anchor + rotation * RestLocal is
		// computed once per vertex.
		Vec3[] pos = Pos, prevPos = PrevPos, pred = Pred, vel = Vel, prevVel = PrevVel, restLocal = RestLocal, goal = _goal;
		float[] invMass = InvMass;
		int n = pos.Length;
		float ax = anchor.X, ay = anchor.Y, az = anchor.Z;
		float qx = rotation.X, qy = rotation.Y, qz = rotation.Z, qw = rotation.W;
		for ( int i = 0; i < n; i++ )
		{
			ref Vec3 vl = ref vel[i];
			vl.X *= 0.98f; vl.Y *= 0.98f; vl.Z *= 0.98f;

			ref Vec3 r = ref restLocal[i];
			float tx = 2f * (qy * r.Z - qz * r.Y);
			float ty = 2f * (qz * r.X - qx * r.Z);
			float tz = 2f * (qx * r.Y - qy * r.X);
			ref Vec3 g = ref goal[i];
			g.X = ax + (r.X + qw * tx + (qy * tz - qz * ty));
			g.Y = ay + (r.Y + qw * ty + (qz * tx - qx * tz));
			g.Z = az + (r.Z + qw * tz + (qx * ty - qy * tx));

			ref Vec3 pr = ref pred[i];
			if ( invMass[i] == 0.0f )
			{
				pr = g;
			}
			else
			{
				ref Vec3 p = ref pos[i];
				pr.X = p.X + vl.X; pr.Y = p.Y + vl.Y; pr.Z = p.Z + vl.Z;
			}
		}

		Geometry.SolveDistance( _distance, pred, invMass );

		bool rigid = MathF.Sqrt( anchorVelocity.X * anchorVelocity.X + anchorVelocity.Y * anchorVelocity.Y + anchorVelocity.Z * anchorVelocity.Z ) > 0.05f;
		float[] blend = _blend, blendKeep = _blendKeep;
		float nx = normal.X, ny = normal.Y, nz = normal.Z;
		for ( int i = 0; i < n; i++ )
		{
			ref Vec3 g = ref goal[i];
			ref Vec3 pr = ref pred[i];
			if ( rigid )
			{
				pr.X = g.X * 0.9f + pr.X * 0.1f;
				pr.Y = g.Y * 0.9f + pr.Y * 0.1f;
				pr.Z = g.Z * 0.9f + pr.Z * 0.1f;
			}
			else
			{
				float f = blend[i], keep = blendKeep[i];
				pr.X = g.X * keep + pr.X * f;
				pr.Y = g.Y * keep + pr.Y * f;
				pr.Z = g.Z * keep + pr.Z * f;
			}

			// Keep everything above the face's tangent plane.
			float d = (pr.X - ax) * nx + (pr.Y - ay) * ny + (pr.Z - az) * nz;
			if ( d < 0.0f && invMass[i] != 0.0f )
			{
				pr.X -= nx * d; pr.Y -= ny * d; pr.Z -= nz * d;
			}

			ref Vec3 p = ref pos[i];
			ref Vec3 vl = ref vel[i];
			prevVel[i] = vl;
			vl.X = pr.X - p.X; vl.Y = pr.Y - p.Y; vl.Z = pr.Z - p.Z;
			prevPos[i] = p;
			p = pr;
		}
		Geometry.ComputeNormals( pos, Indices, Normals );
	}

	public float ComputeVolume() => Metrics.Volume( Pos, ClosedIndices );

	public float ComputeAverageSpeed() => Metrics.AverageSpeed( Vel );
}