using System;
using System.Collections.Generic;
using System.Text;
using RungeKutta.Integrators;

namespace RungeKutta.SimStep
{
    class RungeKutta : SimStepper
    {
        public State Integrate(State oldState, float timeStep, ODEs.ODE ode)
        {
            State k1 = new State(),
                    k2 = new State(),
                    k3 = new State(),
                    k4 = new State();
            
            k1 = oldState;
            k1.acceleration = ode.EvaluateForceAtState(k1);

            k2.position = EulerStep.Integrate(oldState.position, timeStep / 2.0f, k1.velocity);
            k2.velocity = EulerStep.Integrate(oldState.velocity, timeStep / 2.0f, k1.acceleration);
            k2.acceleration = ode.EvaluateForceAtState(k2);

            k3.position = EulerStep.Integrate(oldState.position, timeStep / 2.0f, k2.velocity);
            k3.velocity = EulerStep.Integrate(oldState.velocity, timeStep / 2.0f, k2.acceleration);
            k3.acceleration = ode.EvaluateForceAtState(k3);

            k4.position = EulerStep.Integrate(oldState.position, timeStep, k3.velocity);
            k4.velocity = EulerStep.Integrate(oldState.velocity, timeStep, k3.acceleration);
            k4.acceleration = ode.EvaluateForceAtState(k4);

            State WeightedAverage = k1 + (k2 + k3) * 2 + k4;
            WeightedAverage *= 1.0f / 6.0f;

            State newState = new State();

            //newState = predictor.Integrate(oldState, timeStep, WeightedAverage.acceleration);
            newState.position = EulerStep.Integrate(oldState.position, timeStep, WeightedAverage.velocity);
            newState.velocity = EulerStep.Integrate(oldState.velocity, timeStep, WeightedAverage.acceleration);
            newState.acceleration = WeightedAverage.acceleration;

            return newState;
        }

        public int ForceEvaluations
        {
            get
            {
                return 4;
            }
        }

        public override string ToString()
        {
            return "RK4";
        }
    }
}