/*Siddhartha Kasivajhula
* Georgia Institute of Technology
* c/o Predrag Cvitanovic
* Advisor: Rytis Paskauskas
* Fall 2005
*/

//import cs1.Keyboard;
/*Now x and y velocities are parameters. So momentum p will be known if a mass is assumed*/

//! @brief This class represents the AFM.

/// The AFM is modeled here. AFM motion in the absence of the surface is fully characterized.
/// The trajectory, when the AFM has traversed 1 period of the surface, is stored in arrays of length <AFMConstants.SAMPLES>.
/// The trajectory can be recalculated at any point via methods such as reset(). This needs to be done when there is
/// interaction with the surface. This class AFM knows nothing about the surface. It only knows the dynamics of the AFM.
public class AFM {
    private double[] t;            //!< time, reset to zero at the start of every period
    private double[] x;            //!< x position of AFM at a particular time
    private double[] y;            //!< y position of AFM at a particular time
    private double[] px;           //!< momentum in x-direction
    private double[] py;           //!< momentum in y-direction
    private double[] F_x;          //!< Force in x-direction
    private double[] F_y;          //!< Force in y-direction
    private double timeStep;       //!< time interval between samples
    private double y0;             //!< equilibrium position of vertical AFM motion
    private double V;              //!< shear velocity
    private double e;              //!< epsilon. Frequency ratio, vertical/horizontal
    private double a;              //!< alpha. co-efficient of restitution
    private double[] time;         //!< returns actual simulation running time


    //! Constructor. sets up arrays, initializes parameters. This needs to be cleaned up.
    public AFM()    //Constructor, sets up arrays, initializes parameters
    {
        time = new double[AFMConstants.SAMPLES];
        t = new double[AFMConstants.SAMPLES];
        x = new double[AFMConstants.SAMPLES];
        y = new double[AFMConstants.SAMPLES];
        px = new double[AFMConstants.SAMPLES];
        py = new double[AFMConstants.SAMPLES];
        F_x = new double[AFMConstants.SAMPLES];
        F_y = new double[AFMConstants.SAMPLES];
        y0 = 0;
        V = 0.025836841411021969;
        e = 0.0791855;
        a = 0.9;
        t[0] = 0;
        x[0] = -0.016280304943651295;
        y[0] = 0.031850050627578848;
        px[0] = -0.0089501733818892516;
        py[0] = 0.065677502933238513;
        F_x[0] = 0;     // must change later
        F_y[0] = 0;     // must change later
    }//end constructor

    //! initializes AFM motion
    public void reset(double totalTime) {
        t[0] = 0;
        timeStep = totalTime / (AFMConstants.SAMPLES - 1);
        for (int i = 1; i < AFMConstants.SAMPLES; i++) {
            time[i] = time[i - 1] + timeStep;
            t[i] = t[i - 1] + timeStep;
            px[i] = px[0] * Math.cos(t[i] - t[0]) - (x[0] - V * t[0]) * Math.sin(t[i] - t[0]);
            py[i] = (py[0] * Math.cos(e * (t[i] - t[0])) - e * (y[0] - y0) * Math.sin(e * (t[i] - t[0])));
            x[i] = (x[0] - V * t[0]) * Math.cos(t[i] - t[0]) + px[0] * Math.sin(t[i] - t[0]) + V * t[i];
            y[i] = (y[0] - y0) * Math.cos(e * (t[i] - t[0])) + py[0] / e * Math.sin(e * (t[i] - t[0])) + y0;
            F_x[i] = - px[0] * Math.sin(t[i] - t[0]) - (x[0] - V*t[0]) * Math.cos(t[i] - t[0]);
            F_y[i] = - e * py[0] * Math.sin(e * (t[i] - t[0])) - e*e * (y[0] - y0) * Math.cos(e * (t[i] - t[0]));
        }
    }//end reset(totalTime)

