using System;
using System.Collections.Generic;
using System.Text;

namespace SalsaModel
{
    class Check
    {
      const double DefaultPrecision = 0.015;
      static public void Equal(double exp, double act) {
        Equal(exp, act, DefaultPrecision);
      }
      static public void Equal(Location exp, Location act) {
        Equal(exp, act, DefaultPrecision);
      }

        static public void Equal(double exp, double act, double prec)
        {
            if (Math.Abs(exp - act) > prec)
            {
                StringBuilder sb = new StringBuilder("Bad posture: expected ");
                sb.Append(exp);
                sb.Append(" but got ");
                sb.Append(act);
                throw new Exception(sb.ToString());
            }
        }
        static public void Equal(Location exp, Location act, double prec) {
          Equal(exp.X, act.X, prec);
          Equal(exp.Y, act.Y, prec);
          Equal(exp.Z, act.Z, prec);
        }
        public static void Posture(PositionModel model, BodyGeometry body)
        {
            Equal(0, model.LLeg.Toe.Z, 0.0001);
            Equal(0, model.RLeg.Toe.Z, 0.0001);
            Equal(model.HipHeight, model.RLeg.Hip.HalfWayTo(model.LLeg.Hip).Z);
            Equal(body.Shin, model.RLeg.Foot.DistanceTo(model.RLeg.Knee));
            Equal(body.Thigh, model.RLeg.Knee.DistanceTo(model.RLeg.Hip));
            Equal(body.Shin, model.LLeg.Foot.DistanceTo(model.LLeg.Knee));
            Equal(body.Thigh, model.LLeg.Knee.DistanceTo(model.LLeg.Hip));
            Equal(body.UpperArm, model.LArm.Shoulder.DistanceTo(model.LArm.Elbow));
            Equal(body.LowerArm, model.LArm.Elbow.DistanceTo(model.LArm.Wrist));
            Equal(body.UpperArm, model.RArm.Shoulder.DistanceTo(model.RArm.Elbow));
            Equal(body.LowerArm, model.RArm.Elbow.DistanceTo(model.RArm.Wrist));
            Equal(body.HipWidth, model.RLeg.Hip.DistanceTo(model.LLeg.Hip));
            Equal(body.ShoulderWidth, model.RArm.Shoulder.DistanceTo(model.LArm.Shoulder));
            Location hipMidpoint = model.RLeg.Hip.HalfWayTo(model.LLeg.Hip);
            Location shoulderMidpoint = model.RArm.Shoulder.HalfWayTo(model.LArm.Shoulder);
            Equal(body.Trunk, shoulderMidpoint.Z - hipMidpoint.Z);
        }
    }
}