using namespace std;

#include "main.h"
#include "extern__inline_vect.h"


///////////////////////////////////////////////////////////


//
// Real System Parameters
//
// vertical tip frequency:              40 kHz
// normal spring rest point:            -0.05 nm
// longitudinal tip frequency:          250 kHz
// arm velocity:                        [ 1 nm/s, 50 microm/s ]
// tip mass:                            408 ng
// mica lattice spacing:                0.52 nm
// surface amplitude:                   0.05 nm
//


const double dRealVertTipOmega = 40e3 ;      // rad / s
const double dRealLongTipOmega = 250e3 ;     // rad / s

const double dRealRestPoint = -0.05e-9;          // m

const double dRealTipMass = 408e-12;            // kg
const double dRealLatticeSpacing = 0.52e-9;     // m
const double dRealSurfaceAmplitude = 1*0.05e-9;   // m

const double dRealV0BASIS = dRealLatticeSpacing*(dRealVertTipOmega/(2.0*pi));             // m/s
//const double dRealV0Min =  3.25  * dRealV0BASIS;         // m/s
const double dRealV0Min =  3.4  * dRealV0BASIS;         // m/s
const double dRealV0Max =  10 * dRealV0BASIS;         // m/s




////////////////////////////////////////
// dRealV0Min =  3.4  * dRealV0BASIS;  ->  two cycle VANISHES
// dRealV0Min =  3.25  * dRealV0BASIS;  ->  two cycle present!
////////////////////////////////////////




////////////////////
// SYSTEM SCALING //
////////////////////

// length scale
const double dLengthScale = dRealLatticeSpacing;

// time scale
const double dTimeScale = 1.0/dRealLongTipOmega;

// velocity scale
const double dVelocityScale = dLengthScale / dTimeScale;

// mass scale (tip mass is the most natural unit for mass)
const double dMassScale = dRealTipMass;

// energy scale
const double dEnergyScale = dMassScale * dVelocityScale * dVelocityScale;


////////////////
// PARAMETERS //
////////////////

//
// frequencies of each of the motions
//
const double dOmegaH = dRealLongTipOmega / (1.0/dTimeScale);       // longitudinal
const double dOmegaV = dRealVertTipOmega / (1.0/dTimeScale);       // vertical


//
// constant motion of the tip's potential minimum ( will vary in program most likely)
//
static double dV0 = dRealV0Min / dVelocityScale;
//const Vect vV0 = {dV0, 0.0, 0.0};  // the vector for such motion



//
// the period, amplitude, and height (relative to the center of oscillation) of
//      the surface profile
//
const double dSurfPeriod = dRealLatticeSpacing / dLengthScale;
const double dSurfAmp  = 1.0*dRealSurfaceAmplitude / dLengthScale;
const double dSurfUp   = (1.0*dSurfAmp + (dRealRestPoint / dLengthScale));

// frequency
const double dSurfFreq = 1.0/dSurfPeriod;
const double dSurfOmega = dSurfFreq * (2.0*pi);



//
// The damping factor after each bounce (between zero and 1.0)
//
//static double dGamma = 0.22; // 0.7;
static double dGamma = 0.7; // 0.7;
//
// The last dGamma value used in the Bounce routine
//
static double dGammaLast = dGamma;




//
// the number of periods of the surface before periodicity occurs (mostly graphical)
//
const int iSurfacePeriodRepeat = 1.0;



//
// When the point goes under this tolerance, we must rectify
//
const double dINTRUSION_TOLERANCE = 1e-10; //dSurfAmp / 1.0e15;


//
// make impulses less than a certain amount totally elastic
//
const double dIMPULSE_TOLERANCE = 1e-2; // 1e-4;


//
// buffer to give the time-step integration
//
const double dTimeStepChange_TOLERANCE = (dSurfAmp + abs(dSurfUp))*0.1;



// the guessed number of time steps for a rotation (actually a factor of 2pi floating around);
const int iTimeStepSteps = 10;



///////////////////////////////////////////////////////////




// the surface function to work with and its derivative
double FSurface(const double& x) {
    return ((dSurfAmp * sin(x * dSurfOmega)) + dSurfUp);
} // FSurface

// first derivative
double FSurfaceDer(const double& x) {
    return (dSurfAmp * dSurfOmega * cos(x * dSurfOmega));
} // FSurfaceDer

// second derivative
double FSurfaceDer2(const double& x) {
    return (-dSurfAmp * dSurfOmega * dSurfOmega * sin(x * dSurfOmega));
} // FSurfaceDer


// curvature for a given point
double FSurfaceCurvature(const double& x) {
    static double fp;
    fp = FSurfaceDer(x);
    //return ( abs(FSurfaceDer2(x)) / pow(1.0 + (fp*fp),3.0/2.0) );
    
    //
    // NOTICE: the sign is important for focusing / defocusing effects
    //
    
    return ( -FSurfaceDer2(x) / pow(1.0 + (fp*fp),3.0/2.0) );
} // FSurfaceCurvature



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

// returns the sign of the number
inline double sgn(double x)
{
    if(x<0) return -1L;
    else return 1L;
} // sgn()

// returns the maximum/minimum number
inline double max(double x, double y)
{
    if(x<y) return y;
    else return x;
} // sgn()
inline double min(double x, double y)
{
    if(x<y) return x;
    else return y;
} // sgn()


// returns the maximum/minimum number
inline int max(int x, int y)
{
    if(x<y) return y;
    else return x;
} // sgn()
inline int min(int x, int y)
{
    if(x<y) return x;
    else return y;
} // sgn()



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

// places in the position and velocity from a state to a vector
inline void StateToVectors(Vect& x, Vect& v, const State& s)
{
    x.x = s.d_State[1];
    x.y = s.d_State[2];

    v.x = s.d_State[3];
    v.y = s.d_State[4];

} // StateToVectors


// places in the position and velocity from a state to a vector in reverse
inline void VectorsToState(State& s, const Vect& x, const Vect& v)
{
    s.d_State[1] = x.x;
    s.d_State[2] = x.y;

    s.d_State[3] = v.x;
    s.d_State[4] = v.y;

} // StateToVectors


///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////




// from numerical recipies
// solves a quadratic equation (with low error)
// assumes good data is put in (check discriminant BEFORE input)
// Input: standard coefficients ax2 + bx + c = 0, and number of root desired (1 or 2)
// Output: the root of the equation
double Quadratic(const double& a, const double& b, const double& c, const int iRoot)
{
    static double q, disc;
    
    disc = b*b-4.0*a*c;
    
    /*
    if (disc<0)
        exit(1);
    */
    
    q = -0.5*( b + sgn(b)*sqrt(disc) );
    
    if ( ((sgn(b)>0) && (iRoot == 1)) || ((sgn(b)<=0) && (iRoot == 2)) )
        return q/a;
    else
        return c/q;

} // quadratic


// same as about, but returns the pair into t1 and t2, only if the determinant >= 0
bool QuadraticPair(double& t1, double& t2, const double& a, const double& b, const double& c)
{
    static double q, disc;
    
    disc = b*b-4.0*a*c;
    
    
    if (disc<0) {
    
        return false;
        
    } // if
    else {
        
        q = -0.5*( b + sgn(b)*sqrt(disc) );
        
        t1 = q/a;
        t2 = c/q;
        
        return true;
    
    } // else
    
    return false;

} // quadratic



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


inline Vect GaussVect(double scale)
{
    static Vect v;
    v.x = scale*Gaussian();
    v.y = scale*Gaussian();
    v.z = scale*Gaussian();
    return v;
} // GaussVect

///////////////////////////////////////////////////////////


// returns the coordinates to plot
double const cPlotScale = 0.5L;
double PlotX(Vect v) {
    return v.y - v.x*cPlotScale;
}
double PlotY(Vect v) {
    return v.z - v.x*cPlotScale;
}
double PlotYFloor(Vect v) {
    return 0 - v.x*cPlotScale;
}



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////




