/* rkdriver.c:  driver functions for Runge Kutta ODE solver given in kutta.c
                Written by Peng Zhang.  Jan.5, 1996                         */

#include <stdio.h>
#include <math.h>
#include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/kutta.h"
#include <matrix.h>
#include <quat.h>
#include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/polytope.h" 
#include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/options.h"


#define SMALL  0.000001
#define MAXSTP 10000
#define TINY 0.000000001

void  cal_vel_change(col_Mat3 Mat, col_Vect3 Delta_v,
                     double frict, double restit);


/* Local globals                                                      */

Status        status;
int kmax, kount;
int STICK = 0;
int FLAG = 0;
double alpha, beta;

col_Mat3 M;
double Frict;
double *xp, **yp, dxsav;

void rkerror(char *str);

/* differential equation used in solving the impulse                   */

void f(double x, double *y, double *dydx)
{
     double tmp;
   
     if(STICK) {

         dydx[0] = 0.0;
         dydx[1] = 0.0;
         dydx[2] = M[2][0]*alpha + M[2][1]*beta + M[2][2];
         }
     else {
         tmp = Frict/sqrt(y[0]*y[0] + y[1]*y[1]);
         dydx[0] = -M[0][0]*y[0]*tmp - M[0][1]*y[1]*tmp + M[0][2];
         dydx[1] = -M[1][0]*y[0]*tmp - M[1][1]*y[1]*tmp + M[1][2];
         dydx[2] = -M[2][0]*y[0]*tmp - M[2][1]*y[1]*tmp + M[2][2];
          }
     return;
} 


/* differential equation used in the sticking mode: since the sticking mode
   is also considered in the above function, f1 is actually not used.     */

void f1(double x, double *y, double *dydx)
{
     dydx[0] = 0.0;
     dydx[1] = 0.0;
     dydx[2] = M[2][0]*alpha + M[2][1]*beta + M[2][2];
     return;
}


/* user storage for intermediate results.              */

/* Runge-kutta driver with adaptive stepsize control.  Integration start 
   with ystart[1..nvar] form x1 to x2 with accuracy  eps.  h1 is set as
   a guessed first stepsize.  hmin as the minimum allowed stepsize. 
   On output nok and nbad are the number of good anf bad steps taken
   and ystart is replaced by values at the end of the integration interval.
   f is the user defined routine to calculate the right-hand side of 
   the differential equation.  while rkqs is the name of the stepper 
   routine to be used.             */

void odeint(double *ystart, int nvar, double x1, double x2, double eps,
            double h1, double hmin, int *nok, int *nbad,
            void (*f)(double, double *, double *),
            void (*rkqs)(double *, double *, int, double *, double,
                double, double *, double *, double *, 
                void (*)(double, double *, double *)))
{
    int nstp, i;
    int ct = 0;
    double xsav, x, hnext, hdid, h;
/*    double *yscal, *y, *dydx;  */

    double yscal[3], y[3], dydx[3];


/*    yscal = VectorAlloc(nvar);
    y = VectorAlloc(nvar);
    dydx = VectorAlloc(nvar);   */

    x = x1;
    h = h1*(x2-x1)/fabs(x2-x1);  


/*    *nok = (*nbad) = kount = 0;             */
    for(i=0; i<nvar; i++) y[i] = ystart[i];
/*    if(kmax > 0) xsav = x-dxsav*2.0;        */       

    for(nstp = 1; nstp <= MAXSTP; nstp++) {
         (*f)(x,y,dydx);
         for(i=0; i<nvar; i++) {
               yscal[i] = fabs(y[i]) + fabs(dydx[i]*h) + TINY;   
            }

/* Followed are codes used for debugging,  which are taken out for efficiency
   in the final version                                                   */

/*         if (kmax>0 && kount<kmax-1 && fabs(x-xsav)>fabs(dxsav)) {
               xp[kount] = x;
               for(i=0;i<nvar;i++) yp[kount][i]=y[i];
               kount++;
               xsav = x;
           }          */  /* Debugging codes end here   */        

         if((x+h-x2)*(x+h-x1)>0.0) h=x2-x;

       if(STICK)
         (*rkqs)(y, dydx, nvar, &x, h, eps, yscal, &hdid, &hnext, f);
       else {
         (*rkqs)(y, dydx, nvar, &x, h, eps, yscal, &hdid, &hnext, f);    

         if((y[X]*y[X]+y[Y]*y[Y]) < 0.00001*status.gravity*COL_COLLIDING_DISTANCE) {
              alpha = (M[1][2]*M[0][1]-M[0][2]*M[1][1])/(M[0][0]*M[1][1] - M[1][0]*M[0][1]);
              beta = (M[0][2]*M[1][0]-M[1][2]*M[0][0])/(M[0][0]*M[1][1] - M[1][0]*M[0][1]);
              if((alpha*alpha + beta*beta) <= Frict*Frict) {
                  STICK = 1;
              printf("odeint: sticking happens!\n");   
                }
              else {
                  if((y[X]*y[X]+y[Y]*y[Y]) < 0.0000001 && !FLAG) {
                       FLAG = 1;   
                    printf("ODEINT:possible sticking not happens!MORE CODE\n");
                      }
                   }
            }
       }

/*         if(hdid == h) ++(*nok); else ++(*nbad);       */
         if((x-x2)*(x2-x1) >= 0.0) {
             for(i=0;i<nvar;i++) ystart[i] = y[i];

/*             if(kmax > ++kount) {
               xp[kount] = x;
               for(i=0; i<nvar; i++) {
                     yp[kount][i] = y[i];
                     }
              }    */                           

/*             VectorFree(nvar, dydx);
             VectorFree(nvar, y);
             VectorFree(nvar, yscal);   */

             return;
          }
          if(fabs(hnext) <= hmin) { 
              printf("Step size too small in odeint\n");
              if(ct++ > 10) return;
              }
          else
              h=hnext;
    }
    rkerror("Too many steps in routine odeint");
}


