/******************************************************************************
  Copyright (c) 2008-2009 Ryan Juckett
  http://www.ryanjuckett.com/

  This software is provided 'as-is', without any express or implied
  warranty. In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.

  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.

  3. This notice may not be removed or altered from any source
     distribution.
******************************************************************************/

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

namespace RJ_Demo_IK
{
	/// <summary>
	/// 2D two bone analytic based inverse kinematics for one solution
	/// </summary>
	public partial class CalcIK_2D_TwoBoneAnalytic : UserControl, INotifyPropertyChanged
	{
		#region Private data
		
		// bone lengths
		private double m_bone1_length = 100;
		private double m_bone2_length = 40;

		// bone angles
		private double m_bone1_angle = 0;
		private double m_bone2_angle = 0;

		// IK solution mode
		private bool m_bone2_usePositiveAngle = true;

		// target position to reach for
		private Point m_targetPos = new Point(0,0);

		// result of current IK calculation
		private bool m_validSolution = false;

		#endregion

		#region INotifyPropertyChanged interface

		// event used by the user interface to bind to our properties
		public event PropertyChangedEventHandler PropertyChanged;

		// helper function to notify PropertyChanged subscribers
		protected void NotifyPropertyChanged(string propertyName)
		{
			if (PropertyChanged != null)
			{
				PropertyChanged(this, new PropertyChangedEventArgs(propertyName));
			}
		}
		#endregion

		#region Public properties

		public bool ValidSolution
		{
			get { return m_validSolution; }
			set
			{
				m_validSolution = value;
				NotifyPropertyChanged("ValidSolution"); // update the bound UI
			}
		}

		public double Bone1_Length
		{
			get { return m_bone1_length; }
			set
			{
				m_bone1_length = Math.Max(0,value); // only allow positive length values
				UpdateBonePositions(); // redraw the IK state
			}
		}

		public double Bone2_Length
		{
			get { return m_bone2_length; }
			set
			{ 
				m_bone2_length = Math.Max(0,value); // only allow positive length values
				UpdateBonePositions(); // redraw the IK state
			}
		}

		public bool Bone2_UsePositiveAngle
		{
			get { return m_bone2_usePositiveAngle; }
			set
			{
				m_bone2_usePositiveAngle = value;
				UpdateBonePositions(); // redraw the IK state
			}
		}

		public double TargetPosX
		{
			get { return m_targetPos.X; }
			set
			{
				m_targetPos.X = value;
				NotifyPropertyChanged("TargetPosX"); // update the bound UI
				UpdateBonePositions(); // redraw the IK state
			}
		}

		public double TargetPosY
		{
			get { return m_targetPos.Y; }
			set
			{
				m_targetPos.Y = value;
				NotifyPropertyChanged("TargetPosY"); // update the bound UI
				UpdateBonePositions(); // redraw the IK state
			}
		}
		
		public double Bone1_Degrees { get { return m_bone1_angle * 180.0 / Math.PI; } }
		public double Bone2_Degrees { get { return m_bone2_angle * 180.0 / Math.PI; } }
		
		#endregion

		#region Lifespan functions
		public CalcIK_2D_TwoBoneAnalytic()
		{
			InitializeComponent();

			// iniitalize the desired position to the end point of the bones
			TargetPosX = m_bone1_length * Math.Cos(m_bone1_angle) + m_bone2_length * Math.Cos(m_bone1_angle + m_bone2_angle);
			TargetPosY = m_bone1_length * Math.Sin(m_bone1_angle) + m_bone2_length * Math.Sin(m_bone1_angle + m_bone2_angle);

			// update the display
			UpdateBonePositions();
		}
		#endregion
		
		#region Coordinate Conversion

		// compute the logical origin in _viewport coordinated
		private double ViewportWidth { get { return _viewportColumn.ActualWidth; } }
		private double ViewportHieght { get { return _mainGrid.ActualHeight; } }
		private double ViewportCenterX { get { return ViewportWidth / 2; } }
		private double ViewportCenterY { get { return ViewportHieght / 2; } }

		// convert logical coordinates to _viewport coordinates
		private double LogicalToViewportX(double logicalX) { return logicalX + ViewportCenterX; }
		private double LogicalToViewportY(double logicalY) { return -logicalY + ViewportCenterY; }
	
