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

//import cs1.Keyboard;

/* Revisions: 1. Substituted Math.round() for simple int.*/

/*Fixes: (12/01/2005) The time now resets after each traversal. This should correct precision error.
                      To implement this change, added t=0; in reset(totalTime) in class AFM
                      In getMore(), changed afm.setX(0, SAMPS-1) to afm.setX(0, SAMPS-1)-Surface.L
                      Finally, reset t0, x0, y0, px0, py0 (reference values) at the beginning of each traversal.
*/
public class AFMSimulator {
    /**
     * Surface and AFM that will be simulated by this class
     */
    private Surface surface;
    private AFM afm;
    private final double SURF_TOLERANCE = 1.e-11;
    static final int STROBE_OFFSET = 0;//AFMConstants.SAMPLES/4;
    private double totalTime;
    public static final double default_a = 0.8,            //0.9 for periodic
            default_t = 0,//9.685791164220795,
            default_x = 0.018038552741981575050571488355,//0.2682888029922321,//-0.016280304943651295,
            default_y = 0.5985119378367934,//0.031850050627578848,
            default_px = 0.004445733640121142,//-0.0089501733818892516,
            default_py = 0.045538494373958266,//0.065677502933238513,
            default_y0 = 0,
            default_V = 0.025836841411021969;

    public static final double periodic_a = 0.9,
            periodic_t = 0,//9.685791164220795,
            periodic_x = 0.018038552741981575050571488355,//0.2682888029922321,//-0.016280304943651295,
            periodic_y = 0.5985119378367934,//0.031850050627578848,
            periodic_px = 0.004445733640121142,//-0.0089501733818892516,
            periodic_py = 0.045538494373958266,//0.065677502933238513,
            periodic_y0 = 0,
            periodic_V = 0.025836841411021969;

    /**
     * Constructor. Sets up the Simulator engine.
     */
    public AFMSimulator() {
        surface = new Surface();
        afm = new AFM();
    }//end constructor

    public void resetSim() {
        double err, to, tn, yo, yn, t;
        if (afm.getY(0) < surface.gimmeY(afm.getX(0)))
            afm.setY(0, surface.gimmeY(afm.getX(0)));

        double t0 = afm.getT(0);
        double x0 = afm.getX(0);
        double y0 = afm.getY(0);
        double px0 = afm.getPX(0);
        double py0 = afm.getPY(0);

        totalTime = Surface.L / afm.getV();
        afm.reset(totalTime);    // passes total time to AFM
        for (int i = 0; i < AFMConstants.SAMPLES; i++) {
            int hmm = 0;
            if (afm.getY(i) < surface.gimmeY(afm.getX(i))) {
                // implementing bisection (numerical newton) method to find surface and trajectory intersection.

                /*for(int j=0;j<i;j+=20)
                        System.out.println(getAFM().getT(j) + "\t" + afm.getX(j) + "\t" + getAFM().getY(j)
                        + "\t" + getAFM().getPX(j) + "\t" + getAFM().getPY(j));

                    System.exit(0);*/
                if (i > 0)
                    to = afm.getT(i - 1);
                else {
                    to = afm.getT(i) - afm.getTimeStep();
                }//Warning: not on surface
                tn = afm.getT(i);
                yo = afm.gimmeY(to, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(to, t0, x0, y0, px0, py0));
                yn = afm.gimmeY(tn, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(tn, t0, x0, y0, px0, py0));
                do {
                    t = -1 * (tn * yo - to * yn) / (yn - yo);        // now that to can be negative, is this affected?
                    err = afm.gimmeY(t, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(t, t0, x0, y0, px0, py0));
                    if (err * yn > 0) {
                        yn = err;
                        tn = t;
                    } else {
                        yo = err;
                        to = t;
                    }
                    //System.out.println("bisection error =  " + err + "\n");
                    hmm++;
                    if (hmm > 20) {
                        System.out.println("An infinite amount of work");
                        break;
                    }
                }
                while (Math.abs(err) > SURF_TOLERANCE);
                // set the arrays from this point (t).
                //System.out.println("Done");
                afm.setY(i, surface.gimmeY(afm.gimmeX(t, t0, x0, y0, px0, py0)));        // AFM is moved to point of intersection, i.e. the
                afm.setX(i, afm.gimmeX(t, t0, x0, y0, px0, py0));                            // current index now contains the "true" point of
                afm.setT(i, t);                                                // intersection, and the AFM is on the surface at that point
                /* Even the time at this index is changed to the time of "true" intersection, so that the motion equations
                    /* in AFM.java have the correct values for "t[i]-t[i0]"
                    */
                afm.reset(i, surface.getNX(afm.getX(i)), surface.getNY(afm.getX(i)), afm.gimmePX(t, t0, x0, y0, px0, py0), afm.gimmePY(t, t0, x0, y0, px0, py0));
                /* the AFM motion equations are iterated from this point with the precise values for
                    /* "incoming" x, y, px, py
                    */
                /* This now becomes the "last"(previous) point of intersection.*/
                t0 = afm.getT(i);
                x0 = afm.getX(i);
                y0 = afm.getY(i);
                px0 = afm.getPX(i);
                py0 = afm.getPY(i);
            }//end if(below surface)
        }// end for
        //System.out.println("Shifted params are : t0 = " + t0 + " x0 = " + x0 + " y0 = " + y0 + " px0 = " + px0 + " py0 = " + py0);

//End Shifted Trajectory
    }// end resetSim()

