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


namespace Manipulation
{
    public static class ManipulatorTask
    {
        public static double[] MoveManipulatorTo(double x, double y, double angle)
        {
            var firstCatheterPalm = Manipulator.Palm * Math.Cos(angle);
            var firstX = x - firstCatheterPalm;
            var firstY = y;
            var secondCatheterPalm = Manipulator.Palm * Math.Sin(angle);
            var secondX = firstX;
            double secondY = firstY + secondCatheterPalm;
            var sideShoulderWrist = Math.Sqrt(Math.Pow(secondX, 2) + Math.Pow(secondY, 2));
            var elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, sideShoulderWrist);
            var oxShoulderWristCorner = Math.Atan2(secondY, secondX);
            var shoulder = TriangleTask.GetABAngle(sideShoulderWrist, Manipulator.UpperArm, Manipulator.Forearm) + oxShoulderWristCorner;
            var wrist = -angle - shoulder - elbow;
			if (double.IsNaN(elbow) || double.IsNaN(shoulder) || double.IsNaN(wrist))
            {
                wrist = double.NaN;
                elbow = double.NaN;
                shoulder = double.NaN;
            }
            return new[] { shoulder, elbow, wrist };
        }
    }
    [TestFixture]
    public class ManipulatorTask_Tests
    {
        [Test]
        public void TestMoveManipulatorTo()
        {
            var random = new Random();
            double x = random.NextDouble() * random.Next();
            double y = random.NextDouble() * random.Next();
            double angle = random.NextDouble() * random.Next();
            var expectedResult = (AnglesToCoordinatesTask.GetJointPositions(ManipulatorTask.MoveManipulatorTo(x, y, angle)[0], ManipulatorTask.MoveManipulatorTo(x, y, angle)[1], ManipulatorTask.MoveManipulatorTo(x, y, angle)[2]))[2];
            var result = new PointF((float)x, (float)y);
            Assert.AreEqual(expectedResult, result);
        }
    }
}