// add states, etc.
inline void sAddM(State& ans, const State& s1, const State& s2)
{
    static int i;
    for(i = 0; i < iSTATESIZE; i++) {
        ans.d_State[i] = s1.d_State[i] + s2.d_State[i];
    } // for i
    return;
} // sAdd
inline void sSubtM(State& ans, const State& s1, const State& s2)
{
    static int i;
    for(i = 0; i < iSTATESIZE; i++) {
        ans.d_State[i] = s1.d_State[i] - s2.d_State[i];
    } // for i
    return;
} // sAdd
inline void sMultM(State& ans, const double& X, const State& s)
{
    static int i;
    for(i = 0; i < iSTATESIZE; i++) {
        ans.d_State[i] = s.d_State[i] * X;
    } // for i
    return;
} // sAdd

//////////////////////////


// add states, etc.
inline void sAddTo(State& s1, const State& s2)
{
    sAddM(s1, s1, s2);
    return;
} // sAdd
inline void sSubtOff(State& s1, const State& s2)
{
    sSubtM(s1, s1, s2);
    return;
} // sAdd
inline void sMultThis(const double& X, State& s)
{
    sMultM(s, X, s);
    return;
} // sAdd


//////////////////////////


// add states, etc.
inline State sAdd(const State& s1, const State& s2)
{
    static State ans;
    sAddM(ans,s1,s2);
    return ans;
} // sAdd
inline State sSubt(const State& s1, const State& s2)
{
    static State ans;
    sSubtM(ans,s1,s2);
    return ans;
} // sAdd
inline State sMult(const double& X, const State& s)
{
    static State ans;
    sMultM(ans,X,s);
    return ans;
} // sAdd



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


// a short function that sets the value with the minimum abs(x1 or x2) to ans
inline void MinAbs(double& ans, const double& x1, const double& x2)
{
    if(abs(x1) < abs(x2))
        ans = x1;
    else
        ans = x2;

} // MinAbs



// returns normal to the surface
void SurfaceNormal(Vect& n, const double& x)
{
    static double a;

    a = FSurfaceDer(x);
    n.x = -a;
    n.y = 1.0;
    VectUnitThis(n);

}

// returns tangent to the surface
void SurfaceTang(Vect& vt, const double& x)
{
    static double a;

    a = FSurfaceDer(x);
    vt.x = 1.0;
    vt.y = a;
    VectUnitThis(vt);

}


// reflects the particle off the surface, assuming that the particle is on the surface
void Bounce(State& sNow, double& dEnergyLoss, const double& dGamma_In)
{
    static double p;
    static Vect r,v,n;
    
    static double dEnergy1, dEnergy2;
    
    StateToVectors(r,v,sNow);
    
    
    dEnergy1 = VectLength(v);
    
    SurfaceNormal(n, r.x);
    
    p = VectDot(n,v);
    
    
    
    // to avoid chattering events, make the collisions totally elastic for small collisions
    // dGammaLast is set to the last value of gamma used
    if(abs(p)>dIMPULSE_TOLERANCE) {
        if(p<0.0) {
            dGammaLast = dGamma_In;
            VectMultThis(-(1.0L + dGammaLast)*p,n);
            VectAddTo(v,n);
        } // if
    } // if
    //else
    //   exit(1);
    else {
        if(p<0.0) {
            dGammaLast = (dGamma_In + (1.0-dGamma_In)*(1.0-(abs(p)/dIMPULSE_TOLERANCE)));
            VectMultThis(-(1.0 + dGammaLast)*p,n);
            VectAddTo(v,n);
        } // if        
    } // else
    
    
    //cout << abs(p) << "\n";
    


    dEnergy2 = VectLength(v);
    dEnergyLoss = dEnergy2 - dEnergy1;


    VectorsToState(sNow, r,v);

} // Bounce




// sets the point back on the surface doesn't change time
void SetOnSurface(State& sNow)
{
    static double x;
    x = sNow.d_State[1];
    sNow.d_State[2] = FSurface(x) + dINTRUSION_TOLERANCE;
} // SetOnSurface



// returns whether or not an intersection with the surface is possible, given the current state
// returns in 'delt' the time forward needed to get near the surface (up to second order)
// bForwardOnly: if this is set true, we only take forward time steps, otherwise we will take
//      the closest (in either direction)
bool IntersectSurface(double& delt, const State& sNow, const bool bForwardOnly)
{

    //
    // a "failsafe" routine to check the one below.
    //
    //delt = -(sNow.d_State[2]-FSurface(sNow.d_State[1]))/sNow.d_State[4];
    //return true;
    
    
    

    
    // several working variables to ease the computation
    static double f0, a, b, x0, y0, v0x, v0y, xc;
    
    // shorthand
    const double wx2 = dOmegaH*dOmegaH;
    const double wy2 = dOmegaV*dOmegaV;
    
    
    // unpack regular variables
    x0  = sNow.d_State[1];
    y0  = sNow.d_State[2];
    v0x = sNow.d_State[3];
    v0y = sNow.d_State[4];
    xc  = sNow.d_State[5];
    
    
    // variables describing function coefficients
    f0 = FSurface(x0);
    a = FSurfaceDer(x0);
    b = (-1.0/2.0) * FSurfaceDer2(x0);



    //f0 = 0.0;
    //a = -1.0;
    //b = 1.0;
    
    
    
    // make the coefficients in the quadratic equation - derived in a maple program
    static double A,B,C;
    
    A = -a*x0*wx2/2.0+b*v0x*v0x+a*xc*wx2/2.0+y0*wy2/2.0;
    
    B = a*v0x-v0y;
    
    C = f0-y0;
    
    
    ///////////////////////////////////////////////////////////
    // escape if A is simply too small (up to some tolerance)
    const double dERROR_TOLERANCE = 0.0; //1e-15;
    if(abs(A)<dERROR_TOLERANCE)
        return false;
    ///////////////////////////////////////////////////////////
    
    
    
    // only try to compute if the determinant is positive
    static double t1, t2;
    
    if(QuadraticPair(t1,t2,A,B,C) == true) {
        
        if(bForwardOnly==true) {
                
            if( (t1 > 0.0) && (t2 > 0.0) ) {
                MinAbs(delt, t1, t2);
            } else if(t1 <= 0.0) {

                if(t2 <= 0.0) {
                  // no acceptable solution
                  return false;
                } else {
                  delt = t2;
                }
                
            } else {
                // only alternative is t1>0
                delt = t1;
            }
                    
        
        } // if forward only
        else {

            // just return the minimum abs
            MinAbs(delt, t1, t2);        
        
        } // else
                
        
        return true;
        
    } // if
    else {
    
        // no intersection detected
        return false;
    
    } // else
    
    
    return false;
    
    


} // IntersectSurface


///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

const double dDrawBox = 0.2;

const double xmax = dSurfPeriod*iSurfacePeriodRepeat;
const double xmin = 0.0;
const double ymax = dSurfPeriod*iSurfacePeriodRepeat-dDrawBox;
const double ymin = -dDrawBox;

/*
const double xmax = dSurfPeriod*iSurfacePeriodRepeat;
const double xmin = 0.0;
const double ymax = 2.0*dDrawBox;
const double ymin = -dDrawBox;
*/

// scaling functions
void ScaleX(double& x)
{
    x = (2*(x-xmin)/(xmax-xmin)) - 1;
} // ScaleX
void ScaleY(double& y)
{
    y = (2*(y-ymin)/(ymax-ymin)) - 1;
} // ScaleY


// sets the values for 'x' and 'y' for a given state
void SetStateXY(const State& s, double& x, double& y)
{
    //x = (-sin(dPAngle*(pi/180L)) * s.d_State[1]) + (cos(dPAngle*(pi/180L)) * s.d_State[2]);
    x = s.d_State[1];
    y = s.d_State[2];
} // SetStateXY


//
// Displays line between two function segments, sNow and sLast, with given times
//
void DispSeg(const State& sNow, const State& sLast)
{
    
    static double x1,x2,y1,y2;

    SetStateXY(sNow, x1, y1);
    SetStateXY(sLast, x2, y2);

    ScaleX(x1);ScaleX(x2);
    ScaleY(y1);ScaleY(y2);
    
    DispLine(x1, y1, x2, y2, SCALE);
    //DispDot(x1,y1,SCALE);

} // DispSeg




