using System;

namespace CharacterEditor;
/// <summary>Fit per-view 2D silhouettes using camera pose and native deformation responses.</summary>
public static class PhotoProfileFit
{
	public static Vector3[] Geometry(PhotoFace face)
	{
		var points = new Vector3[PhotoProfileAnchors.Ids.Length];
		for (int h = 0; h < 7; h++)
			for (int p = 0; p < points.Length; p++)
			{
				var point = PhotoProfileAnchors.Heads[h][p];
				for (int c = 0; c < PhotoProfileAnchors.Controls.Length; c++)
				{
					float offset = face.Offsets.GetValueOrDefault(PhotoProfileAnchors.Controls[c]);
					point += PhotoProfileAnchors.Deltas[h * PhotoProfileAnchors.Controls.Length * 2 + c * 2 + (offset >= 0 ? 1 : 0)][p] * MathF.Abs(offset) * 2;
				}

				// These semantic anchors lie on the midsagittal contour. Remove tiny
				// left/right offsets introduced by selecting the closest mesh vertex.
				points[p] += new Vector3(point.x, 0, point.z) * face.HeadWeights[h];
			}

		return points;
	}

	public static Vector2[] Project(Vector3[] points, float[] m) => FaceMeasurement.NormalizeProfile(points.Select(p => // Source axes -> canonical: right=-y, up=z, toward camera=x.
	new Vector2(-p.y * m[0] + p.z * m[4] + p.x * m[8], -(-p.y * m[1] + p.z * m[5] + p.x * m[9]))).ToArray(), m);
	public static float Error(PhotoFace face, IReadOnlyList<FaceMeasurement> views)
	{
		var points = Geometry(face);
		float total = 0, weight = 0;
		foreach (var view in views.Where(x => x.ProfileUsable))
		{
			var predicted = Project(points, view.Pose);
			float confidence = view.Quality * Math.Clamp((view.Yaw - 15) / 40, .1f, 1);
			foreach (int i in new[]
			{
				0,
				2,
				3,
				4,
				5
			}

			)
			{
				float importance = i is 2 or 3 ? 2 : i == 0 ? .25f : .75f;
				// Robust loss reduces the influence of an occluded or poorly placed point.
				float Huber(float x)
				{
					x = MathF.Abs(x);
					return x < .08f ? x * x : .16f * x - .0064f;
				}

				var delta = predicted[i] - view.Profile[i];
				total += confidence * importance * (Huber(delta.x) + Huber(delta.y));
				weight += importance;
			}
		}

		return weight > 0 ? total / weight : 0;
	}

	public static void Refine(PhotoFace face, IReadOnlyList<FaceMeasurement> views, Func<float[], float> frontError)
	{
		if (!views.Any(x => x.ProfileUsable))
			return;
		var starting = face.HeadWeights.ToArray();
		string[] controls =
		{
			"nose",
			"noseheight",
			"chin",
			"mouthprojection",
			"chinheight"
		};
		var initialOffsets = controls.ToDictionary(x => x, x => face.Offsets.GetValueOrDefault(x));
		float depthConfidence = Math.Clamp((views.Where(x => x.ProfileUsable).Max(x => x.Yaw) - 15) / 50, 0, 1);
		float Limit(string id) => id is "nose" or "noseheight" ? .15f + .35f * depthConfidence : .5f;
		float Score() => Error(face, views) + (frontError?.Invoke(face.HeadWeights) ?? 0) * .012f + .001f * controls.Sum(x => MathF.Pow(face.Offsets.GetValueOrDefault(x) - initialOffsets[x], 2)) + .00003f * Enumerable.Range(0, 7).Sum(i => MathF.Pow(face.HeadWeights[i] - starting[i], 2)) + .004f * Math.Max(0, -face.Offsets.GetValueOrDefault("nosewidth")) * MathF.Pow(Math.Max(0, face.Offsets.GetValueOrDefault("nose")), 2);
		float best = Score();
		foreach (float step in new[]
		{
			.2f,
			.08f,
			.025f,
			.008f
		}

		)
			for (int pass = 0; pass < 4; pass++)
			{
				bool changed = false;
				foreach (var id in controls)
				{
					float original = face.Offsets.GetValueOrDefault(id), chosen = original;
					foreach (float direction in new[]
					{
						-1f,
						1f
					}

					)
					{
						float limit = Limit(id);
						face.Offsets[id] = Math.Clamp(original + step * direction, -limit, limit);
						float error = Score();
						if (error < best)
						{
							best = error;
							chosen = face.Offsets[id];
							changed = true;
						}
					}

					face.Offsets[id] = chosen;
				}

				float headStep = step * .5f;
				for (int from = 0; from < 7; from++)
					for (int to = 0; to < 7; to++)
					{
						if (from == to || !PhotoFaceFit.SameFamily(from, to) || face.HeadWeights[from] < headStep)
							continue;
						face.HeadWeights[from] -= headStep;
						face.HeadWeights[to] += headStep;
						float error = Score();
						if (error < best)
						{
							best = error;
							changed = true;
						}
						else
						{
							face.HeadWeights[from] += headStep;
							face.HeadWeights[to] -= headStep;
						}
					}

				if (!changed)
					break;
			}

		var sum = face.HeadWeights.Sum();
		for (int i = 0; i < 7; i++)
			face.HeadWeights[i] /= sum;
	}
}