/*
Generated 24-Jun-2008 21:10:06 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 srssroot. */

void srssposfunc(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.;
    }
    srssang2st(vars,pos);
    srssstate(param[0],pos,vel);
    srssperr(resid);
}

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

    srssstate(param[5],param,vars);
    srssverr(resid);
}

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

    srssang2st(vars,pos);
    srssstate(param[5],pos,param);
    srssuforce(param[5],pos,param);
    srssperr(resid);
    srssderiv(qdotdum,&resid[0]);
}

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

    srssang2st(vars,pos);
    srssstate(param[0],pos,&vars[5]);
    srssuforce(param[0],pos,&vars[5]);
    srssperr(resid);
    srssverr(&resid[0]);
    srssderiv(qdotdum,&resid[0]);
}

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

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

    srssstate(time,state,&state[5]);
    srssuforce(time,state,&state[5]);
    srssderiv(dstate,&dstate[5]);
    *status = 0;
}

/* This routine performs assembly analysis. */

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

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(50,42);
    }
    param[0] = time;
    *err = 0;
    *fcnt = 0;
    srssposfunc(state,param,perrs);
    *fcnt = *fcnt+1;
}

/* This routine performs initial velocity analysis. */

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

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(51,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[i];
    }
    param[5] = time;
    *err = 0;
    *fcnt = 0;
    srssvelfunc(&state[5],param,verrs);
    *fcnt = *fcnt+1;
}

/* This routine performs static analysis. */

void srssstatic(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;

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(52,42);
    }
    for (i = 0; i < 5; i++) {
        param[i] = state[5+i];
    }
    param[5] = time;
    srssroot(srssstatfunc,state,param,5,5,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srssstatfunc(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 srsssteady(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;

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(53,42);
    }
    param[0] = time;
    srssroot(srssstdyfunc,state,param,5,10,5,lock,
      ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr);
    srssstdyfunc(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 srssmotion(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;

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(54,42);
    }
    param[0] = ctol;
    ttime = *time;
    if (*flag != 0) {
        srssmotfunc(ttime,state,dstate,param,&ferr);
        step = dt;
        *flag = 0;
    }
    if (step <= 0.) {
        step = dt;
    }
    srssvinteg(srssmotfunc,&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 srssfmotion(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;

    srssgentime(&i);
    if (i != 211006) {
        srssseterr(55,42);
    }
    param[0] = ctol;
    *err = 0;
    ttime = *time;
    if (*flag != 0) {
        srssmotfunc(ttime,state,dstate,param,&ferr);
        *flag = 0;
    }
    srssfinteg(srssmotfunc,&ttime,state,dstate,param,dt,10,work,errest,&ferr);
    if (ferr != 0) {
        *err = 1;
    }
    *time = ttime;
}