void DispState(const State& s)
{
    // map onto normal coordinates
    //static State sTemp;
    //NormalCoords(sTemp,s);
    
    
    static double x, y;
    SetStateXY(s, x, y);
    //SetStateXY(sTemp, x, y);
    

    // output for external graphical use
    //cout << x << "\t" << y << "\n";
    

    ScaleX(x);
    ScaleY(y);
    
    DispDot(x, y, SCALE);

} // DispSeg




// displays the surface
void DispSurface()
{
    static State s1,s2;
    static double x;
    const double delx = (xmax - xmin) / 200;
    
    const double dSurfOmega = dSurfFreq * (2.0*pi);
    
    x = xmin;
    while(x<xmax) {
            s1.d_State[1] = x;
            s1.d_State[2] = FSurface(x);
            s2.d_State[1] = x+delx;
            s2.d_State[2] = FSurface(x+delx);
            
            DispSeg(s1,s2);
            
            x += delx;
    } // while

} //



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


//const int iSurfacePeriodRepeat = 5.0;

// resets the system to a periodic location, also keeps particle above the surface
bool EnforcePeriodic(State& sNow, int& iMoved)
{
    static bool bChanged;
    bChanged = false;

    if(sNow.d_State[1]>iSurfacePeriodRepeat*dSurfPeriod) {
        sNow.d_State[1] -= iSurfacePeriodRepeat*dSurfPeriod;
        sNow.d_State[5] -= iSurfacePeriodRepeat*dSurfPeriod;
        bChanged = true;
        iMoved++;
    }
    else if (sNow.d_State[1] < 0.0) {
        sNow.d_State[1] += iSurfacePeriodRepeat*dSurfPeriod;
        sNow.d_State[5] += iSurfacePeriodRepeat*dSurfPeriod;
        bChanged = true;
        iMoved--;
    }
    

    
    return bChanged;

} // EnforcePeriodic



//
// return the unbounced integrable motion of the tip (includes translation)
//
double Oscillate(State& sNow, const double& t1, const double& t2)
{
    static State sNext;
    static double t;
    
    t = t2 - t1;

    // the position and velocity
    static Vect x, v;
    StateToVectors(x, v, sNow);
    
    // find origin and remove the translational velocity
    x.x -= sNow.d_State[5];    // the origin
    v.x -= dV0;

    
    
    // motion is simply sines and cosines, independent harmonic motion
    static double sinwtH, sinwtV, coswtH, coswtV;
    sinwtH = sin(dOmegaH*t);
    sinwtV = sin(dOmegaV*t);
    coswtH = cos(dOmegaH*t);
    coswtV = cos(dOmegaV*t);
    
    
    sNow.d_State[0] += t;

    sNow.d_State[1] = ( x.x * coswtH )          + ( (1.0/dOmegaH) * v.x * sinwtH ) + (dV0 * t);
    sNow.d_State[2] = ( x.y * coswtV )          + ( (1.0/dOmegaV) * v.y * sinwtV ) + 0.0;

    sNow.d_State[3] = (-dOmegaH * x.x * sinwtH) + ( v.x * coswtH );
    sNow.d_State[4] = (-dOmegaV * x.y * sinwtV) + ( v.y * coswtV );


    // put back on the distances in the coordinates of the system
    sNow.d_State[1] += sNow.d_State[5];
    sNow.d_State[3] += dV0;
    
    
    // finally shift the origin appropriately
    sNow.d_State[5] += dV0*t;


    return t2;

} // Oscillate




///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////




// Returns the projection matrix for a given state.
// bProjectForward: true if good for multiplying on left, false if on right (backwards projection)
void ProjectOperator(Matrix& mJProj, const State& sNow)
{
    

    static Vect r,v,n, A;
    StateToVectors(r,v,sNow);
    
    SurfaceNormal(n,r.x);
    
    static double vdotn;
    vdotn = VectDot(v,n);
    

    A.x = -dOmegaH*dOmegaH*(r.x-sNow.d_State[5]);
    A.y = -dOmegaV*dOmegaV*r.y;
    
    
    
    MatrixUnit(mJProj);
    
    // spatial parts
    mJProj.M[0][0] -= v.x*n.x / vdotn;
    mJProj.M[0][1] -= v.x*n.y / vdotn;
    mJProj.M[1][0] -= v.y*n.x / vdotn;
    mJProj.M[1][1] -= v.y*n.y / vdotn;


    // velocity parts
    mJProj.M[2][0] -= A.x*n.x / vdotn;
    mJProj.M[2][1] -= A.x*n.y / vdotn;
    mJProj.M[3][0] -= A.y*n.x / vdotn;
    mJProj.M[3][1] -= A.y*n.y / vdotn;
    
    
    // "time" parts - for the extension parameter
    mJProj.M[4][0] -= dV0 * n.x / vdotn;
    mJProj.M[4][1] -= dV0 * n.y / vdotn;

} // ProjectOperator




// Returns the Jacobian in Poincare section coordinates through projection.
// Different from phase-space Jacobian in that the coordinates of the P-surface
//  do not have y as a variable.
// Coordinates are (in order):
//      x
//      vx
//      vy
//      DeltaX (extension)
//
// The final column / row in the matrix is set to zero
//
void ProjectJacobian(Matrix& mJProj, const Matrix& mJ, const State& sNow)
{
    static Matrix mTemp;
    mTemp = mJ;
    
    ProjectOperator(mJProj, sNow);
    
    //MatrixTranspose(mJProj,mJProj);
    MatrixMultiply(mJProj, mJProj, mTemp);
    
} // ProjectJacobian




// Properly reduces the coordinates of the Jacobian into those of the surface.
// Using the initial point (for initial coordinates) and final point (for final coords),
//  the full phase space Jacobian is reduced into a 4x4 matrix, essentially.
//
// The remaining 5th element is set to 0.0 such that problems should be minimized for
//  Newton searching and cycle expansions (1.0 - 0.0 = 1.0), even if we never use
//  N-R root finding.
//
void PMapJacobian(Matrix& mJProj, const Matrix& mJ, const State& sNow, const State& sLast)
{

    // do projection
    ProjectJacobian(mJProj, mJ, sNow);
    

    // account for x -> y when moving along surface -> gives precisely what numerical tests see
    static int i;
    for(i=0;i<5;i++) {
        mJProj.M[i][0] += mJProj.M[i][1]*FSurfaceDer(sLast.d_State[1]);
        mJProj.M[i][1] = 0;
    } // for
    
    
    // reduce the row to zero, since we don't need it (will not be seen in numerical studies)
    for(i=0;i<5;i++) {
        mJProj.M[1][i] = 0;
    } // for



} // PMapJacobian






///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////



// returns the free-flight Jacobian matrix for a given lapse in time t
void FreeFlightJacobian(Matrix& mJ, const double& delt)
{
    static double cosH, sinH, cosV, sinV;
    cosH = cos(dOmegaH*delt);
    sinH = sin(dOmegaH*delt);
    cosV = cos(dOmegaV*delt);
    sinV = sin(dOmegaV*delt);
    
    MatrixDiag(mJ, 0.0);
    
    mJ.M[0][0] = cosH;             mJ.M[0][2] = (1.0/dOmegaH)*sinH;
    mJ.M[2][0] = -dOmegaH * sinH;  mJ.M[2][2] = cosH;

    mJ.M[1][1] = cosV;             mJ.M[1][3] = (1.0/dOmegaV)*sinV;
    mJ.M[3][1] = -dOmegaV * sinV;  mJ.M[3][3] = cosV;
    
    
    
    // do pieces just for extension part
    mJ.M[0][4] = 1.0-cosH;
    mJ.M[2][4] = dOmegaH*sinH;
    mJ.M[4][4] = 1;

} // FreeFlightJacobian