		// convert _viewport coordinates to logical coordinates
		private double ViewportToLogicalX(double viewportX) { return viewportX - ViewportCenterX; }
		private double ViewportToLogicalY(double viewportY) { return -viewportY + ViewportCenterY; }
		
		#endregion

		#region Logic Functions

		/// <summary>
		/// Evaluate the IK problem based on the current parameters and update the display
		/// shapes inside the current viewport.
		/// </summary>
		private void UpdateBonePositions()
		{
			// calculate the bone angles
			ValidSolution = IKSolver.CalcIK_2D_TwoBoneAnalytic(
								out m_bone1_angle, out m_bone2_angle,
								m_bone2_usePositiveAngle,
								Bone1_Length, Bone2_Length,
								TargetPosX, TargetPosY );

			// compute the the end points of the bones in logical space

			double cosAngle1 = Math.Cos(m_bone1_angle);
			double sinAngle1 = Math.Sin(m_bone1_angle);
			double cosAngle12 = Math.Cos(m_bone1_angle + m_bone2_angle);
			double sinAngle12 = Math.Sin(m_bone1_angle + m_bone2_angle);

			double bone1_endX = Bone1_Length * cosAngle1;
			double bone1_endY = Bone1_Length * sinAngle1;
			double bone2_endX = bone1_endX + Bone2_Length * cosAngle12;
			double bone2_endY = bone1_endY + Bone2_Length * sinAngle12;

			// output the coordinates in _viewport space
			_bone1_line.X1 = LogicalToViewportX( 0 );
			_bone1_line.Y1 = LogicalToViewportY( 0 );
			_bone1_line.X2 = LogicalToViewportX( bone1_endX );
			_bone1_line.Y2 = LogicalToViewportY( bone1_endY );

			_bone2_line.X1 = _bone1_line.X2;
			_bone2_line.Y1 = _bone1_line.Y2;
			_bone2_line.X2 = LogicalToViewportX( bone2_endX );
			_bone2_line.Y2 = LogicalToViewportY( bone2_endY );

			// compute the range of our bones
			double minRange = Math.Abs(Bone1_Length - Bone2_Length);
			double maxRange = Bone1_Length + Bone2_Length;

			// draw the boundaries
			Canvas.SetLeft( _minRangeEllipse, LogicalToViewportX(-minRange) );
			Canvas.SetTop( _minRangeEllipse, LogicalToViewportY(minRange) );
			_minRangeEllipse.Width = 2*minRange;
			_minRangeEllipse.Height = 2*minRange;

			Canvas.SetLeft( _maxRangeEllipse, LogicalToViewportX(-maxRange) );
			Canvas.SetTop( _maxRangeEllipse, LogicalToViewportY(maxRange) );
			_maxRangeEllipse.Width = 2*maxRange;
			_maxRangeEllipse.Height = 2*maxRange;

			// draw the range of bone2
			Canvas.SetLeft( _bone2RangeEllipse, LogicalToViewportX(bone1_endX - Bone2_Length) );
			Canvas.SetTop( _bone2RangeEllipse, LogicalToViewportY(bone1_endY + Bone2_Length) );
			_bone2RangeEllipse.Width = 2*Bone2_Length;
			_bone2RangeEllipse.Height = 2*Bone2_Length;

			// draw the target
			Canvas.SetLeft( _targetEllipse, LogicalToViewportX(TargetPosX - _targetEllipse.Width/2) );
			Canvas.SetTop( _targetEllipse, LogicalToViewportY(TargetPosY + _targetEllipse.Height/2) );
			
			// draw the extended bone 1 line
			_bone1_extended_line.X1 = LogicalToViewportX( 0 );
			_bone1_extended_line.Y1 = LogicalToViewportY( 0 );
			_bone1_extended_line.X2 = LogicalToViewportX( (Bone1_Length + Bone2_Length) * cosAngle1 );
			_bone1_extended_line.Y2 = LogicalToViewportY( (Bone1_Length + Bone2_Length) * sinAngle1 );

			// draw the axes
			_xAxisLine.X1 = 0;
			_xAxisLine.Y1 = ViewportCenterY;
			_xAxisLine.X2 = ViewportWidth;
			_xAxisLine.Y2 = ViewportCenterY;

			_yAxisLine.X1 = ViewportCenterX;
			_yAxisLine.Y1 = 0;
			_yAxisLine.X2 = ViewportCenterX;
			_yAxisLine.Y2 = ViewportHieght;

			// draw the theta arcs
			{
				double theta1ArcRadius = 0.3 * Bone1_Length;

				PathGeometry theta1ArcPathGeo = _theta1ArcPath.Data as PathGeometry;
				PathFigure theta1ArcPathFigure = theta1ArcPathGeo.Figures[0];
				ArcSegment theta1Arc = theta1ArcPathFigure.Segments[0] as ArcSegment;
				theta1Arc.Size = new Size(theta1ArcRadius,theta1ArcRadius);
				theta1ArcPathFigure.StartPoint = new Point( LogicalToViewportX(theta1ArcRadius), LogicalToViewportY(0) );
				theta1Arc.Point = new Point(	LogicalToViewportX( cosAngle1 * theta1ArcRadius ),
												LogicalToViewportY( sinAngle1 * theta1ArcRadius ) );
				theta1Arc.SweepDirection = (m_bone1_angle >= 0) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise;
				theta1Arc.IsLargeArc = (m_bone1_angle > Math.PI);
			}
			
			{
				double theta2ArcRadius = 0.3 * Math.Min( Bone1_Length, Bone2_Length );

				PathGeometry theta2ArcPathGeo = _theta2ArcPath.Data as PathGeometry;
				PathFigure theta2ArcPathFigure = theta2ArcPathGeo.Figures[0];
				ArcSegment theta2Arc = theta2ArcPathFigure.Segments[0] as ArcSegment;
				theta2Arc.Size = new Size(theta2ArcRadius,theta2ArcRadius);
				theta2ArcPathFigure.StartPoint = new Point(	LogicalToViewportX( (Bone1_Length + theta2ArcRadius) * cosAngle1 ),
															LogicalToViewportY( (Bone1_Length + theta2ArcRadius) * sinAngle1 ) );
				theta2Arc.Point = new Point(	LogicalToViewportX( bone1_endX + cosAngle12 * theta2ArcRadius ),
												LogicalToViewportY( bone1_endY + sinAngle12 * theta2ArcRadius ) );
				theta2Arc.SweepDirection = (Bone2_UsePositiveAngle) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise;
				theta2Arc.IsLargeArc = false;
			}

			// notify the user interface of changes
			NotifyPropertyChanged("Bone1_Degrees");
			NotifyPropertyChanged("Bone2_Degrees");
		}

