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

package cubicsplines;

import org.newdawn.slick.Color;
import org.newdawn.slick.Graphics;

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

    private double A,B,C,D,E,F,G,H;
    private float x0,y0,x1,y1,x2,y2,x3,y3;
    private float totalDelta;

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

    public void addNewPosition(
            float fromPosX,
            float fromPosY,
            float toPosX,
            float toPosY,
            float fromVelX,
            float fromVelY,
            float toVelX,
            float toVelY)
    {
        float distanceBetweenPoints = fromPosX - toPosX;
        float controlPointLength = distanceBetweenPoints / 3;

        // Coordinate 1 = starting position
        x0 = fromPosX;
        y0 = fromPosY;

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

        // Coordinate 3 - Position after 1 second using reversed ending velocity = Coordinate4 – EndVelocity
        x2 = toPosX - toVelX * controlPointLength;
        y2 = toPosY - toVelY * controlPointLength;

        // Coordinate 4 = Ending position
        x3 = toPosX;
        y3 = toPosY;

        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("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);
        }

        totalDelta = 0;
    }

    public void follow(int delta)
    {
        totalDelta += delta;

        float t = totalDelta / 500; // 500 ms before next recieved packet

        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);
            }
        }
        else
            totalDelta = 0;
    }


    public void draw(Graphics g)
    {
        g.setColor(Color.red);
        g.drawRoundRect(x0,y0, 15, 15, 10);
        g.setColor(Color.green);
        g.drawRoundRect(x1,y1, 15, 15, 10);
        g.drawRoundRect(x2,y2, 15, 15, 10);
        g.setColor(Color.white);
        g.drawRoundRect(x3,y3, 15, 15, 10);


        g.drawString("p0" + "x: " + x0 + " y: " + y0, 10, 50);
        g.drawString("p1" + "x: " + x1 + " y: " + y1, 10, 70);
        g.drawString("p2" + "x: " + x2 + " y: " + y2, 10, 90);
        g.drawString("p3" + "x: " + x3+ " y: " + y3, 10, 110);

    }
}