// returns the bounced Jacobian matrix, given the state (from which
//      the surface normal is computed, and velocity converted)
//      and the gamma-factor (which may possibly change due to the state)
void DampedBounceJacobian(Matrix& mJ, const State& sNow, const double& dGamma_Passed)
{
    
    static double C1,C2, v0p, v0T, Ay;
    static Vect r,v,vn,vt;
    
    // convert state to vectors
    StateToVectors(r,v,sNow);
    
    // get the surface normal and tangent vectors
    SurfaceNormal(vn,r.x);
    SurfaceTang(vt,r.x);
       
    
    //////////
    ///////////////////////
    // create the 4x4 rotation matrix (resolves into sub-blocks)
    
    static Matrix mR, mRT;
    
    MatrixDiag(mR, 0.0);
    
    mR.M[0][0] = vt.x;  mR.M[0][1] = vt.y;
    mR.M[1][0] = vn.x;  mR.M[1][1] = vn.y;

    mR.M[2][2] = vt.x;  mR.M[2][3] = vt.y;
    mR.M[3][2] = vn.x;  mR.M[3][3] = vn.y;
    
    //////////        
    // finally, make sure the extension neighborhood is preserved
    mR.M[4][4] = 1;
    //////////    

    // inverse (transpose) of the matrix
    MatrixTranspose(mRT, mR);

    ///////////////////////
    //////////
    
    
    
    Ay = -vn.y*(dOmegaV*dOmegaV*r.y)  -  vn.x*(dOmegaH*dOmegaH*(r.x-sNow.d_State[5]));
    
    
    
    //////////
    ///////////////////////
    // first compute the matrix in coordinates in the parallel / perpindicular
    //    frame of reference


    // find the normal and perpindicular velocity 
    v0p =  (VectDot(vt, v));
    v0T = -(1.0/dGamma_Passed)*abs(VectDot(vn, v));
    
    
    // constant factor used often in a sub-block
    C1 = (1.0 + dGamma_Passed)*FSurfaceCurvature(r.x);
    C2 = (1.0 + dGamma_Passed);

    
    MatrixDiag(mJ, 0.0);
    
    mJ.M[0][0] = 1;  mJ.M[1][1] = -dGamma_Passed;

    mJ.M[2][2] = 1;  mJ.M[3][3] = -dGamma_Passed;
    
    
    
    mJ.M[2][0] = -C1*v0T;  mJ.M[2][1] = C1*v0p;
    mJ.M[3][0] = -C1*v0p;  mJ.M[3][1] = C1*( (v0p*v0p/v0T) );
    
    mJ.M[3][1] += (C2/v0T)*( Ay );    
    

    //////////    
    
    // finally, make sure the extension neighborhood is preserved
    mJ.M[4][4] = 1;



    ///////////////////////
    //////////



    
    
    
    // change frame of the Jacobian back into lab coordinates
    //MatrixMultiply(mJ, mJ, mRT);
    //MatrixMultiply(mJ, mR, mJ);

    MatrixMultiply(mJ, mRT, mJ);
    MatrixMultiply(mJ, mJ, mR);
    
    //MatrixTranspose(mJ,mJ);
    

} // DampedBounceJacobian



// Combines the above two operations for a translation and a bounce.  This implies
//  that we are at t + epsilon from our current position
// Multipies the result into the current Jacobian that is passed in.
void FullJacobianStep(Matrix& mJ, const State& sNow, const double& dGamma_Passed, const double& delt)
{
    static Matrix mJt, mJb;
    
    
    FreeFlightJacobian(mJt, delt);
    DampedBounceJacobian(mJb, sNow, dGamma_Passed);
       

    //MatrixMultiply(mJt, mJt, mJb);
    //MatrixMultiply(mJ, mJ, mJt);
    
    //MatrixTranspose(mJt, mJt);
    //MatrixTranspose(mJb, mJb);
    
    
    //cout << "\n\n"; MatrixPrint(mJt); cout << "\n\n";
    //cout << "\n\n"; MatrixPrint(mJb); cout << "\n\n";
    
    
    MatrixMultiply(mJt, mJb, mJt);
    MatrixMultiply(mJ, mJt, mJ);


} // FullJacobian




///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////



double Lyapunov(const Matrix& mJNow, const double& delt)
{
    static Matrix JT;
    MatrixTranspose(JT,mJNow);
    MatrixMultiply(JT, JT, mJNow);
    return (1.0/(2.0*delt))*log(JT.M[0][0]);
} // Lyapunov



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

// returns a measure of the maximum velocity for a state
inline void FindTimeStep(double& dt, const State& sNow)
{
    static Vect x,v;
    StateToVectors(x,v,sNow);
    
    static double velx2,vely2,delx,dely;
    
    delx = x.x-sNow.d_State[5];
    dely = x.y;
    
    velx2 = (dV0*dV0) + (v.x*v.x) + (dOmegaH*dOmegaH*delx*delx);
    vely2 = (v.y*v.y) + (dOmegaV*dOmegaV*dely*dely);

    
    // pick the time-step differently depending on if the particle is above or below
    //    the highest part of the surface
    //
    // below the surface max: pick a very small time-step
    // above the surface: pick a time-step that goes to the highest part and back down again
    if(x.y < dSurfUp + abs(dSurfAmp) + dTimeStepChange_TOLERANCE) {
                
        //dt = dSurfPeriod / sqrt(velx2+vely2);
        dt = dTimeStepChange_TOLERANCE / sqrt(velx2+vely2);
        
        dt /= iTimeStepSteps;

    } // if
    else {
    
        //else
        dt = dTimeStepChange_TOLERANCE/sqrt(vely2);
        
    } // else
    
     

} // FindTimeStep

///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////





// number of times to do iteration in search
const int iREPEAT_SEARCH = 3;




// Integrates step by step, and will recursively search for the surface.
// The function will return after the surface has been "found".
//
// State altered by reference.
//
// Returns true if we make a net movement of at least one-half of a surface wavelength.
//
// bIntegrateForward: if false, this routine does backward integration.
//
// !!!!!!!!!!!
// Having two variables the same place in memory, such as dFricWork and t, can cause errors
// !!!!!!!!!!!
//
bool IntegratePoincare(State& sNow, Matrix& mJNow, double& dFricWork, double& t, bool bIntegrateForward)
{



    //
    // if going backwards, REFLECT at the beginning
    //
    if(bIntegrateForward != true) {
        // reverse the bouncing
        static Vect r,v,n;
        
        // invert velocity
          StateToVectors(r,v,sNow);
          VectInvertThis(v);
          VectorsToState(sNow,r,v);
        
        // reflect the state off of the surface
        Bounce(sNow, dFricWork, 1.0/dGamma);

        // put velocity back in "forward" sense
          StateToVectors(r,v,sNow);
          VectInvertThis(v);
          VectorsToState(sNow,r,v);

    } // if





    // the number of surface periods the particle has moved
    static int iMoved;
    iMoved = 0;
        
    
    
    // store the original state and move the oscillation in one large step
    static State sInitialState;
    sInitialState = sNow;
    static double dInitialTime;
    dInitialTime = t;



    // the original coordinate of translation for the pulling point
    static double dPullingOriginal;
    dPullingOriginal = sInitialState.d_State[5];


    // go until we've hit a Poincare surface
    static bool bDone;
    bDone = false;
    
    
    // starting time and time step
    static double dt;
    
    
    
    
    while(bDone == false) {
    

        // figure out the time-step by lattice spacing and velocity
        FindTimeStep(dt, sNow);
        
        
        // if going backwards, reverse time
        if(bIntegrateForward != true)
                dt *= -1.0;
        
        
        // take a normal time-step
        t = Oscillate(sNow, t, t+dt);
        
        //cout << dt << "\n";
        
        
        // check for collisions the old-fashioned stupid way (avoids grazing)
        if(FSurface(sNow.d_State[1]) > sNow.d_State[2]) {
        
            
            
            // do one main step to get minimum error (hopefully) and set the state this way
            //sNow = sInitialState;
            //cout << t - dInitialTime << "\n";
            //t = Oscillate(sNow, dInitialTime, t);
            
            
            
            
            // step back once if we are integrating forward (where lots of problems can occur)
            if(bIntegrateForward==true)
                    t = Oscillate(sNow, t, t-dt);
            
            
            
            //
            // check for collisions, and make sure to only step backwards the first step
            //      if integrating forwards
            //
            static double delt;
            if(IntersectSurface(delt, sNow, bIntegrateForward) == true) {
                if(abs(delt)<(1.0+0.5)*abs(dt)) {
                
                                
                   //cout << delt << "\n";
                
                                   
                   // keep track of the time before the adjustment
                   static double dTimeOriginal;
                   dTimeOriginal = t;
    
    
                   // take the first surface finding step
                   t = Oscillate(sNow, t, t+delt);
    
    
                   // iterate the surface finding routine to get the particle near the surface
                   static int m;
                   for(m=0; m<iREPEAT_SEARCH; m++) {
                   
                       if(IntersectSurface(delt, sNow, false) == true) {                         
                         t = Oscillate(sNow, t, t+delt);
                       } // if intersect surface
                       //else
                       //   exit(1);
                         
                   } // for
                   
                } // if
                //else                        ///////////////////////////////////////////////////////
                //   exit(1);                 /// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! ///
                                            ///////////////////////////////////////////////////////
                
            } // if there is a time found that hits the surface (should always be yes)
            else {
                //exit(1);
                cout << "ERROR: bad reflection event at t = \t" << t << "\n";
            } // else if some problem happens

           
            // explicitly set the particle's position on the surface + epsilon
            SetOnSurface(sNow);


            //static Vect r,v,n;
            //StateToVectors(r,v,sNow);
            //SurfaceNormal(n,r.x);
            
            


            // if going forward, REFLECT the state off of the surface
            if(bIntegrateForward == true) {
                Bounce(sNow, dFricWork, dGamma);
            } // if




            // display the state
            DispState(sNow);

           
            // register as being on the Poincare section
            bDone = true;


        } // if the particle goes below the surface
        
        
        // keep the position in the x-direction on a specified torus
        if(EnforcePeriodic(sNow, iMoved) == true) {
              // nothing
        }
    
    
        // display the state (might only do every so often later)
        DispState(sNow);
        
    } // while
    
    
    
    ///////////
    /////////////////////
    // step the Jacobian matrix as well
    
    FullJacobianStep(mJNow, sNow, dGammaLast, t-dInitialTime);

    /////////////////////
    ///////////
    
    
    const double dOtherSideDivide = 4.0;
    
    if( abs((dPullingOriginal) - (sNow.d_State[5]+(iMoved*dSurfPeriod))) >= (dSurfPeriod/dOtherSideDivide) ) {
        return true;
    } // if
    else {
        return false;
    } // else
    


} // IntegratePoincare






