/*
Generated 24-Jun-2008 21:10:41 by SD/FAST, Kane's formulation
(sdfast B.2.8 #30123) on machine ID unknown
Copyright (c) 1990-1997 Symbolic Dynamics, Inc.
Copyright (c) 1990-1997 Parametric Technology Corp.
RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S.
Government is subject to restrictions as set forth in subparagraph
(c)(1)(ii) of the Rights in Technical Data and Computer Software
clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA
FAR Supplement.  Symbolic Dynamics, Inc., Mountain View, CA 94041
*/
#include <math.h>

/* These routines are passed to srdsroot. */

void srdsposfunc(double vars[5],
    double param[1],
    double resid[5])
{
    int i;
    double pos[5],vel[5];

    for (i = 0; i < 5; i++) {
        vel[i] = 0.;
    }
    srdsang2st(vars,pos);
    srdsstate(param[0],pos,vel);
    srdsperr(resid);
}

void srdsvelfunc(double vars[5],
    double param[6],
    double resid[5])
{

    srdsstate(param[5],param,vars);
    srdsverr(resid);
}

void srdsstatfunc(double vars[5],
    double param[6],
    double resid[10])
{
    double pos[5],qdotdum[5];

    srdsang2st(vars,pos);
    srdsstate(param[5],pos,param);
    srdsuforce(param[5],pos,param);
    srdsperr(resid);
    srdsderiv(qdotdum,&resid[5]);
}

void srdsstdyfunc(double vars[10],
    double param[1],
    double resid[15])
{
    double pos[5],qdotdum[5];

    srdsang2st(vars,pos);
    srdsstate(param[0],pos,&vars[5]);
    srdsuforce(param[0],pos,&vars[5]);
    srdsperr(resid);
    srdsverr(&resid[5]);
    srdsderiv(qdotdum,&resid[10]);
}

/* This routine is passed to the integrator. */

void srdsmotfunc(double time,
    double state[10],
    double dstate[10],
    double param[1],
    int *status)
{
    double err[5];
    int i;

    srdsstate(time,state,&state[5]);
    srdsuforce(time,state,&state[5]);
    srdsderiv(dstate,&dstate[5]);
    *status = 1;
    srdsverr(err);
    for (i = 0; i < 5; i++) {
        if (fabs(err[i]) > param[0]) {
            return;
        }
    }
    srdsperr(err);
    for (i = 0; i < 5; i++) {
        if (fabs(err[i]) > param[0]) {
            return;
        }
    }
    *status = 0;
}

/* This routine performs assembly analysis. */

void srdsassemble(double time,
    double state[10],
    int lock[5],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double perrs[5],param[1];
    int i;
    double jw[25],dw[200],rw[80];
    int iw[40],rooterr;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(50,42);
    }
    param[0] = time;
    srdsroot(srdsposfunc,state,param,5,5,0,lock,tol,tol,maxevals,
      jw,dw,rw,iw,perrs,fcnt,&rooterr);
    srdsposfunc(state,param,perrs);
    *fcnt = *fcnt+1;
    if (rooterr == 0) {
        *err = 0;
    } else {
        if (*fcnt >= maxevals) {
            *err = 2;
        } else {
            *err = 1;
        }
    }
}

/* This routine performs initial velocity analysis. */

void srdsinitvel(double time,
    double state[10],
    int lock[5],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double verrs[5],param[6];
    int i;
    double jw[25],dw[200],rw[80];
    int iw[40],rooterr;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(51,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[i];
    }
    param[5] = time;
    srdsroot(srdsvelfunc,&state[5],param,5,5,0,lock,tol,tol,maxevals,
      jw,dw,rw,iw,verrs,fcnt,&rooterr);
    srdsvelfunc(&state[5],param,verrs);
    *fcnt = *fcnt+1;
    if (rooterr == 0) {
        *err = 0;
    } else {
        if (*fcnt >= maxevals) {
            *err = 2;
        } else {
            *err = 1;
        }
    }
}

/* This routine performs static analysis. */

void srdsstatic(double time,
    double state[10],
    int lock[5],
    double ctol,
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double resid[10],param[6],jw[50],dw[450],rw[115];
    int iw[60],rooterr,i;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(52,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[5+i];
    }
    param[5] = time;
    srdsroot(srdsstatfunc,state,param,10,5,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srdsstatfunc(state,param,resid);
    *fcnt = *fcnt+1;
    if (rooterr == 0) {
        *err = 0;
    } else {
        if (*fcnt >= maxevals) {
            *err = 2;
        } else {
            *err = 1;
        }
    }
}

/* This routine performs steady motion analysis. */

void srdssteady(double time,
    double state[10],
    int lock[10],
    double ctol,
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double resid[15],param[1];
    double jw[150],dw[1250],rw[195];
    int iw[100],rooterr,i;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(53,42);
    }
    param[0] = time;
    srdsroot(srdsstdyfunc,state,param,15,10,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srdsstdyfunc(state,param,resid);
    *fcnt = *fcnt+1;
    if (rooterr == 0) {
        *err = 0;
    } else {
        if (*fcnt >= maxevals) {
            *err = 2;
        } else {
            *err = 1;
        }
    }
}

/* This routine performs state integration. */

void srdsmotion(double *time,
    double state[10],
    double dstate[10],
    double dt,
    double ctol,
    double tol,
    int *flag,
    int *err)
{
    static double step;
    double work[60],ttime,param[1];
    int vintgerr,which,ferr,i;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(54,42);
    }
    param[0] = ctol;
    ttime = *time;
    if (*flag != 0) {
        srdsmotfunc(ttime,state,dstate,param,&ferr);
        step = dt;
        *flag = 0;
    }
    if (step <= 0.) {
        step = dt;
    }
    srdsvinteg(srdsmotfunc,&ttime,state,dstate,param,dt,&step,10,tol,work,&
      vintgerr,&which);
    *time = ttime;
    *err = vintgerr;
}

/* This routine performs state integration with a fixed-step integrator. */

void srdsfmotion(double *time,
    double state[10],
    double dstate[10],
    double dt,
    double ctol,
    int *flag,
    double *errest,
    int *err)
{
    double work[40],ttime,param[1];
    int ferr,i;

    srdsgentime(&i);
    if (i != 211041) {
        srdsseterr(55,42);
    }
    param[0] = ctol;
    *err = 0;
    ttime = *time;
    if (*flag != 0) {
        srdsmotfunc(ttime,state,dstate,param,&ferr);
        *flag = 0;
    }
    srdsfinteg(srdsmotfunc,&ttime,state,dstate,param,dt,10,work,errest,&ferr);
    if (ferr != 0) {
        *err = 1;
    }
    *time = ttime;
}