    //! Re-evaluates AFM equations of motion and generates new trajectory, given new initial conditions (this is called when there is a surface interaction)
    public void reset(int i0, double nx, double ny, double pxI, double pyI) {
        /*After each bounce, AFM equations of motion are updated here*/
        px[i0] = (1 - (1 + a) * nx * nx) * (pxI + V) - pyI * (1 + a) * nx * ny - V;            // x, y remain the same for point of intersection
        py[i0] = ((-(1 + a) * nx * ny) * (pxI + V) + (1 - (1 + a) * ny * ny) * pyI);        // px, py iterated
        for (int i = i0 + 1; i < AFMConstants.SAMPLES; i++)        // loop from (point of intersection + 1) till end, updating
        {                                                            // all values
            px[i] = px[i0] * Math.cos(t[i] - t[i0]) - (x[i0] - V * t[i0]) * Math.sin(t[i] - t[i0]);        // V*t0 is x_cm
            py[i] = (py[i0] * Math.cos(e * (t[i] - t[i0])) - e * (y[i0] - y0) * Math.sin(e * (t[i] - t[i0])));

            x[i] = (x[i0] - V * t[i0]) * Math.cos(t[i] - t[i0]) + px[i0] * Math.sin(t[i] - t[i0]) + V * (t[i]);
            y[i] = (y[i0] - y0) * Math.cos(e * (t[i] - t[i0])) + py[i0] / e * Math.sin(e * (t[i] - t[i0])) + y0;

            F_x[i] = - px[i0] * Math.sin(t[i] - t[i0]) - (x[i0] - V*t[i0]) * Math.cos(t[i] - t[i0]);
            F_y[i] = - e * py[i0] * Math.sin(e * (t[i] - t[i0])) - e*e * (y[i0] - y0) * Math.cos(e * (t[i] - t[i0]));

        }
    }//end reset(params..)


    /***** Getters *****/

    //! returns the timestep -- interval between samples
    public double getTimeStep() {
        return timeStep;
    }

    //! returns time at the given index (time for this traversal; for simulation running time, see getTime() )
    public double getT(int index)        // returns time within traversal at index
    {
        return t[index];
    }

    //! returns simulation running time
    public double getTime(int index)    // returns actual simulation running time
    {
        return time[index];
    }

    //! returns coefficient of restitution. This should be a surface parameter. What's it doing here? Maybe put it in Surface.. or AFMSimulator
    public double getRestit() {
        return a;
    }

    //! returns y0, the vertical equilibrium position
    public double getEquiPos() {
            return y0;
        }

    //! returns the shear velocity
    public double getV() {
        return V;
    }

    //! returns the frequency ratio, epsilon
    public double getE() {
        return e;
    }

    //! returns x-position at the given index
    public double getX(int index) {
        return x[index];
    }

    //! returns y-position at the given index
    public double getY(int index) {
        return y[index];
    }

    //! returns momentum in the x-direction at the given index
    public double getPX(int index) {
        return px[index];
    }

    //! returns momentum in the y-direction at the given index
    public double getPY(int index) {
        return py[index];
    }

    //! returns Force in the x-direction at the given index
    public double getFX(int index) {
        return F_x[index];
    }

    //! returns Force in the y-direction at the given index
    public double getFY(int index) {
        return F_y[index];
    }


    /***** Setters *****/

    //! sets the time at the given index to the value provided (time for this traversal, for simulation running time, see setTime() )
    public void setT(int i, double tNew)    // sets time value for given index
     {
         t[i] = tNew;
     }

    //! sets the time at the given index to the value provided (simulation running time)
    public void setTime(int i, double tNew)
     {
         time[i] = tNew;
     }

    //! sets the coefficient of restitution. Again, a better place for this might be Surface
    public void setRestit(double a) {
        if (a > -41) this.a = a;
    }

    //! sets the vertical equilibrium position of the AFM. "Raises or lowers the AFM apparatus"
    public void setEquiPos(double y0) {
        if (y0 > -41) this.y0 = y0;
    }

    //! sets the shear velocity
    public void setV(double V) {
        this.V = V;
    }

    //! sets the frequency ratio, epsilon
    public void setE(double e) {
        this.e = e;
    }

    //! sets the x-position at the given index
    public void setX(int index, double xNew) {
        if (xNew > -41) x[index] = xNew;        // if the new value is "-42", it does not change the default values
    }

    //! sets the y-position at the given index
    public void setY(int index, double yNew) {
        if (yNew > -41) y[index] = yNew;
    }

    //! sets the momentum in the x-direction at the given index
    public void setPX(int index, double px) {
        if (px > -41) this.px[index] = px;
    }

    //! sets the momentum in the y-direction at the given index
    public void setPY(int index, double py) {
        if (py > -41) this.py[index] = py;
    }


    /****** Get entire arrays *******/

    //! returns entire time array (time for this traversal; for simulation running time, see getTime() )
    public double[] getT()        // returns entire time array
    {
        return t;
    }

    //! returns entire x array
    public double[] getX()        // returns entire x array
    {
        return x;
    }

    //! returns entire y array
    public double[] getY()        // returns entire y array
    {
        return y;
    }

