/******************************************************************************
  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.Collections.ObjectModel;
using System.ComponentModel;
using System.Diagnostics;
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;
using System.Windows.Threading;

namespace RJ_Demo_IK
{
	/// <summary>
	/// 2D relaxation based inverse kinematics
	/// </summary>
	public partial class CalcIK_2D_ConstraintRelaxation : UserControl, INotifyPropertyChanged
	{
		#region Internal types
		
		// this class represents a bone in it's parent space
		public class BoneData : INotifyPropertyChanged
		{
			// actual bone data
			private double m_length = 0;
			private double m_angle = 0;
			private double m_weight = 0;

			// positions used to display the relaxation process
			private double m_relaxedX = 0;
			private double m_relaxedY = 0;

			#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
	
			public double Length
			{
				get { return m_length; }
				set { m_length = value; NotifyPropertyChanged("Length"); }
			}
			public double Radians
			{ 
				get { return m_angle; }
				set { m_angle = value; NotifyPropertyChanged("Radians"); NotifyPropertyChanged("Degrees"); }
			}
			public double Degrees
			{ 
				get { return m_angle * 180.0 / Math.PI; }
				set { m_angle = value * Math.PI / 180.0; NotifyPropertyChanged("Radians"); NotifyPropertyChanged("Degrees"); }
			}
			public double Weight
			{
				get { return m_weight; }
				set { m_weight = value; NotifyPropertyChanged("Weight"); }
			}
			public double RelaxedX
			{
				get { return m_relaxedX; }
				set { m_relaxedX = value; NotifyPropertyChanged("RelaxedX"); }
			}
			public double RelaxedY
			{
				get { return m_relaxedY; }
				set { m_relaxedY = value; NotifyPropertyChanged("RelaxedY"); }
			}
		}
		
		#endregion

		#region Private data

		private static Brush[] s_boneLineColors = 
		{
			Brushes.Black,
			Brushes.GreenYellow,
			Brushes.DarkBlue,
			Brushes.DarkRed,
			Brushes.MintCream,
		};

		// number of iterations to perform on each update
		private int m_iterationsPerUpdate = 0;

		// bone lengths
		private ObservableCollection<BoneData> m_bones = new ObservableCollection<BoneData>();
		
		// lines used to draw the bones
		class BoneLine
		{
			public Line m_actualBone = null;
			public Line m_relaxedBone = null;
		}
		private List<BoneLine> m_boneLines = new List<BoneLine>();
		
		// target position to reach for
		private Point m_targetPos = new Point(0,0);

		// positions used to display the relaxation process
		private double m_relaxedTargetX = 0;
		private double m_relaxedTargetY = 0;

		private DispatcherTimer m_updateTimer;

		#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 ObservableCollection<BoneData> Bones
		{
			get { return m_bones; }
		}

		public int IterationsPerUpdate
		{
			get { return m_iterationsPerUpdate; }
			set
			{
				m_iterationsPerUpdate = Math.Max(1,value);
				NotifyPropertyChanged("IterationsPerUpdate"); // update the bound UI
			}
		}

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

		public double TargetPosY
		{
			get { return m_targetPos.Y; }
			set
			{
				m_targetPos.Y = value;
				NotifyPropertyChanged("TargetPosY"); // update the bound UI
				UpdateDisplay(); // redraw
			}
		}

		#endregion

		#region Lifespan functions
		public CalcIK_2D_ConstraintRelaxation()
		{
			InitializeComponent();
			
			// set the iteration number
			IterationsPerUpdate = 1;

			// create the timer
			m_updateTimer = new DispatcherTimer(DispatcherPriority.Normal);
			m_updateTimer.Tick += new EventHandler(UpdateTimer_Tick);
			m_updateTimer.Interval = new TimeSpan(0, 0, 0, 0, 100);

			// add the initial bones
			AddBone();
			AddBone();

			TargetPosX = 100;
			TargetPosY = 0;

			// update the display
			UpdateDisplay();
		}
		#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>
		/// Add a new bone to the chain at the selected location or at the end if no location is selected.
		/// </summary>
		void AddBone()
		{
			BoneData newBone = new BoneData();
			newBone.Length = 50;
			newBone.Weight = 1;

			newBone.PropertyChanged += BonePropertyChanged;

			// insert at the end if no bone is selected
			if( _boneList.SelectedIndex == -1 )
				Bones.Add( newBone );
			else
				Bones.Insert( _boneList.SelectedIndex, newBone );
		}

		/// <summary>
		/// Remove a new bone from the chain at the selected location or from the end if no location is selected.
		/// </summary>
		private void RemoveBone()
		{
			if( Bones.Count == 0 )
				return;

			// remove the end bone if no bone is selected
			int removeIdx = _boneList.SelectedIndex;
			if( removeIdx == -1 )
				removeIdx = (Bones.Count - 1);


			Bones[removeIdx].PropertyChanged -= BonePropertyChanged;
			Bones.RemoveAt( removeIdx );
		}

		/// <summary>
		/// Perform an iteration of IK
		/// </summary>
		private void UpdateIK()
		{
			int numBones = Bones.Count;
			
			if( numBones == 0 )
				return;

			// calculate the bone angles
			List< IKSolver.Bone_2D_ConstraintRelaxation > relaxBones = new List< IKSolver.Bone_2D_ConstraintRelaxation >();
			for( int boneIdx = 0; boneIdx < numBones; ++boneIdx )
			{
				IKSolver.Bone_2D_ConstraintRelaxation newRelaxBone = new IKSolver.Bone_2D_ConstraintRelaxation();
				newRelaxBone.angle	= Bones[boneIdx].Radians;
				newRelaxBone.length	= Bones[boneIdx].Length;
				newRelaxBone.weight = Bones[boneIdx].Weight;
				relaxBones.Add( newRelaxBone );
			}

			// convert to worldspace bones
			List< IKSolver.Bone_2D_ConstraintRelaxation_World > worldBones;
			IKSolver.CalcIK_2D_ConstraintRelaxation_ConvertToWorld( out worldBones, relaxBones );

			// iterate IK
			for( int itrCount = 0; itrCount < IterationsPerUpdate; ++itrCount )
			{
				IKSolver.CalcIK_2D_ConstraintRelaxation( ref worldBones, TargetPosX, TargetPosY );
			}

			// convert bones back to local space
			IKSolver.CalcIK_2D_ConstraintRelaxation_ConvertToLocal( ref relaxBones, worldBones, TargetPosX, TargetPosY );
			
			// extract the new bone data from the results
			for( int boneIdx = 0; boneIdx < numBones; ++boneIdx )
			{
				Bones[boneIdx].Radians = relaxBones[boneIdx].angle;
				Bones[boneIdx].RelaxedX = worldBones[boneIdx].x;
				Bones[boneIdx].RelaxedY = worldBones[boneIdx].y;
			}

			// track the target position we used for relaxation (this prevents the relaxed bone display from changing while paused)
			m_relaxedTargetX = TargetPosX;
			m_relaxedTargetY = TargetPosY;
		}

		/// <summary>
		/// Update the scene displayed in the viewport
		/// </summary>
		private void UpdateDisplay()
		{
			int numBones = Bones.Count;
			
			// resize the number of bone lines
			while( m_boneLines.Count > numBones )
			{
				_viewport.Children.Remove( m_boneLines[m_boneLines.Count-1].m_actualBone );
				_viewport.Children.Remove( m_boneLines[m_boneLines.Count-1].m_relaxedBone );
				m_boneLines.RemoveAt( m_boneLines.Count-1 );
			}

			while( m_boneLines.Count < numBones )
			{
				BoneLine newBoneLine = new BoneLine();
				newBoneLine.m_actualBone = new Line();
				newBoneLine.m_actualBone.Stroke = s_boneLineColors[m_boneLines.Count % s_boneLineColors.Length];
				newBoneLine.m_actualBone.StrokeThickness	= 3;
				newBoneLine.m_actualBone.Opacity = 0.85;
				newBoneLine.m_actualBone.SetValue( Panel.ZIndexProperty, 100 );

				newBoneLine.m_relaxedBone = new Line();
				newBoneLine.m_relaxedBone.Stroke = s_boneLineColors[m_boneLines.Count % s_boneLineColors.Length];
				newBoneLine.m_relaxedBone.StrokeThickness = 7;
				newBoneLine.m_relaxedBone.Opacity = 0.15;
				newBoneLine.m_relaxedBone.SetValue( Panel.ZIndexProperty, 99 );

				m_boneLines.Add( newBoneLine );
				_viewport.Children.Add( newBoneLine.m_actualBone );
				_viewport.Children.Add( newBoneLine.m_relaxedBone );
			}

			// compute the orientations of the bone lines in logical space
			double curAngle = 0;
			for( int boneIdx = 0; boneIdx < numBones; ++boneIdx )
			{
				BoneData curBone = Bones[boneIdx];

				curAngle += curBone.Radians;
				double cosAngle = Math.Cos( curAngle );
				double sinAngle = Math.Sin( curAngle );

				if( boneIdx > 0 )
				{
					m_boneLines[boneIdx].m_actualBone.X1 = m_boneLines[boneIdx-1].m_actualBone.X2;
					m_boneLines[boneIdx].m_actualBone.Y1 = m_boneLines[boneIdx-1].m_actualBone.Y2;
				}
				else
				{
					m_boneLines[boneIdx].m_actualBone.X1 = 0;
					m_boneLines[boneIdx].m_actualBone.Y1 = 0;
				}

				m_boneLines[boneIdx].m_actualBone.X2 = m_boneLines[boneIdx].m_actualBone.X1 + cosAngle*curBone.Length;
				m_boneLines[boneIdx].m_actualBone.Y2 = m_boneLines[boneIdx].m_actualBone.Y1 + sinAngle*curBone.Length;

				m_boneLines[boneIdx].m_relaxedBone.X1 = curBone.RelaxedX;
				m_boneLines[boneIdx].m_relaxedBone.Y1 = curBone.RelaxedY;

				if( boneIdx < numBones-1 )
				{
					m_boneLines[boneIdx].m_relaxedBone.X2 = Bones[boneIdx+1].RelaxedX;
					m_boneLines[boneIdx].m_relaxedBone.Y2 = Bones[boneIdx+1].RelaxedY;
				}
				else
				{
					m_boneLines[boneIdx].m_relaxedBone.X2 = m_relaxedTargetX;
					m_boneLines[boneIdx].m_relaxedBone.Y2 = m_relaxedTargetY;
				}
			}

			// convert the bone positions to viewport space
			foreach( BoneLine curLine in m_boneLines )
			{
				curLine.m_actualBone.X1 = LogicalToViewportX(curLine.m_actualBone.X1);
				curLine.m_actualBone.Y1 = LogicalToViewportY(curLine.m_actualBone.Y1);

				curLine.m_actualBone.X2 = LogicalToViewportX(curLine.m_actualBone.X2);
				curLine.m_actualBone.Y2 = LogicalToViewportY(curLine.m_actualBone.Y2);

				curLine.m_relaxedBone.X1 = LogicalToViewportX(curLine.m_relaxedBone.X1);
				curLine.m_relaxedBone.Y1 = LogicalToViewportY(curLine.m_relaxedBone.Y1);

				curLine.m_relaxedBone.X2 = LogicalToViewportX(curLine.m_relaxedBone.X2);
				curLine.m_relaxedBone.Y2 = LogicalToViewportY(curLine.m_relaxedBone.Y2);
		}

			// draw the target
			Canvas.SetLeft( _targetEllipse, LogicalToViewportX(TargetPosX - _targetEllipse.Width/2) );
			Canvas.SetTop( _targetEllipse, LogicalToViewportY(TargetPosY + _targetEllipse.Height/2) );
			
			// 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;
		}

		/// <summary>
		/// Update logic at a set interval
		/// </summary>
		private void UpdateTimer_Tick(object sender, EventArgs e)
        {
			UpdateIK();
			UpdateDisplay();
		}

		#endregion
		
		#region Event Handlers

		private void BonePropertyChanged(object sender, PropertyChangedEventArgs e)
		{
			switch (e.PropertyName)
			{
				case "Radians":
					UpdateDisplay();
					break;
				case "Length":
					UpdateDisplay();
					break;
			}
		}

		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
			UpdateDisplay();
		}
		
		private void _thisWindow_IsVisibleChanged(object sender, DependencyPropertyChangedEventArgs e)
		{			
			if( m_updateTimer != null )
			{
				if( this.IsVisible )
				{
					if( _playRadioButton.IsChecked == true )
						m_updateTimer.Start();
				}
				else
				{
					m_updateTimer.Stop();
				}
			}
		}

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

		private void _addBoneButton_Click(object sender, RoutedEventArgs e)
		{
			AddBone();
			UpdateDisplay();
		}

		private void _removeBoneButton_Click(object sender, RoutedEventArgs e)
		{
			RemoveBone();
			UpdateDisplay();
		}	

		private void _playRadioButton_Checked(object sender, RoutedEventArgs e)
		{
			if( m_updateTimer != null )
				m_updateTimer.Start();
		}

		private void _pauseRadioButton_Checked(object sender, RoutedEventArgs e)
		{
			if( m_updateTimer != null )
				m_updateTimer.Stop();
		}

		private void _singleUpdateButton_Click(object sender, RoutedEventArgs e)
		{
			_pauseRadioButton.IsChecked = true;
			UpdateIK();
			UpdateDisplay();
		}

		#endregion
	}
}