		#endregion

		#region Event Handlers

		private void viewport_MouseLeftButtonDown(object sender, MouseButtonEventArgs e)
		{
			// capture the mouse to keep grabing MouseMove events if the user drags the
			// mouse outside of the _viewport bounds
			if (!_viewport.IsMouseCaptured)
            {
                _viewport.CaptureMouse();
			}

			// update the target position
			Point viewportPos = e.GetPosition(_viewport);
			TargetPosX = ViewportToLogicalX( viewportPos.X );
			TargetPosY = ViewportToLogicalY( viewportPos.Y );
		}

		private void viewport_MouseLeftButtonUp(object sender, MouseButtonEventArgs e)
		{
			// release the captured mouse
			if (_viewport.IsMouseCaptured)
            {
                _viewport.ReleaseMouseCapture();
            }
		}

		private void viewport_MouseMove(object sender, MouseEventArgs e)
		{
			// update the target position if we are still in a captured state
			// (i.e. the user has not released the mouse button)
			if (_viewport.IsMouseCaptured)
            {
				Point viewportPos = e.GetPosition(_viewport);
				TargetPosX = ViewportToLogicalX( viewportPos.X );
				TargetPosY = ViewportToLogicalY( viewportPos.Y );
            }
		}

		private void _thisWindow_SizeChanged(object sender, SizeChangedEventArgs e)
		{
			// update the display shapes based on the new window size
			UpdateBonePositions();
		}

		private void _websiteLink_Click(object sender, RoutedEventArgs e)
		{
			System.Diagnostics.Process.Start( "http://www.ryanjuckett.com" );
		}

		#endregion
	}
}