package fall

import (
	"math"

	"silverpose/v2/internal/pose"
)

var torsoJoints = [...]int{5, 6, 11, 12}
var lowerJoints = [...]int{13, 14, 15, 16}

func assessPoseQuality(person pose.PersonPose, threshold float32, requireLowerBody bool) PoseQuality {
	if threshold < 0 || threshold > 1 {
		return PoseQuality{Reason: "invalid_threshold"}
	}
	visible := 0
	for _, keypoint := range person.Keypoints {
		if keypoint.Confidence >= threshold {
			visible++
		}
	}
	for _, index := range torsoJoints {
		if person.Keypoints[index].Confidence < threshold {
			return PoseQuality{Reason: "required_joint_low_confidence", VisibleJointCount: visible}
		}
	}
	if requireLowerBody {
		for _, index := range lowerJoints {
			if person.Keypoints[index].Confidence < threshold {
				return PoseQuality{Reason: "required_joint_low_confidence", VisibleJointCount: visible}
			}
		}
	}
	return PoseQuality{Accepted: true, Reason: "accepted", VisibleJointCount: visible}
}

func extractEvidence(person pose.PersonPose, quality PoseQuality, previous *PoseEvidence, horizontalAngleThreshold float32) PoseEvidence {
	if !quality.Accepted {
		return PoseEvidence{Reason: quality.Reason}
	}
	shoulderX, shoulderY := midpoint(person, 5, 6)
	hipX, hipY := midpoint(person, 11, 12)
	vectorX := hipX - shoulderX
	vectorY := hipY - shoulderY
	torsoLength := float32(math.Hypot(float64(vectorX), float64(vectorY)))
	if torsoLength == 0 {
		return PoseEvidence{HipCenterY: hipY, HasHipCenterY: true, TorsoLength: 0, Reason: "degenerate_torso"}
	}
	cosine := math.Max(-1, math.Min(1, math.Abs(float64(vectorX))/float64(torsoLength)))
	angle := float32(math.Acos(cosine) * 180 / math.Pi)
	rapid := previous != nil && previous.Accepted && previous.HasHipCenterY && hipY-previous.HipCenterY >= 0.5*torsoLength
	return PoseEvidence{
		Accepted: true, HorizontalPose: angle <= horizontalAngleThreshold,
		RapidVerticalChange: rapid, HorizontalAngleDegree: angle, HasAngle: true,
		HipCenterY: hipY, HasHipCenterY: true, TorsoLength: torsoLength, Reason: "accepted",
	}
}

func midpoint(person pose.PersonPose, first, second int) (float32, float32) {
	return (person.Keypoints[first].X + person.Keypoints[second].X) / 2,
		(person.Keypoints[first].Y + person.Keypoints[second].Y) / 2
}