    public void getMore()    // this method is called by the graphics applet. It generates (1000) new values
    {                            // for the AFM, i.e. another single traversal of the surface.
        double err, to, tn, yo, yn, t;
        afm.setX(0, afm.getX(AFMConstants.SAMPLES - 1)-Surface.L);    // 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.setT(0, afm.getT(AFMConstants.SAMPLES - 1));
        afm.setTime(0, afm.getTime(AFMConstants.SAMPLES - 1));
        afm.reset(totalTime);    // passes total time to AFM
        double t0 = afm.getT(0);
        double x0 = afm.getX(0);
        double y0 = afm.getY(0);
        double px0 = afm.getPX(0);
        double py0 = afm.getPY(0);
        for (int i = 0; i < AFMConstants.SAMPLES; i++) {
            int hmm = 0;
            if (afm.getY(i) < surface.gimmeY(afm.getX(i)))        //** CHANGE TO GIMMEY.  meaningless to use getY when gimmey is available.
            {    // bisection again
                if (i > 0)
                    to = afm.getT(i - 1);
                else
                    to = afm.getT(i) - afm.getTimeStep();
                tn = afm.getT(i);
                yo = afm.gimmeY(to, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(to, t0, x0, y0, px0, py0));
                yn = afm.gimmeY(tn, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(tn, t0, x0, y0, px0, py0));
                do {
                    if (to == tn) {
                        t = to;
                        break;
                    }
                    t = -1 * (tn * yo - to * yn) / (yn - yo);
                    //System.out.println(afm.gimmeX(t, t0, x0, y0, px0, py0));
                    err = afm.gimmeY(t, t0, x0, y0, px0, py0) - surface.gimmeY(afm.gimmeX(t, t0, x0, y0, px0, py0)); // y(t) - surfaceY(x(t)) maybe some parameters too???
                    if (err * yn > 0) {
                        yn = err;
                        tn = t;
                    } else {
                        yo = err;
                        to = t;
                    }
/*					System.out.println("bisection error =  " + err + "\n");*/
                    hmm++;
                    if (hmm > 20) {
                        System.out.println("An infinite amount of work");
                        break;
                    }
                }
                while (Math.abs(err) > SURF_TOLERANCE);
                // set the arrays from this point (t).
                afm.setY(i, surface.gimmeY(afm.gimmeX(t, t0, x0, y0, px0, py0)));    // bring AFM to surface at intersection point
                afm.setX(i, afm.gimmeX(t, t0, x0, y0, px0, py0));    // store precise x position in current array index
                afm.setT(i, t);                        // store precise time of intersection in current index
                afm.reset(i, surface.getNX(afm.getX(i)), surface.getNY(afm.getX(i)), afm.gimmePX(t, t0, x0, y0, px0, py0), afm.gimmePY(t, t0, x0, y0, px0, py0));
                /*iterate AFM equations after point of intersection*/
                /* This now becomes the "last"(previous) point of intersection.*/
                t0 = afm.getT(i);
                x0 = afm.getX(i);
                y0 = afm.getY(i);
                px0 = afm.getPX(i);
                py0 = afm.getPY(i);
            }//end if(below surface)
        }// end for
    }// end getMore()

