context: using UnityEngine;
  using System.Collections.Generic;
  
  using Pathfinding;
  using Pathfinding.Util;
  
  [AddComponentMenu ("Pathfinding/Modifiers/Simple Smooth")]
  [System.Serializable]
  /** Modifier which smooths the path. This modifier can smooth a path by either moving the points closer together (Simple) or using Bezier curves (Bezier).\n
   * \ingroup modifiers
   * Attach this component to the same GameObject as a Seeker component.
   * \n
   * This component will hook in to the Seeker's path post-processing system and will post process any paths it searches for.
   * Take a look at the Modifier Priorities settings on the Seeker component to determine where in the process this modifier should process the path.
   * \n
   * \n
   * Several smoothing types are available, here follows a list of them and a short description of what they do, and how they work.
   * But the best way is really to experiment with them yourself.\n
   * 
   * - <b>Simple</b> Smooths the path by drawing all points close to each other. This results in paths that might cut corners if you are not careful.
   * It will also subdivide the path to create more more points to smooth as otherwise it would still be quite rough.
   * \shadowimage{smooth_simple.png}
   * - <b>Bezier</b> Smooths the path using Bezier curves. This results a smooth path which will always pass through all points in the path, but make sure it doesn't turn too quickly.
   * \shadowimage{smooth_bezier.png}
   * - <b>OffsetSimple</b> An alternative to Simple smooth which will offset the path outwards in each step to minimize the corner-cutting.
   * But be careful, if too high values are used, it will turn into loops and look really ugly.
   * - <b>Curved Non Uniform</b> \shadowimage{smooth_curved_nonuniform.png}
   * 
   * \note Modifies vectorPath array
   * \todo Make the smooth modifier take the world geometry into account when smoothing
   * */
  public class SimpleSmoothModifier : MonoModifier {
  	
  #if UNITY_EDITOR
  	[UnityEditor.MenuItem ("CONTEXT/Seeker/Add Simple Smooth Modifier")]
  	public static void AddComp (UnityEditor.MenuCommand command) {
  		(command.context as Component).gameObject.AddComponent (typeof(SimpleSmoothModifier));
  	}
  #endif
  	
  	public override ModifierData input {
  		get { return ModifierData.All; }
  	}
  	
  	public override ModifierData output {
  		get {
  			ModifierData result = ModifierData.VectorPath;
  			if (iterations == 0 && smoothType == SimpleSmoothModifier.SmoothType.Simple && !uniformLength) {
  				result |= ModifierData.StrictVectorPath;
  			}
  			return result;
  		}
  	}
  	
  	/** Type of smoothing to use */
  	public SmoothType smoothType = SmoothType.Simple;
  	
  	/** Number of times to subdivide when not using a uniform length */
  	public int subdivisions = 2;
  	
  	/** Number of times to apply smoothing */
  	public int iterations = 2;
  	
  	/** The strength of the smoothing. A value from 0 to 1 is recommended, but values larger than 1 works too.*/
  	public float strength = 0.5F;
  	
  	/** Toggle to divide all lines in equal length segments.
  	  * \see #maxSegmentLength
  	  */
  	public bool uniformLength = true;
  	
  	/** The length of the segments in the smoothed path when using #uniformLength.
  	 * A high value yields rough paths and low value yields very smooth paths, but is slower */
  	public float maxSegmentLength = 2F;
  	
  	/** Length factor of the bezier curves' tangents' */
  	public float bezierTangentLength = 0.4F;
  	
  	/** Offset to apply in each smoothing iteration when using Offset Simple. \see #smoothType */
  	public float offset = 0.2F;
  	
  	public enum SmoothType {
  		Simple,
  		Bezier,
  		OffsetSimple,
  		CurvedNonuniform
  	}
  	
  	public override void Apply (Path p, ModifierData source) {
  		
  		//This should never trigger unless some other modifier has messed stuff up
  		if (p.vectorPath == null) {
  			Debug.LogWarning ("Can't process NULL path (has another modifier logged an error?)");
  			return;
  		}
  		
  		List<Vector3> path = null;
  		
  		switch (smoothType) {
  			case SmoothType.Simple:
  				path = SmoothSimple (p.vectorPath); break;
  			case SmoothType.Bezier:
  				path = SmoothBezier (p.vectorPath); break;
  			case SmoothType.OffsetSimple:
  				path = SmoothOffsetSimple (p.vectorPath); break;
  			case SmoothType.CurvedNonuniform:
  				path = CurvedNonuniform (p.vectorPath); break;
  		}
  		
  		if (path != p.vectorPath) {
  			ListPool<Vector3>.Release (p.vectorPath);
  			p.vectorPath = path;
  		}
  		//.vectorPath.Clear ();
  		//p.vectorPath.AddRange (path);
  	}
  	
  	public float factor = 0.1F;
  	
  	public List<Vector3> CurvedNonuniform (List<Vector3> path) {
  		
  		if (maxSegmentLength <= 0) {
  			Debug.LogWarning ("Max Segment Length is <= 0 which would cause DivByZero-exception or other nasty errors (avoid this)");
  			return path;
  		}
  		
  		int pointCounter = 0;
  		for (int i=0;i<path.Count-1;i++) {
  			//pointCounter += Mathf.FloorToInt ((path[i]-path[i+1]).magnitude / maxSegmentLength)+1;
  			
  			float dist = (path[i]-path[i+1]).magnitude;
  			//In order to avoid floating point errors as much as possible, and in lack of a better solution
  			//loop through it EXACTLY as the other code further down will
  			for (float t=0;t<=dist;t+=maxSegmentLength) {
  				pointCounter++;
  			}
  		}
  		
  		List<Vector3> subdivided = ListPool<Vector3>.Claim (pointCounter);
  		
  		//Set first velocity
  		Vector3 preEndVel = (path[1]-path[0]).normalized;
  		
  		for (int i=0;i<path.Count-1;i++) {
  			//subdivided[counter] = path[i];
  			//counter++;
  			
  			float dist = (path[i]-path[i+1]).magnitude;
  			
  			Vector3 startVel1 = preEndVel;
  			Vector3 endVel1 = i < path.Count-2 ? ((path[i+2]-path[i+1]).normalized - (path[i]-path[i+1]).normalized).normalized : (path[i+1]-path[i]).normalized;
  			
  			Vector3 startVel = startVel1 * dist * factor;
  			Vector3 endVel = endVel1 * dist * factor;
  			
  			
  			               
  			Vector3 start = path[i];
  			Vector3 end = path[i+1];
  			
  			//Vector3 p1 = start + startVel;
  			//Vector3 p2 = end - endVel;
  			           
  			float onedivdist = 1F / dist;
  			
  			for (float t=0;t<=dist;t+=maxSegmentLength) {
  				
  				float t2 = t * onedivdist;
  				
  				subdivided.Add (GetPointOnCubic(start,end,startVel,endVel,t2));
  				//counter++;
  			}
  			
  			preEndVel = endVel1;
  			
  		}
  		
  		subdivided[subdivided.Count-1] = path[path.Count-1];
  		
  		return subdivided;
  	}
  	
  	public static Vector3 GetPointOnCubic (Vector3 a, Vector3 b, Vector3 tan1, Vector3 tan2, float t) {
  		float t2 = t*t, t3 = t2*t;
  		
  		//float s = (float)t / (float)steps;  // scale s to go from 0 to 1
  		float h1 =  2*t3 - 3*t2 + 1;       	  // calculate basis function 1
  		float h2 = -2*t3 + 3*t2;              // calculate basis function 2
  		float h3 =   t3 -  2*t2 + t;       	  // calculate basis function 3
  		float h4 =   t3 -  t2;            	  // calculate basis function 4
  		return		h1*a +                   	 // multiply and sum all funtions
  					h2*b +                   	 // together to build the interpolated
  					h3*tan1 +                	 // point along the curve.
  					h4*tan2;
  		
  		//return //(b*(3*t*t-2*t*t*t)+c*(t-2*t*t+t*t*t)+d*(-t*t+t*t*t)+a*(1-3*t*t+2*t*t*t));
  			
  		//float mt = 1-t;
  		//return start*mt*mt*mt + (start+startVel)*3*t*mt*mt + 3*mt*t*t*(end-endVel) + t*t*t*end;
  		//return start*t*t*t + (start+startVel)*t*t + (end-endVel)*t + end;
  		//return start*t*t*t + start*t*t + startVel*t*t + end*t + endVel*t + end;
  	}
  	
  	public List<Vector3> SmoothOffsetSimple (List<Vector3> path) {
  		
  		if (path.Count <= 2 || iterations <= 0) {
  			return path;
  		}
  		
  		if (iterations > 12) {
  			Debug.LogWarning ("A very high iteration count was passed, won't let this one through");
  			return path;
  		}
  		
  		int maxLength = (path.Count-2)*(int)Mathf.Pow(2,iterations)+2;
  		
  		List<Vector3> subdivided = ListPool<Vector3>.Claim (maxLength);
  		//new Vector3[(path.Length-2)*(int)Mathf.Pow(2,iterations)+2];
  		List<Vector3> subdivided2 = ListPool<Vector3>.Claim (maxLength);
  		//new Vector3[(path.Length-2)*(int)Mathf.Pow(2,iterations)+2];
  		
  		for (int i=0;i<maxLength;i++) { subdivided.Add (Vector3.zero); subdivided2.Add (Vector3.zero); }
  		
  		for (int i=0;i<path.Count;i++) {
  			subdivided[i] = path[i];
  		}
  		
  		for (int iteration=0;iteration < iterations; iteration++) {
  			int currentPathLength = (path.Count-2)*(int)Mathf.Pow(2,iteration)+2;
  			
  			//Switch the arrays
  			List<Vector3> tmp = subdivided;
  			subdivided = subdivided2;
  			subdivided2 = tmp;
  			
  			float nextMultiplier = 1F;
  			
  			for (int i=0;i<currentPathLength-1;i++) {
  				Vector3 current = subdivided2[i];
  				Vector3 next = subdivided2[i+1];
  				
  				Vector3 normal = Vector3.Cross (next-current,Vector3.up);
  				normal = normal.normalized;
  				
  				//This didn't work very well, made the path jaggy
  				/*Vector3 dir = next-current;
  				dir *= strength*0.5F;
  				current += dir;
  
gt: 				next -= dir;*/