double  ODEINT(double *ystart, int nvar, double x1, double x2, double eps,
            double h1, double hmin, int *nok, int *nbad,
            void (*f)(double, double *, double *),
            void (*rkqs)(double *, double *, int, double *, double,
                double, double *, double *, double *,
                void (*)(double, double *, double *)))
{
    int nstp, i;
    int ct = 0;
    double xsav, x, hnext, hdid, h;
    double yscal[3], y[3], dydx[3];
    double lastyz = -1.0, lastx;

/*    double *yscal, *y, *dydx;   */

/*    yscal = VectorAlloc(nvar);
    y = VectorAlloc(nvar);
    dydx = VectorAlloc(nvar);   */

    x = x1;
    h = h1*(x2-x1)/fabs(x2-x1);


/*    *nok = (*nbad) = kount = 0;   */
    for(i=0; i<nvar; i++) y[i] = ystart[i];

/*    if(kmax > 0) xsav = x-dxsav*2.0;   */

    for(nstp = 1; nstp <= MAXSTP; nstp++) {
         (*f)(x,y,dydx);
         for(i=0; i<nvar; i++) {
               yscal[i] = fabs(y[i]) + fabs(dydx[i]*h) + TINY;   
            }

/*         if (kmax>0 && kount<kmax-1 && fabs(x-xsav)>fabs(dxsav)) {
               xp[kount] = x;
               for(i=0;i<nvar;i++) yp[kount][i]=y[i];
               kount++;
               xsav = x;
           }    */

         if((x+h-x2)*(x+h-x1)>0.0) h=x2-x;

       if(STICK)
         (*rkqs)(y, dydx, nvar, &x, h, eps, yscal, &hdid, &hnext, f);
       else {
         (*rkqs)(y, dydx, nvar, &x, h, eps, yscal, &hdid, &hnext, f);   


         if((y[X]*y[X]+y[Y]*y[Y]) < 0.00001*status.gravity*COL_COLLIDING_DISTANCE) {
              alpha = (M[1][2]*M[0][1]-M[0][2]*M[1][1])/(M[0][0]*M[1][1] - M[1][0]*M[0][1]);
              beta = (M[0][2]*M[1][0]-M[1][2]*M[0][0])/(M[0][0]*M[1][1] - M[1][0]*M[0][1]);
              if((alpha*alpha + beta*beta) < Frict*Frict) {
         printf("from ODEINT, sticking happens.\n");
                  STICK = 1;
               }
              else {
                  if((y[X]*y[X]+y[Y]*y[Y]) < 0.0000001 && !FLAG) {
                       FLAG = 1;   
                    printf("ODEINT:possible sticking not happens!MORE CODE\n");
                      }
                   }
             }
 
         }

/*         if(hdid == h) ++(*nok); else ++(*nbad);     */
         if(lastyz*y[Z] < 0.0 || lastyz*y[Z] == 0.0 ) {
             for(i=0;i<nvar;i++) ystart[i] = y[i];

/*             if(kmax > ++kount) {
               xp[kount] = x;
               for(i=0; i<nvar; i++) yp[kount][i] = y[i];
              }                     */

/*             VectorFree(nvar, dydx);
             VectorFree(nvar, y);
             VectorFree(nvar, yscal);     */

             return x;
          }
          if(fabs(hnext) <= hmin) {
              printf("Step size too small in ODEINT");
              h=100*hnext;
              if(ct++ > 10) return;
              }
          else
              h=hnext;
          lastyz = y[Z];
          lastx = x;
    }
    rkerror("Too many steps in routine ODEINT");
}


void  cal_vel_change(col_Mat3 Mat, col_Vect3 Delta_v,
                     double frict, double restit)
{
   double tmp_x, x1, x2, eps, epsmin, h1, hmin, inc;
   int i, j, n, nbad, nok;

   __col_mat3Copy(Mat, M);
   Frict = frict;

   x1 = 0.0;
   STICK = 0;

   epsmin = FindMachEps();    
/*   epsmin = 0.0000000000000001;  */
   eps = epsmin * 100000;
   h1 = eps*100000;      
   hmin = eps;
   n = 3;
  

 /*  kmax = 1000;  */
   kmax = 0;  
   dxsav = 0.01;
/*   xp = VectorAlloc(kmax);
   yp = MatrixAlloc(kmax, kmax);   */

      x2 = x1 + 5000;
      tmp_x = ODEINT(Delta_v, n, x1, x2, eps, h1, hmin, &nok, &nbad,
              &f, &rkqs);

      x2 = (1 + restit) * tmp_x;       
      odeint(Delta_v, n, tmp_x, x2, eps, h1, hmin, &nok, &nbad,
              &f, &rkqs);
/*
      VectorFree(kmax, xp);
      MatrixFree(kmax, yp);    */

}