/*
Generated 27-Jun-2008 21:07:30 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 sraroot. */

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

    for (i = 0; i < 7; i++) {
        vel[i] = 0.;
    }
    sraang2st(vars,pos);
    srastate(param[0],pos,vel);
    sraperr(resid);
}

void sravelfunc(double vars[7],
    double param[8],
    double resid[1])
{

    srastate(param[7],param,vars);
    sraverr(resid);
}

void srastatfunc(double vars[7],
    double param[8],
    double resid[7])
{
    double pos[7],qdotdum[7];

    sraang2st(vars,pos);
    srastate(param[7],pos,param);
    srauforce(param[7],pos,param);
    sraperr(resid);
    sraderiv(qdotdum,&resid[0]);
}

void srastdyfunc(double vars[14],
    double param[1],
    double resid[7])
{
    double pos[7],qdotdum[7];

    sraang2st(vars,pos);
    srastate(param[0],pos,&vars[7]);
    srauforce(param[0],pos,&vars[7]);
    sraperr(resid);
    sraverr(&resid[0]);
    sraderiv(qdotdum,&resid[0]);
}

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

void sramotfunc(double time,
    double state[14],
    double dstate[14],
    double param[1],
    int *status)
{

    srastate(time,state,&state[7]);
    srauforce(time,state,&state[7]);
    sraderiv(dstate,&dstate[7]);
    *status = 0;
}

/* This routine performs assembly analysis. */

void sraassemble(double time,
    double state[14],
    int lock[7],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double perrs[1],param[1];
    int i;

    sragentime(&i);
    if (i != 210729) {
        sraseterr(50,42);
    }
    param[0] = time;
    *err = 0;
    *fcnt = 0;
    sraposfunc(state,param,perrs);
    *fcnt = *fcnt+1;
}

/* This routine performs initial velocity analysis. */

void srainitvel(double time,
    double state[14],
    int lock[7],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double verrs[1],param[8];
    int i;

    sragentime(&i);
    if (i != 210729) {
        sraseterr(51,42);
    }
    for (i = 0; i < 7; i++) {
        param[i] = state[i];
    }
    param[7] = time;
    *err = 0;
    *fcnt = 0;
    sravelfunc(&state[7],param,verrs);
    *fcnt = *fcnt+1;
}

/* This routine performs static analysis. */

void srastatic(double time,
    double state[14],
    int lock[7],
    double ctol,
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double resid[7],param[8],jw[49],dw[392],rw[112];
    int iw[56],rooterr,i;

    sragentime(&i);
    if (i != 210729) {
        sraseterr(52,42);
    }
    for (i = 0; i < 7; i++) {
        param[i] = state[7+i];
    }
    param[7] = time;
    sraroot(srastatfunc,state,param,7,7,7,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srastatfunc(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 srasteady(double time,
    double state[14],
    int lock[14],
    double ctol,
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double resid[7],param[1];
    double jw[98],dw[882],rw[175];
    int iw[84],rooterr,i;

    sragentime(&i);
    if (i != 210729) {
        sraseterr(53,42);
    }
    param[0] = time;
    sraroot(srastdyfunc,state,param,7,14,7,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srastdyfunc(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 sramotion(double *time,
    double state[14],
    double dstate[14],
    double dt,
    double ctol,
    double tol,
    int *flag,
    int *err)
{
    static double step;
    double work[84],ttime,param[1];
    int vintgerr,which,ferr,i;

    sragentime(&i);
    if (i != 210729) {
        sraseterr(54,42);
    }
    param[0] = ctol;
    ttime = *time;
    if (*flag != 0) {
        sramotfunc(ttime,state,dstate,param,&ferr);
        step = dt;
        *flag = 0;
    }
    if (step <= 0.) {
        step = dt;
    }
    sravinteg(sramotfunc,&ttime,state,dstate,param,dt,&step,14,tol,work,&
      vintgerr,&which);
    *time = ttime;
    *err = vintgerr;
}

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

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

    sragentime(&i);
    if (i != 210729) {
        sraseterr(55,42);
    }
    param[0] = ctol;
    *err = 0;
    ttime = *time;
    if (*flag != 0) {
        sramotfunc(ttime,state,dstate,param,&ferr);
        *flag = 0;
    }
    srafinteg(sramotfunc,&ttime,state,dstate,param,dt,14,work,errest,&ferr);
    if (ferr != 0) {
        *err = 1;
    }
    *time = ttime;
}