    public void setParams(double t, double a, double x, double y, double px, double py, double equiPos, double V) {    /*receives parameter values from GUI and forwards them to afm*/
        afm.setT(0, t);
        afm.setTime(0, t);
        afm.setRestit(a);        //
        afm.setX(0, x);            // set all
        afm.setY(0, y);            // parameter
        afm.setPX(0, px);        // values
        afm.setPY(0, py);        //
        afm.setEquiPos(equiPos);
        afm.setV(V);
    }

    public AFM getAFM()    // returns AFM object
    {
        return afm;
    }

    public Surface getSurface()    //returns Surface object
    {
        return surface;
    }

    public boolean isOnSurface(double x, double y) {
        return Math.abs(surface.gimmeY(x) - y) <= 0.01;
    }

    public double getTotalTime() {
        return totalTime;
    }

    /*public AFMSimulator clone()
         {
         }*/
    public static void main(String[] args)    // for testing purposes
    {
        AFMSimulator afmSim = new AFMSimulator();
        double a = AFMSimulator.periodic_a;
        double x = AFMSimulator.periodic_x;
        double y = AFMSimulator.periodic_y;
        double px = AFMSimulator.periodic_px;
        double py = AFMSimulator.periodic_py;
        double y0 = AFMSimulator.periodic_y0;
        double V = AFMSimulator.periodic_V;
    	  afmSim.setParams(0, a, x, y, px, py, y0, V);
		  afmSim.resetSim();
    	  int iters = 5;
        //double a = 0.9;
        double data[][] = new double[5][4];
        double good[][] = new double[100][4];
        int goodCounter = 0;
        boolean isGood = true;
        /* Parameters */
/*
		double x_m = -0.016280304943651295;
		double y_m = -0.031850050627578848;
		double px_m = -0.0089501733818892516;
		double py_m = 0.065677502933238513;
		
		for(double iX=x_m-0.04; iX <= x_m + 0.04 + 1.e-13; iX += 0.0000005)
			{
			for(double iY=y_m-0.04; iY <= y_m + 0.04 + 1.e-13; iY += 0.0000005)
				{
				if(afmSim.isOnSurface(iX,iY))
					{
					for(double iPX=px_m-0.04; iPX <= px_m + 0.04 + 1.e-13; iPX += 0.0000005)
						{
						for(double iPY=py_m-0.04; iPY <= py_m + 0.04 + 1.e-13; iPY += 0.0000005)
							{
							System.out.println("hey");
							afmSim.setParams(a,iX,iY,iPX,iPY);	// set AFM initial parameters
							afmSim.resetSim();					// begin the simulation
							//System.out.println("x = " + iX + " y = " + iY + " px = " + iPX + " py = " + iPY);
							isGood=true;
							for(int i=1; i<iters; i++)
								{
								afmSim.getMore();
								data[i][0] = afmSim.getAFM().getX(AFMConstants.SAMPLES-1);
								data[i][1] = afmSim.getAFM().getY(AFMConstants.SAMPLES-1);
								data[i][2] = afmSim.getAFM().getPX(AFMConstants.SAMPLES-1);
								data[i][3] = afmSim.getAFM().getPY(AFMConstants.SAMPLES-1);
								}
							
							int i=iters-1;
							for(int j=1; j<i; j++)
								{
								System.out.println(data[j][0] + " " + data[i][0]);
								Keyboard.readInt();
								if(Math.abs(data[j][0]-data[i][0]) > 5) {isGood=false; break;}
								if(Math.abs(data[j][1]-data[i][1]) > 5) {isGood=false; break;}
								if(Math.abs(data[j][2]-data[i][2]) > 5) {isGood=false; break;}
								if(Math.abs(data[j][3]-data[i][3]) > 5) {isGood=false; break;}
								//System.out.println("x = " + iX + " y = " + iY + " px = " + iPX + " py = " + iPY);
								}
							if(isGood)
								{
								good[goodCounter][0] = iX;
								good[goodCounter][1] = iY;
								good[goodCounter][2] = iPX;
								good[goodCounter][3] = iX;
								goodCounter++;
								if(goodCounter>99) { System.out.println("Damn..."); System.exit(0);}
								}//end if(isGood)
							}//end inner loop1
						}//end inner loop2
					}//end if on surface
				//System.out.println("iY = " + iY + " good = " + goodCounter);				
				}//end inner loop3
			System.out.println("iX = " + iX + " good = " + goodCounter);
			}// end for outer

		for(int i=0; i<goodCounter; i++)
			{
			System.out.println("x = " + good[i][0] + " y = " + good[i][1] + " px = " + good[i][2] + " py = " + good[i][3]);
			}
		*/
        /*		for(int i=0; i<iters; i++)					// plots AFM points
			{
			double tempX = afmSim.getAFM().getX(AFMConstants.SAMPLES-1);
			double tempY = afmSim.getAFM().getY(AFMConstants.SAMPLES-1);
			System.out.println(tempX + " " + tempY);
			afmSim.getMore();
			}// end for*/

        for (int i = 0; i < 1000; i += 20) {
            System.out.println(afmSim.getAFM().getT(i) + "\t" + afmSim.getAFM().getX(i) + "\t" + afmSim.getAFM().getY(i)
                    + "\t" + afmSim.getAFM().getPX(i) + "\t" + afmSim.getAFM().getPY(i) + "\t" + afmSim.getAFM().getFX(i)
                    + "\t" + afmSim.getAFM().getFY(i));
        }
        System.out.println(afmSim.getAFM().getT(999) + "\t" + afmSim.getAFM().getX(999) + "\t" + afmSim.getAFM().getY(999)
                + "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999) + "\t" + afmSim.getAFM().getFX(999)
                + "\t" + afmSim.getAFM().getFY(999));

        afmSim.getMore();
        for (int i = 0; i < 1000; i += 20) {
            System.out.println(afmSim.getAFM().getT(i) + "\t" + afmSim.getAFM().getX(i) + "\t" + afmSim.getAFM().getY(i)
                    + "\t" + afmSim.getAFM().getPX(i) + "\t" + afmSim.getAFM().getPY(i) + "\t" + afmSim.getAFM().getFX(i)
                    + "\t" + afmSim.getAFM().getFY(i));
        }
        System.out.println(afmSim.getAFM().getT(999) + "\t" + afmSim.getAFM().getX(999) + "\t" + afmSim.getAFM().getY(999)
                + "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999) + "\t" + afmSim.getAFM().getFX(999)
                + "\t" + afmSim.getAFM().getFY(999));

/*		afmSim.getMore();
		for(int i=0; i<1000; i+=20)
			{
			System.out.println(afmSim.getAFM().getT(i) + "\t" + AFMMath.mod(afmSim.getAFM().getX(i),Surface.L) + "\t" + afmSim.getAFM().getY(i)
			+ "\t" + afmSim.getAFM().getPX(i) + "\t" + afmSim.getAFM().getPY(i));
			}
		System.out.println(afmSim.getAFM().getT(999) + "\t" + AFMMath.mod(afmSim.getAFM().getX(999),Surface.L) + "\t" + afmSim.getAFM().getY(999)
		+ "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999));
		//System.out.println(afmSim.getAFM().getT(999) + "\t" + (AFMMath.mod(afmSim.getAFM().getX(999),Surface.L)-1) + "\t" + afmSim.getAFM().getY(999)
		//+ "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999));
		afmSim.getMore();
		for(int i=0; i<1000; i+=20)
			{
			System.out.println(afmSim.getAFM().getT(i) + "\t" + AFMMath.mod(afmSim.getAFM().getX(i),Surface.L) + "\t" + afmSim.getAFM().getY(i)
			+ "\t" + afmSim.getAFM().getPX(i) + "\t" + afmSim.getAFM().getPY(i));
			}
		System.out.println(afmSim.getAFM().getT(999) + "\t" + AFMMath.mod(afmSim.getAFM().getX(999),Surface.L) + "\t" + afmSim.getAFM().getY(999)
		+ "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999));

/*
		for(int i=0; i<2; i++)
			afmSim.getMore();
		System.out.println(afmSim.getAFM().getT(999) + "\t" + (afmSim.getAFM().getX(999)+1) + "\t" + afmSim.getAFM().getY(999)
		+ "\t" + afmSim.getAFM().getPX(999) + "\t" + afmSim.getAFM().getPY(999));
/*		for(int i=0; i<1000; i+=20)
			{
			System.out.println(afmSim.getAFM().getT(i) + "\t" + (afmSim.getAFM().getX(i)+1) + "\t" + afmSim.getAFM().getY(i));
			}
		System.out.println(afmSim.getAFM().getT(999) + "\t" + (afmSim.getAFM().getX(999)+1) + "\t" + afmSim.getAFM().getY(999));
		/*for(int i=0; i<20; i++)
			{
			afmSim.getMore();
			}*/

/*		afmSim.getAFM().reset(Surface.L/afmSim.getAFM().getV(), true);

        afmSim.getAFM().setX(0,afmSim.getAFM().getX(AFMConstants.SAMPLES-1)-Surface.L);	// x = x-L
        afmSim.getAFM().setY(0,afmSim.getAFM().getY(AFMConstants.SAMPLES-1));				// y = y
        afmSim.getAFM().setPX(0,afmSim.getAFM().getPX(AFMConstants.SAMPLES-1));				// px = px
        afmSim.getAFM().setPY(0,afmSim.getAFM().getPY(AFMConstants.SAMPLES-1));				// py = py
        afmSim.getAFM().setT(0,afmSim.getAFM().getT(AFMConstants.SAMPLES-1));
        afmSim.getAFM().reset(Surface.L/afmSim.getAFM().getV(), false);	// passes total time to AFM
*/
/*		for(int i=0; i<AFMConstants.SAMPLES; i+=100)
			{
			System.out.println(afmSim.getAFM().getX(i) + "\t" + afmSim.getSurface().getX(i));
			}
		System.out.println(afmSim.getAFM().getX(AFMConstants.SAMPLES-1));
		for(int k=0; k<10; k++)
			{
			
		afmSim.getMore();
		for(int i=0; i<AFMConstants.SAMPLES; i+=100)
			{
			System.out.println(afmSim.getAFM().getX(i) + "\t" + afmSim.getSurface().getX(i));
			}
		}
		afmSim.getMore();
		for(int i=0; i<AFMConstants.SAMPLES; i+=100)
			{
			System.out.println(afmSim.getAFM().getX(i) + "\t" + afmSim.getSurface().getX(i));
			}

		/*for(int i=0; i<100; i++)
			{
			System.out.println(afmSim.getAFM().getPX(i));// + "\tS:" + afmSim.getSurface().getY(i) + "\tA:" + afmSim.getAFM().getY(i));
			}*/
    }//end main()
}//end AFMSimulator