///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


// norm between vectors
double dSNorm(const State& s1, const State& s2)
{
    static int i;
    static double dTot;
    static double x;
    dTot = 0;
    for(i=2;i<=4;i++) {
        x = s1.d_State[i]-s2.d_State[i];
        dTot += x*x;
    }
    
    
    // treat cases for x and extension separately
    
    // x - periodic - check middle, backward, and forward
    static double xtemp;
    x = abs(s1.d_State[1]-s2.d_State[1]);
    xtemp = abs(s1.d_State[1]-s2.d_State[1] - dSurfPeriod);
    if(xtemp < x)
        x = xtemp;
    xtemp = abs(s1.d_State[1]-s2.d_State[1] + dSurfPeriod);
    if(xtemp < x)
        x = xtemp;
    dTot += x*x;

    
    
    // measure pulling point by EXTENSION, not absolute location
    x = (s1.d_State[1]-s1.d_State[5])-(s2.d_State[1]-s2.d_State[5]);
    dTot += x*x;
    
    
    return sqrt(dTot);

} // dSNorm




// an integrated norm for periodic points by going backwards and forwards
//
// constant is the large value if we do not traverse at least one half surface period
const double dMAXIMUM_NORM = 1e15;
//
double dSNormPeriodic(const State& sTest)
{
    static State sTestInt1, sTestInt2;
    static double t,dNormTest;
    
    //t = 0;

    
    // dummy variables to pass
    static double dDummy;
    static Matrix mDummy;
    
    //dDummy = 0.0;
    //MatrixUnit(mDummy);


    // track whether or not the test trajectory travels at least half the system
    //    period (otherwise could have false fixed points)
    static bool bOtherSide;
    bOtherSide = true;


    // do the integration and test the norm and whether it "crossed to the other side"
    sTestInt1 = sTest;
    bOtherSide = IntegratePoincare(sTestInt1, mDummy, dDummy, t, true);
    //dNormTest = dSNorm(sTest,sTestInt);
    
    // do backwards integration and measure
    sTestInt2 = sTest;
    bOtherSide = IntegratePoincare(sTestInt2, mDummy, dDummy, t, false) && bOtherSide;
    //dNormTest += dSNorm(sTest,sTestInt);
    
    
    //sTestInt1 = sTest;
    dNormTest = dSNorm(sTestInt1,sTestInt2);
    

    /*
    if(bOtherSide == true)
        return dNormTest;
    else
        return dMAXIMUM_NORM;
    */
    
    return dNormTest;
    

} // dSNorm



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


//
// outputs the 3d coordinate choices for the Poincare section to cerr stream
//
void OutputPoincare3D(const State& sNow)
{
    // output points for Poincare section            
    static Vect r,v,vn,vt;
    StateToVectors(r,v,sNow);


    SurfaceTang(vt,r.x);
    SurfaceNormal(vn,r.x);



    /*
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectLength(v) << ", ";
    cerr << VectDot(v,vn) << "\n";
    */
    
    /*
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectLength(v) << ", ";
    cerr << VectDot(v,vt) << "\n";
    */
    
    /*
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectDot(v,vn) << ", ";
    cerr << VectDot(v,vt) << "\n";
    */
    
    /*
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << r.x << ", ";
    cerr << VectDot(v,vt) << "\n";
        cerr << sNow.d_State[5] - r.x << ", ";
        cerr << r.x + dSurfPeriod << ", ";
        cerr << VectDot(v,vt) << "\n";
    */

    
    /*
    cerr << VectDot(v,vt) << ", ";
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectLength(v) - VectDot(v,vt) << "\n";
    */
    








    
    cerr << VectDot(v,vt) << ", ";
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectDot(v,vn) << "\n";
    

    /*
    cerr << sNow.d_State[1] << ", ";
    cerr << sNow.d_State[5] - r.x << ", ";
    cerr << VectDot(v,vt) << "\n";
    */

    
    return;

} // OutputPoincare3D



//
// attempts some sort of 1d-1d map in some coordinate
//
void OutputPoincareMap(const State& sNow, const State& sLast)
{
    
    // output points for Poincare section            
    static Vect r1,v1,vn1,vt1;
    StateToVectors(r1,v1,sNow);

    SurfaceTang(vt1,r1.x);
    SurfaceNormal(vn1,r1.x);

    static Vect r2,v2,vn2,vt2;
    StateToVectors(r2,v2,sLast);

    SurfaceTang(vt2,r2.x);
    SurfaceNormal(vn2,r2.x);



    /*
    cerr << sNow.d_State[5]  -  sNow.d_State[1] << ", ";
    cerr << sLast.d_State[5] - sLast.d_State[1] << "\n";
    */
    
    
    
    //cerr << sNow.d_State[5]  -  sNow.d_State[1] << ", ";
    cerr << VectDot(v1,vt1) << ", ";
    cerr << VectDot(v2,vt2) << "\n";
    


    /*
    cerr << sNow.d_State[5] -   sNow.d_State[1] << ", ";
    cerr << sLast.d_State[5] - sLast.d_State[1] << ", ";
    cerr << VectDot(v1,vt1) << "\n";
    */


} // OutputPoincareMap


///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


