using System;
using System.Drawing;
using NUnit.Framework;

namespace Manipulation
{
    public static class AnglesToCoordinatesTask
    {
        public static PointF[] GetJointPositions(double shoulder, double elbow, double wrist)
        {
            var result = new PointF[3];
            float elbowX = Manipulator.UpperArm * (float)Math.Cos(shoulder);
            float elbowY = (float)Manipulator.UpperArm * (float)Math.Sin(shoulder);
            result[0] = new PointF(elbowX, elbowY);
            var elbowAlpha = elbow - Math.PI / 2 - Math.PI / 2 + shoulder;
            var wristX = elbowX + Manipulator.Forearm * (float)Math.Cos(elbowAlpha);
            var wristY=elbowY+ Manipulator.Forearm * (float)Math.Sin(elbowAlpha);
            result[1] = new PointF(wristX, wristY);
            var wristAlpha = wrist - Math.PI / 2 + elbowAlpha;
            var palmX = wristX + Manipulator.Palm * (float)Math.Sin(wristAlpha);
            var palmY = wristY - Manipulator.Palm * (float)Math.Cos(wristAlpha);
            result[2] = new PointF(palmX, palmY);
            return result;
        }
    }
    [TestFixture]
    public class AnglesToCoordinatesTask_Tests
    {
        [TestCase(Math.PI / 2, Math.PI / 2, Math.PI, Manipulator.Forearm + Manipulator.Palm, Manipulator.UpperArm, Manipulator.Forearm, Manipulator.UpperArm, 0, Manipulator.UpperArm)]
        [TestCase(0, Math.PI, Math.PI, Manipulator.UpperArm + Manipulator.Forearm + Manipulator.Palm, 0, Manipulator.UpperArm + Manipulator.Forearm, 0, Manipulator.UpperArm, 0)]
        [TestCase(Math.PI / 2, Math.PI, Math.PI, 0, Manipulator.UpperArm + Manipulator.Forearm + Manipulator.Palm, 0, Manipulator.UpperArm + Manipulator.Forearm, 0, Manipulator.UpperArm)]
        public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY, double wristX, double wristY, double elbowX, double elbowY )
        {
            var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
            Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
            Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
            Assert.AreEqual(wristX, joints[1].X, 1e-5, "wrist X");
            Assert.AreEqual(wristY, joints[1].Y, 1e-5, "wrist Y");
            Assert.AreEqual(elbowX, joints[0].X, 1e-5, "elbow X");
            Assert.AreEqual(elbowY, joints[0].Y, 1e-5, "elbow Y");
            Assert.AreEqual(Math.Sqrt(Math.Pow((palmEndX - wristX),2)+Math.Pow((palmEndY - wristY),2)), Manipulator.Palm);
            Assert.AreEqual(Math.Sqrt(Math.Pow((elbowX - wristX), 2) + Math.Pow((elbowY - wristY), 2)), Manipulator.Forearm);
            Assert.AreEqual(Math.Sqrt(Math.Pow((elbowX), 2) + Math.Pow((elbowY), 2)), Manipulator.UpperArm);
        }
    }
}