using System;

using UnityEngine;

namespace Framework.Support {

  /// <summary>Integrates position and velocity using the Runge Kutta 4 method</summary>
  public struct RungeKutta4 {
    
    #region struct State

    /// <summary>Stores the current state of the system</summary>
    private struct State {
      
      /// <summary>Position of the object</summary>
      public Vector3 Position;
      /// <summary>Velocity the object is moving at</summary>
      public Vector3 Velocity;

    }

    #endregion // struct State

    #region struct Derivative

    /// <summary>Stores the derivative state of the system</summary>
    private struct Derivative {
      
      /// <summary>Velocity of the object</summary>
      public Vector3 Velocity;
      /// <summary>Acceleration the object's velocity is increasing at</summary>
      public Vector3 Acceleration;

    }

    #endregion // struct Derivative

    /// <summary>Performs one Runge Kutta 4 simulation step</summary>
    /// <param name="velocity">Velocity of the object</param>
    /// <param name="acceleration">Acceleration of the object at (t + dt)</param>
    /// <param name="deltaTime">Amount of time that will be simulated</param>
    /// <returns>The translation of the object after the specified delta time</returns>
    public Vector3 Step(ref Vector3 velocity, Vector3 acceleration, float deltaTime) {
      var state = new State();
      state.Position = Vector3.zero;
      state.Velocity = velocity;

      this.acceleration = acceleration;
      integrate(ref state, deltaTime);

      velocity = state.Velocity;
      return state.Position;
    }

    /// <summary>Integrates the state over the specified delta time</summary>
    /// <param name="state">State that will be integrated</param>
    /// <param name="deltaTime">Amount of time that has passed</param>
    private void integrate(ref State state, float deltaTime) {
      Derivative a, b, c, d;

      a = evaluate(state, 0.0f, new Derivative());
      b = evaluate(state, deltaTime * 0.5f, a);
      c = evaluate(state, deltaTime * 0.5f, b);
      d = evaluate(state, deltaTime, c);

      Vector3 velocity = (
        1.0f / 6.0f * (a.Velocity + 2.0f * (b.Velocity + c.Velocity) + d.Velocity)
      ); 
      Vector3 acceleration = (
        1.0f / 6.0f * (a.Acceleration + 2.0f * (b.Acceleration + c.Acceleration) + d.Acceleration)
      );

      state.Position += velocity * deltaTime;
      state.Velocity += acceleration * deltaTime;
    }

    /// <summary>Linearly integrates a single state over the specified time</summary>
    /// <param name="initial">State at the beginning of the simulation</param>
    /// <param name="deltaTime">Amount of time that should be simulated</param>
    /// <param name="derivative">Derivative describing how the state changes</param>
    /// <returns>The state after the specified amount of time has passed</returns>
    private Derivative evaluate(State initial, float deltaTime, Derivative derivative) {
      State state;
      state.Position = initial.Position + derivative.Velocity * deltaTime;
      state.Velocity = initial.Velocity + derivative.Acceleration * deltaTime;

      Derivative output;
      output.Velocity = state.Velocity;
      output.Acceleration = this.acceleration;

      return output;
    }

    
    private Vector3 acceleration;
    
  }
  
} // namespace Framework.Support