    //! returns entire x-momentum array
    public double[] getPX()        // returns entire px array
    {
        return px;
    }

    //! returns entire y-momentum array
    public double[] getPY()        // returns entire py array
    {
        return py;
    }

    //! returns entire Fx array (force in x-direction)
    public double[] getFX()        // returns entire Fx array
    {
        return F_x;
    }

    //! returns entire Fy array (force in y-direction)
    public double[] getFY()        // returns entire Fy array
    {
        return F_y;
    }


    /***** Calculate and return values at given precise time *****/

    //! calculates and returns x at any given time
    public double gimmeX(double t1, double t_i, double x_i, double y_i, double px_i, double py_i)    // "gimme" methods return precise values at some precise time
    {                                                    // i0 is point of last intersection
        return ((x_i - V * t_i) * Math.cos(t1 - t_i) + px_i * Math.sin(t1 - t_i) + V * t1);
    }

    //! calculates and returns y at any given time
    public double gimmeY(double t1, double t_i, double x_i, double y_i, double px_i, double py_i) {
        return ((y_i - y0) * Math.cos(e * (t1 - t_i)) + py_i / e * Math.sin(e * (t1 - t_i)) + y0);
    }

    //! calculates and returns px at any given time
    public double gimmePX(double t1, double t_i, double x_i, double y_i, double px_i, double py_i) {
        return (px_i * Math.cos(t1 - t_i) - (x_i - V * t_i) * Math.sin(t1 - t_i));
    }

    //! calculates and returns py at any given time
    public double gimmePY(double t1, double t_i, double x_i, double y_i, double px_i, double py_i) {
        return ((py_i * Math.cos(e * (t1 - t_i)) - e * (y_i - y0) * Math.sin(e * (t1 - t_i))));
    }

    //! calculates and returns Fx at any given time
    public double gimmeFX(double t1, double t_i, double x_i, double y_i, double px_i, double py_i) {
        return (- px_i * Math.sin(t1 - t_i) - (x_i - V*t_i) * Math.cos(t1 - t_i));
    }

    //! calculates and returns Fy at any given time
    public double gimmeFY(double t1, double t_i, double x_i, double y_i, double px_i, double py_i) {
        return (- e * py_i * Math.sin(e * (t1 - t_i)) - e*e * (y_i - y0) * Math.cos(e * (t1 - t_i)));
    }

    //! For testing purposes only.
    public static void main(String[] args)        // for testing
    {
        AFM afm = new AFM();
        afm.reset(1 / afm.getV());
        for (int i = 0; i < AFMConstants.SAMPLES; i += 20) {
            System.out.println(afm.getT(i) + "\t" + afm.getX(i) + "\t" + afm.getY(i));
        }
        System.out.println(afm.getT(999) + "\t" + afm.getX(999) + "\t" + afm.getY(999));
        afm.setT(0, afm.getT(AFMConstants.SAMPLES - 1));
        afm.setX(0, afm.getX(AFMConstants.SAMPLES - 1));    // x = x-L
        afm.setY(0, afm.getY(AFMConstants.SAMPLES - 1));                // y = y
        afm.setPX(0, afm.getPX(AFMConstants.SAMPLES - 1));                // px = px
        afm.setPY(0, afm.getPY(AFMConstants.SAMPLES - 1));                // py = py

        afm.reset(1 / afm.getV());
        for (int i = 0; i < AFMConstants.SAMPLES; i += 20) {
            System.out.println(afm.getT(i) + "\t" + afm.getX(i) + "\t" + afm.getY(i));
        }

        System.out.println(afm.getT(999) + "\t" + afm.getX(999) + "\t" + afm.getY(999));
        afm.setX(0, afm.getX(AFMConstants.SAMPLES - 1) - 2 * Surface.L);    // x = x-L
        afm.setY(0, afm.getY(AFMConstants.SAMPLES - 1));                // y = y
        System.out.println((afm.getT(999) - afm.getT(0)) + "\t" + afm.getX(0) + "\t" + afm.getY(0));
        //System.out.println(afm.getTimeStep());
    }
}//end AFM

/*
CALCULATIONS:

Normal vector at any point should be of unit length.
    Therefore, nx^2 + ny^2 = 1
    Consequently, given the slope (dy/dx) of the line: 'm',
    We get:	ty = m*tx (eqn of line)
                and tx = sqrt(1/(m^2-1))
    The positive velocity vector v+ = Ua v-,
        where Ua =  [1 0] - (1+a) [nx^2	nxny], a = Coefficient of Restitution
                        [0 1]			  [nxny  ny^2]
*/