//
// Performs the step to stochastically search for a minimum solution.
// Purpose: technically easier than using the inverse of the Jacobian matrix, but
//      in the future, I may change the primary routine to an inverse Jacobian search.
//
// !!! Currently only searches for periodic points !!!
//
void StochPeriodicStep(State& sNow, double& dNorm, const double& dScaleFactor)
{

    // test norm and states
    static State sTest;
    sTest = sNow;
    
    // time lapse (mostly a dummy variable
    static double t, dNormTest, dDummy;
    
                    
    
    // create the test trajectory by varying ALL coordinates, and then ensuring
    //    they are on the Poincare surface
    static int ii;
    for(ii=1;ii<=5;ii++)
         sTest.d_State[ii] += Gaussian()*dScaleFactor;
    SetOnSurface(sTest);
    Bounce(sTest, dDummy, 1.0);
    
    
    // get norm for test state
    dNormTest = dSNormPeriodic(sTest);
    
    
    // update state and norm if the trajectory has a lower norm
    if(dNormTest < dNorm) {
    
       dNorm = dNormTest;
       sNow = sTest;       
    
    } // if


    return;

} // StochPeriodicStep



// multiplier of the norm for searching
const double dStochasticWidthMult = 0.01;


//
// Performs several steps to look for and lock on a periodic orbit.  If the initial
//      guess is poor, convergence may be slow.
//
void StochPeriodicSearch(State& sNow)
{
    static double dNorm;
    dNorm = dSNormPeriodic(sNow);

    static int k;
    for(k=0; k<10000; k++) {

        StochPeriodicStep(sNow, dNorm, dNorm*dStochasticWidthMult);


        if(k%100 == 0) {
            DispSurface();
            Update();
            Clear();
        } // if


        if(Done()==true)
            exit(1);
    
    
    } // k
        
    
    ///////////////////////////////////////////
    ///////////////////////////////////////////
    
    
    
    static int index;
    cerr << "dNorm\t" << dNorm << "\n";
    for(index=1; index<=5; index++)
      cerr << index << "\t" << sNow.d_State[index] << "\n";
    cerr << "\n\n";



    ///////////////////////////////////////////
    ///////////////////////////////////////////


} // StochPeriodicSearch



///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


// multiplies a matrix onto a state vector (operates from LEFT)
void OperateState(State& sOp, const Matrix& mOp, const State& sNow)
{
    static State sTemp;
    sTemp = sNow;
    
    sMultThis(0.0, sOp);

    static int i,j;
    for(i=0;i<iMatrixSize;i++) {
    for(j=0;j<iMatrixSize;j++) {
        sOp.d_State[i+1] += mOp.M[i][j]*sTemp.d_State[j+1];
    } // for j
    } // for i

} // OperateState



// performs strictly a Jacobian MAP integration
bool IntegratePoincareMap(State& sNow, Matrix& mJMap, double& dFricWork, double& t, bool bIntegrateForward)
{
    // the boolean to return
    static bool bToReturn;
    
    static State sLast;
    sLast = sNow;
    
    
    // !!!!!!!!!!!!!!!!!!!!!!!!
    // try to remove later
    // !!!!!!!!!!!!!!!!!!!!!!!!    
    static Matrix mJNow;
    MatrixUnit(mJNow);
    
    

    bToReturn = IntegratePoincare(sNow, mJNow, dFricWork, t, bIntegrateForward);
    
    PMapJacobian(mJMap, mJNow, sNow, sLast);
    //ProjectJacobian(mJNow, mJNow, sNow);
    
    
    return bToReturn;

} // IntegratePoincareMap




// performs a single step in Newton's method of root finding
void NewtonStep(State& sNow, int iIter)
{

    static double dFricWork, t;
    dFricWork = 0; t = 0;


    static State sLast, sTemp;


    static Matrix mJMap, mUnit;
    MatrixUnit(mJMap);


    // make the unit matrix if the first time around
    static bool bInit = false;
    if(bInit==false) {
        MatrixUnit(mUnit);
        bInit = true;
    } // if
    
    
    
    sNow.d_State[0] = 0;
    sLast = sNow;
    while(iIter > 0) {
        IntegratePoincareMap(sNow, mJMap, dFricWork, t, true);
        iIter--;
    }
    //MatrixPrint(mJMap);


    
    
    sSubtM(sTemp, sNow, sLast);
    //OutputPoincare3D(sTemp);
    
    
    MatrixMult(mJMap,-1.0);
    MatrixAdd(mJMap, mUnit, mJMap);
    MatrixInverse(mJMap);
    
    
    OperateState(sTemp, mJMap, sTemp);
    
    
    sAddM(sNow, sLast, sTemp);
    SetOnSurface(sNow);
    

} // sNow


///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

// returns the term for the evolution operator of a given orbit for iP iterations







///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////


////////////////////////////////////////////////
////////////////////////////////////////////////
///                                          ///
///         The Overall Simulation           ///
///                                          ///
////////////////////////////////////////////////
////////////////////////////////////////////////


