/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package cubicsplines;

/**
 *
 * @author Adam
 */
public class Cubic {


    public boolean cordsDebug, lerpDebug;
    double FREQ = 0.05f;


    private double A,B,C,D,E,F,G,H;
    private float lastPositionTime;


    // Interpolated values along the spline
    public float xPos,yPos;

    public void addNewPosition(
            float fromPosX,
            float fromPosY,
            float toPosX,
            float toPosY,
            float fromVelX,
            float fromVelY,
            float toVelX,
            float toVelY)
    {
        float currTime =  (float) CubicSpline.getTime();
        float timeDiff = currTime - lastPositionTime;

        // Calculate change in acceleration
        float accelerationX = toVelX - fromVelX;
        float accelerationY = toVelX - fromVelY;
        
        // Coordinate 1 = starting position
        float x0 = fromPosX;
        float y0 = fromPosY;

        // Coordinate 2 - Position after 1 second using starting velocity = Coordinate1 + StartVelocity
        float x1 = fromPosX + fromVelX;
        float y1 = fromPosY + fromVelY;

        // Coordinate 3 - Position after 1 second using reversed ending velocity = Coordinate4 – EndVelocity
        float x2 = (float) ((toPosX + toVelX * timeDiff) + (0.9 * accelerationX * (timeDiff * timeDiff)));
        float y2 = (float) ((toPosY + toVelY * timeDiff) + (0.9 * accelerationY * (timeDiff * timeDiff)));


        // Coordinate 4 = Ending position
        float x3 = x2 - (toVelX + accelerationX * timeDiff);
        float y3 = y2 - (toVelY + accelerationX * timeDiff);

 
        A = x3 - (3*x2) + (3*x1) - x0;
        B = (3*x2) - (6*x1) + (3*x0);
        C = (3*x1) - (3*x0);
        D = x0;

        E = y3 - (3*y2) + (3*y1) - y0;
        F = (3*y2) - (6*y1) + (3*y0);
        G = (3*y1) - (3*y0);
        H = y0;


        if(cordsDebug)
        {
            System.out.println("----------------------------");
            System.out.println("currTime: " + currTime);
            System.out.println("From Velocity" + fromVelX + " | " + fromVelY);
            System.out.println("To Velocity" + toVelX + " | " + toVelY);
            System.out.println("Coordinate 1: " + x0 + " | " + y0);
            System.out.println("Coordinate 2: " + x1 + " | " + y1);
            System.out.println("Coordinate 3: " + x2 + " | " + y2);
            System.out.println("Coordinate 4: " + x3 + " | " + y3);
        }

        lastPositionTime = currTime;

    }

    // This function is called outside this class on each game tick


    public void follow()
    {
        double curTime = CubicSpline.getTime();
		//Time since last update
	double timeSince = curTime - lastPositionTime;

	double t = timeSince / FREQ;

        if  (t >= 0.0f && t <= 1.0f)
        {
            xPos = (float) (A*Math.pow(t,3) + B*Math.pow(t,2) + C*t + D);
            yPos = (float) (E*Math.pow(t,3) + F*Math.pow(t,2) + G*t + H);

            if(lerpDebug)
            {
                System.out.println("Pos: " + xPos + " | " + yPos);
            }
        }
    }
}