/*
Generated 30-Jun-2008 20:50:44 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 srssrroot. */

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

    for (i = 0; i < 5; i++) {
        vel[i] = 0.;
    }
    srssrang2st(vars,pos);
    srssrstate(param[0],pos,vel);
    srssrperr(resid);
}

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

    srssrstate(param[5],param,vars);
    srssrverr(resid);
}

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

    srssrang2st(vars,pos);
    srssrstate(param[5],pos,param);
    srssruforce(param[5],pos,param);
    srssrperr(resid);
    srssrderiv(qdotdum,&resid[0]);
}

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

    srssrang2st(vars,pos);
    srssrstate(param[0],pos,&vars[5]);
    srssruforce(param[0],pos,&vars[5]);
    srssrperr(resid);
    srssrverr(&resid[0]);
    srssrderiv(qdotdum,&resid[0]);
}

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

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

    srssrstate(time,state,&state[5]);
    srssruforce(time,state,&state[5]);
    srssrderiv(dstate,&dstate[5]);
    *status = 0;
}

/* This routine performs assembly analysis. */

void srssrassemble(double time,
    double state[10],
    int lock[5],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double perrs[1],param[1];
    int i;

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(50,42);
    }
    param[0] = time;
    *err = 0;
    *fcnt = 0;
    srssrposfunc(state,param,perrs);
    *fcnt = *fcnt+1;
}

/* This routine performs initial velocity analysis. */

void srssrinitvel(double time,
    double state[10],
    int lock[5],
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double verrs[1],param[6];
    int i;

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(51,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[i];
    }
    param[5] = time;
    *err = 0;
    *fcnt = 0;
    srssrvelfunc(&state[5],param,verrs);
    *fcnt = *fcnt+1;
}

/* This routine performs static analysis. */

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

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(52,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[5+i];
    }
    param[5] = time;
    srssrroot(srssrstatfunc,state,param,5,5,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srssrstatfunc(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 srssrsteady(double time,
    double state[10],
    int lock[10],
    double ctol,
    double tol,
    int maxevals,
    int *fcnt,
    int *err)
{
    double resid[5],param[1];
    double jw[50],dw[450],rw[125];
    int iw[60],rooterr,i;

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(53,42);
    }
    param[0] = time;
    srssrroot(srssrstdyfunc,state,param,5,10,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srssrstdyfunc(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 srssrmotion(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;

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(54,42);
    }
    param[0] = ctol;
    ttime = *time;
    if (*flag != 0) {
        srssrmotfunc(ttime,state,dstate,param,&ferr);
        step = dt;
        *flag = 0;
    }
    if (step <= 0.) {
        step = dt;
    }
    srssrvinteg(srssrmotfunc,&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 srssrfmotion(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;

    srssrgentime(&i);
    if (i != 205044) {
        srssrseterr(55,42);
    }
    param[0] = ctol;
    *err = 0;
    ttime = *time;
    if (*flag != 0) {
        srssrmotfunc(ttime,state,dstate,param,&ferr);
        *flag = 0;
    }
    srssrfinteg(srssrmotfunc,&ttime,state,dstate,param,dt,10,work,errest,&ferr);
    if (ferr != 0) {
        *err = 1;
    }
    *time = ttime;
}