//
// The overall MD simulation
//
int MainSequence()
{
  int iPermute = 0;        // counter to screen update
  long int iCounter = 0;      // total counter
  
  

  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  
  //////////////////////////////////////////////////
  //  Parameters to search for eigenvalues with   //
  //////////////////////////////////////////////////

        
  static const int iT_Max = 4;      // total number of sample runs
  static int iT_Iter = 1;                // current sample run

  
  
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
    

  ///////////////////////////////////////////////////////
  // get a time reading for computational rate purposes//
  clock_t tv1, tv2;                                    //
  double time;                                         //
  tv1 = clock();                                       //
  ///////////////////////////////////////////////////////



  ////////////////////////
  
  //
  // OPEN A FILE FOR DATA OUTPUT (may be multiple in the future)
  //
  ofstream fOutput (sOutput_File);
  if (fOutput.is_open() == false) return 1;
  
  // Basic formatting setup
  fOutput.precision(iOutput_Precision);    // set output precision

  ////////////////////////


  ////////////////////////
  ////////////////////////

  // random testing for things
  /*
  Matrix A = {
    1,4,1,1,
    1,-1,1,-3,
    6,1,-5,1,
    1,1,1,2};
  MatrixPrint(A);
  cout << "\n";
  MatrixInverse(A);
  MatrixPrint(A);
  */




  ////////////////////////
  ////////////////////////



  ////////////
  ////////////


    /////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////
    ///                                                       ///
    ///            The Main Loop of the Program               ///
    ///                                                       ///
    /////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////
    
    

    // preferred time difference
    //static double dt = 0.01L;
    
       
    
    // parameters / variables for Lyapunov scanning
    //const long int iMinSkip = 2;
    //const double dTimeMin = 10.0;
    //const double dSampleTimeMin = 10.0;
    const double dMinTime = 50.0;
    //const long int iSample_Max = 100;
    //static long int iSample = 0;
    
    
    /*
    // scan through a parameter (velocity)
    const double dpmin = dRealV0Min / dVelocityScale;
    const double dpmax = dRealV0Max / dVelocityScale;
    const int NSample = 1000;
    const double dp = (dpmax-dpmin)/NSample;
    dV0 = dpmin;
    */
    

    // scan through a parameter (gamma)
    const double dpmin = dGamma;
    const double dpmax = 0.999;
    const int NSample = 10000;
    const double dp = (dpmax-dpmin)/NSample;
    dGamma = dpmin;
    
    
    // number of collions for a given sample and the maximum number of such collisions per sample
    static int iCollisions = 0;
    const int iCollisionMax = 10000;

    // number of Poincare map returns to skip initially
    const int iCollisionSkip = 1000;
    
    

    // keep track of frictional work
    static double dFricWork = 0;



    // time
    double t = 0.0L;
    
    
    
    //cout << "dt:\t" << dt << "\n";
    
   

    //////////////////////////////////////////////////////

    // Initialize the screen
    //Clear();
    OpenScreen();   // not totally necessary?... eh
       

    //////////////////////////////////////////////////////
    

    //////////////////////////////////////////////////////
    //             Do Computational Work                //
    //////////////////////////////////////////////////////

 
 
    // working states
    static State sNow, sLast;
    
    
    // Jacobian of the trajectory
    static Matrix mJNow;
    MatrixUnit(mJNow);
    
 

    /*
    //  good 1-cycle for v = 3.4 * ( )
    sNow.d_State[0] = 0.0;                          //t
    sNow.d_State[1] = 0.7790552555422861;           //xx
    sNow.d_State[2] = -0.09455598399259578;          //xy
    sNow.d_State[3] = 0.08074818402042061;          //vx
    sNow.d_State[4] = 0.05317276999548161;          //vy
    sNow.d_State[5] = 0.7754441689957476;           //origin
    */
    
    
    //  BAD 2-cycle for v = 3.4 * ( )
    sNow.d_State[0] = 0.0;                   //t
    sNow.d_State[1] = 0.649055256;           //xx
    sNow.d_State[2] = -0.077453237;          //xy
    sNow.d_State[3] = 0.096881551;           //vx
    sNow.d_State[4] = 0.007857029;           //vy
    sNow.d_State[5] = 0.689055256;           //origin
   





    //////////////////////////////
    //////////////////////////////



    /*
    // good 1-cycle for v = 3.25 * ( )
    sNow.d_State[0] = 0.0;                          //t
    sNow.d_State[1] = 0.7727990790908271;           //xx
    sNow.d_State[2] = -0.09516895495504646;          //xy
    sNow.d_State[3] = 0.07929761370219941;          //vx
    sNow.d_State[4] = 0.04015038572741248;          //vy
    sNow.d_State[5] = 0.7662009663519059;           //origin
    */
    
    
    /*
    // good 2-cycle for v = 3.25 * ( )
    sNow.d_State[0] = 0.0;                         //t
    sNow.d_State[1] = 0.7185264561949263;           //xx
    sNow.d_State[2] = -0.09427983601390777;          //xy
    sNow.d_State[3] = 0.1009658199841673;          //vx
    sNow.d_State[4] = 0.016856460624811;          //vy
    sNow.d_State[5] = 0.7336502776544807;           //origin
    */

    /*
    // good 2-cycle for v = 3.25 * ( )
    sNow.d_State[0] = 0.0;                         //t
    sNow.d_State[1] = 0.8899493409287317;           //xx
    sNow.d_State[2] = -0.06131434721494262;          //xy
    sNow.d_State[3] = 0.0462365578264169;          //vx
    sNow.d_State[4] = 0.05853144080843686;          //vy
    sNow.d_State[5] = 0.8387313799485394;           //origin
    */


    /*
    // UNCERTAIN 1-cycle for v = 3.25 * ( )
    sNow.d_State[0] = 0.0;                          //t
    sNow.d_State[1] = 0.7409124119876527;           //xx
    sNow.d_State[2] = -0.09599714364790896;          //xy
    sNow.d_State[3] = 0.07974542814528193;          //vx
    sNow.d_State[4] = 0.03340547236429609;          //vy
    sNow.d_State[5] = 0.7556479845375701;           //origin
    */




    // set on the surface and initialize
    SetOnSurface(sNow);
    static State sInit = sNow;
    



    State sTemp;
    sTemp = sNow;
    IntegratePoincare(sTemp, mJNow, dFricWork, t, true);
    sTemp = sNow;
    IntegratePoincare(sTemp, mJNow, dFricWork, t, false);
    DispSurface();
    Update();
    Pause();

    
    /*
    for(int i=0; i<1; i++) {
        IntegratePoincare(sTemp, mJNow, dFricWork, t, true);
        
        if(i%100==1) {
            DispSurface();
            Update();
            //Pause();
            Clear();
        } // if
    }
    DispSurface();
    Update();
    Pause();
    */
    
    
    /*
    ///////////////////
    for(int i=0; i<0; i++) {
        NewtonStep(sNow,2);
        DispSurface();
        Update();
        Pause();
        Clear();
    }
    ///////////////////
    */
    
    
    
    StochPeriodicSearch(sNow);
    
    



    //////////////////////////////////////////////////
    //   Perform shooting integration, adjustment   //
    //////////////////////////////////////////////////




    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////

    
    /*
    State sCenter = {0.0, 0.7790552555422861, -0.09455598399259578, 0.08074818402042061, 0.05317276999548161, 0.7754441689957476};


    State sDir1 = {0.0, -2.548137008, 0, -.84977471e-1, -.7211243395, -3.172604601};
    State sDir2 = {0.0, -.9859185770, 0, -.2151911679 , .1920008701 , -.4726587329};

    State sDir3 = {0.0, -.4941321138, 0, .3798114740, -.1360420570, -.4717769937};
    State sDir4 = {0.0, .1528762712, 0, .1030671642e-1, .1290061124, -.5359498635e-1};


    State sPlane1, sPlane2, sChange;
    double dDist = 0.01;
    int iM = 10;
    double dDDist = dDist/iM;
    for(int i=-iM;i<=iM;i++) {
    for(int j=-iM;j<=iM;j++) {
    
        sPlane1 = sCenter;
        sMultM(sChange, 0.5*i*dDDist, sDir1); sAddM(sPlane1, sPlane1, sChange);
        sMultM(sChange, 0.5*j*dDDist, sDir2); sAddM(sPlane1, sPlane1, sChange);
        SetOnSurface(sPlane1);
        IntegratePoincareMap(sPlane1, mJNow, dFricWork, t, true);
        OutputPoincare3D(sPlane1);
        
        sPlane2 = sCenter;
        sMultM(sChange, i*dDDist, sDir3); sAddM(sPlane2, sPlane2, sChange);
        sMultM(sChange, j*dDDist, sDir4); sAddM(sPlane2, sPlane2, sChange);
        SetOnSurface(sPlane2);
        //OutputPoincare3D(sPlane2);
        
    } // j
        DispSurface();
        Update();
    } // i

    */

    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////


    /*
    IntegratePoincareMap(sNow, mJNow, dFricWork, t, true);
    DispSurface();
    Update();
    Pause();
    */




    ///////////////////
    //StochPeriodicSearch(sNow);
    ///////////////////
    
    
    /*
    // lower and upper bounds to the attactor in state variables
    State sLower, sUpper;
    for(int i=1; i<iSTATESIZE; i++) {
        sLower.d_State[i] = 1e50;
        sUpper.d_State[i] = -1e50;
    }
    
    
    double dEnergyLoss = 0;
    
    int iN = 10000;
    for(int jfaj=0;jfaj<0*1000;jfaj++)
        IntegratePoincareMap(sNow, mJNow, dFricWork, t, true);
    dFricWork = 0;
    t = 0;
    for(int jfaj=0;jfaj<iN;jfaj++) {
        //NewtonStep(sNow,1);
        IntegratePoincareMap(sNow, mJNow, dFricWork, t, true);
        dEnergyLoss += dFricWork;
        
        if(jfaj%100==1) {
            DispSurface();
            Update();
            //Pause();
            Clear();
        } // if
        
        OutputPoincare3D(sNow);
        
        
        for(int i=1; i<iSTATESIZE; i++) {
            if(sNow.d_State[i] < sLower.d_State[i])
                sLower.d_State[i] = sNow.d_State[i];
            if(sNow.d_State[i] > sUpper.d_State[i])
                sUpper.d_State[i] = sNow.d_State[i];
        }

        
        Done();
        
        //cerr << "dNorm\t" << dSNormPeriodic(sNow) << "\n";
    } // for
    //for(int index=1; index<=5; index++)
    //  cerr << index << "\t" << sNow.d_State[index] << "\n";
    //cerr << "\n\n";
    
    cout << "<delta E>\t" << dEnergyLoss / iN << "\n";
    cout << "<delta t>\t" << t / iN << "\n";
    cout << "<dE / dt>\t" << dEnergyLoss / t << "\n";
    
    
    cout << "\n\n";    
    for(int i=1; i<iSTATESIZE; i++) {
        cout << sLower.d_State[i] << "\t";
        cout << sUpper.d_State[i] << "\n";
    }
    */
    
    

    
    
    
    //Update();
    //Pause();
    
    
    /*
    Matrix mJMap;
    MatrixUnit(mJNow);
    sNow.d_State[0] = 0;
    sLast = sNow;
    //IntegratePoincare(sNow, mJNow, dFricWork, t, true);
    //PMapJacobian(mJMap, mJNow, sNow, sLast);
    IntegratePoincareMap(sNow, mJNow, dFricWork, t, true);
    MatrixPrint(mJNow);
    */
    
    
    
    
    
    
    /*

    sNow.d_State[1] += 0.001;
    State sRLast;
    for(int i=0; i<1000+50000; i++) {
        sRLast = sNow;
        
        IntegratePoincare(sNow, mJNow, dFricWork, t, true);
                
        //SetOnSurface(sNow);
        //Bounce(sNow, dFricWork, 1.0);
        
        //if(i % 10 == 0)
        //        cout << i << "\t" << dSNorm(sNow,sRLast) << "\n";
        
        if(i % 100 == 0) {
            DispSurface();
            Update();
            Clear();
        }
        
        if(i>50000) {
            OutputPoincare3D(sNow);
            //OutputPoincareMap(sNow, sRLast);
            //cerr << sNow.d_State[0] << "\n";
        }
            
        Done();
        
    } // for
    
    
    
    DispSurface();
    Update();
    //Pause();
    */
    
    
    
    
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////

    /*

    static Vect vt,vn;
    SurfaceTang(vt, sInit.d_State[1]);
    SurfaceNormal(vn, sInit.d_State[1]);
    static double vIt, vIn;
    vIt = vt.x*sInit.d_State[3] + vt.y*sInit.d_State[4];
    vIn = vn.x*sInit.d_State[3] + vn.y*sInit.d_State[4];

    

    
    const int iDeepScan = 20;
    const double dDeepScale = 1.0/iDeepScan;
    static State sDeep;
    static int i1,i2,i3,i4,i5, iin = 0;
    for(i1=-iDeepScan; i1<iDeepScan; i1++) {
    for(i3=-iDeepScan; i3<iDeepScan; i3++) {
    for(i4=-iDeepScan; i4<iDeepScan; i4++) {
    for(i5=-iDeepScan; i5<iDeepScan; i5++) {
        
        sDeep = sInit;
        
        sDeep.d_State[1] += (i1*dDeepScale)*0.3 + 0.05;

        //static Vect vt,vn;
        SurfaceTang(vt, sDeep.d_State[1]);
        SurfaceNormal(vn, sDeep.d_State[1]);
        
        sDeep.d_State[3] = vt.x*(vIt + (i3*dDeepScale)*0.05) + vn.x*(vIn + (i4*dDeepScale)*0.04 - 0.00);
        sDeep.d_State[4] = vt.y*(vIt + (i3*dDeepScale)*0.05) + vn.y*(vIn + (i4*dDeepScale)*0.04 - 0.00);
        sDeep.d_State[5] = sDeep.d_State[1] + ((i5*dDeepScale) * 0.08);

        Bounce(sDeep, dFricWork, 1.0);        
        SetOnSurface(sDeep);
        
        
        
        static double dNorm;
        dNorm = dSNormPeriodic(sDeep);
        if((dNorm < 0.03) && (dNorm > 1e-7)) {
             OutputPoincare3D(sDeep);
             
             cout << dNorm << "\t" << " _ \t";
             for(int p=1;p<=5;p++)
                cout << sDeep.d_State[p] << "\t";
             cout << "\n";
             
        } // if
            
    }
        //cerr << "\n";
        DispSurface();
        Update();
        Clear();
        Done();
        //IntegratePoincare(sDeep, mJNow, dFricWork, t, false);
    }}} // for's
    
    cout << "\n\nFINISHED SCAN\n\n";
    
 
    //Pause();
    
    
    */
    
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////

    
    /*
    static double del_dV0, dV0_0;
    static int iN = 20;
    del_dV0 = 0.2 * dV0/iN;
    dV0_0 = dV0;
    for(int k = 0; k<=iN; k++) {
        dV0 = dV0_0 + (k-(iN/2))*del_dV0;
        //StochPeriodicSearch(sNow);
    } // for
    */
    
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////

    /*
    t = 0;
    MatrixUnit(mJNow);
    IntegratePoincare(sNow, mJNow, dFricWork, t, true);
    Clear();
    
    
    Oscillate(sNow, 0, t/2.0);
    */


    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////

    /*
    
    SetOnSurface(sNow);
    
    sNow.d_State[4] *= 1.0;
    
    const double dTINY = 2e-4;
    sNow.d_State[2] +=   0.0;
    

    static State sDirection, sStart, sMani;
    
    // -1.235300564, 0, .7408491340e-1, .707256948, 1.320244610
    // -1.092840604, 0, .7070369550, -.450759123e-1, .9683344297
    sDirection.d_State[0] =  0;
    sDirection.d_State[1] =  -1.092840604;
    sDirection.d_State[2] =  0;
    sDirection.d_State[3] =  .7070369550;
    sDirection.d_State[4] =  -.450759123e-1;
    sDirection.d_State[5] =  .9683344297;
        
    
    //sStart = sDirection;
    //sMultThis(dTINY, sStart);
    //SetOnSurface(sStart);
    
    //sEnd = sStart;
    //IntegratePoincare(sEnd, mJNow, dFricWork, t, true);
    
    
    
    // logarithmically break the interval into a number of points
    const int iManiN = 1000;
    
    // number of steps to include
    const int iManiIter = 10;
    
    // the multiple (in units of dTINY) to create forward for 1 step
    const double dManiScale = 2.0;
    
    for(int i = 1; i<=iManiN; i++) {
    
        static double s;
        s = i / ((double)iManiN);    // between 0 and 1
        
        s = (s*(dManiScale) + 1.0)*dTINY;
        
        
        if(i%2==0)
                s *= -1.0;
        
        
        // rescale logarithmically
        //s = dTINY * log(s);
        
        
        sMultM(sMani, s, sDirection);
        sAddM(sMani, sNow, sMani);
        SetOnSurface(sMani);
        for(int j = 0; j<iManiIter; j++) {
                IntegratePoincare(sMani, mJNow, dFricWork, t, true);
                OutputPoincare3D(sMani);
        }
        
        
        
    } // for
    
    
    DispSurface();
    Update();
    Pause();
    */

    
   
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////




    
    
    ////////////////////////////////////////
    
    /*
    // create a line of states and watch the evolution
    const int iEnsemble = 200;
    static double dLambda = 1.0 / iEnsemble;
    static State s_Ensemble [iEnsemble];
    static int p;
    for(p = 0; p < iEnsemble; p++) {
            
        s_Ensemble[p] = sInit;
        s_Ensemble[p].d_State[1] += ((dLambda*p)-0.5) * 0.3;
        s_Ensemble[p].d_State[5] += ((dLambda*p)-0.5) * 0.3;
        
        
        SetOnSurface(s_Ensemble[p]);
    } // for
    


    // get the point on the surface
    // IntegratePoincare(sNow, mJNow, dFricWork, t);

    MatrixUnit(mJNow);
    dFricWork = 0;
    
    
    //static State sLast;
    for(p = 0; p < iEnsemble; p++) {

        sNow = s_Ensemble[p];
        
        
        //IntegratePoincare(sNow, mJNow, dFricWork, t, true);
        //NewtonStep(sNow, 1);
        OutputPoincare3D(sNow);
        
        
        StochPeriodicSearch(sNow);
        
        //DispSurface();
        //Update();
        //Clear();
        
                
        if(Done()==true)
            exit(1);
                
    } // for p

    DispSurface();
    Update();
    Clear();
    //Pause();    
    */

    /*
    if(Done()) {
        DispSurface();
        Update();
        PrintScreen();
        exit(1);
    }
    */
                
        

    //////////////////////////////////////////////////////

      
  
  /////////////////////////////////////

  //
  // Update and Close the screen
  //
  
  Update();
  CloseScreen();
  

  // output the graphics
  PrintScreen();


  /////////////////////////////////////



  //
  // CLOSE THE FILE AND BE NICE
  //
  fOutput.close();  


} // MainSequence



///////////////////////////////////////////////////////////




int main(int argc, char *argv[])
{

  // initialize the screen
  InitScreen();
  
  // Basic formatting setup
  cout.precision(iOutput_Precision);    // set output precision
  cerr.precision(iOutput_Precision);    // set output precision


  ////////////////////////////
  // Begin the real program //
  MainSequence();          //
  ////////////////////////////



  // Remove "//" to pause once before leaving
  // Pause();
  
  //PrintScreen();

  //////////////////////////////////////////////////////////////


  return 0;
}