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 elbowPos = new PointF(0, (float) Manipulator.UpperArm);
            var wristPos = new PointF((float) Manipulator.Forearm, (float) Manipulator.UpperArm);
            var palmEndPos = new PointF((float) (Manipulator.Forearm + Manipulator.Palm), (float) Manipulator.UpperArm);
            elbowPos.X = (float)(Math.Cos(shoulder)*Manipulator.UpperArm);
            elbowPos.Y = (float)(Math.Sin(shoulder) * Manipulator.UpperArm);
            wristPos.X = (float)(elbowPos.X+ Math.Cos(elbow-shoulder)*Manipulator.Forearm);
            wristPos.Y = (float)(elbowPos.Y + Math.Sin(elbow - shoulder) * Manipulator.Forearm);
            palmEndPos.X = (float)(wristPos.X + Math.Cos(180 - wrist - elbow + shoulder) * Manipulator.Palm);
            palmEndPos.Y = (float)(wristPos.Y + Math.Sin(180 - wrist - elbow + shoulder) * Manipulator.Palm);
            return new PointF[]
            {
                elbowPos,
                wristPos,
                palmEndPos
            };
        }
    }
    [TestFixture]
    public class AnglesToCoordinatesTask_Tests
    {
        // Доработайте эти тесты!
        // С помощью строчки TestCase можно добавлять новые тестовые данные.
        // Аргументы TestCase превратятся в аргументы метода.
        [TestCase(Math.PI / 2, Math.PI / 2, Math.PI, Manipulator.Forearm + Manipulator.Palm, Manipulator.UpperArm)]
        public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
        {
            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.Fail("TODO: проверить, что расстояния между суставами равны длинам сегментов манипулятора!");
        }

    }

}