/*
 *  pendulum_taylor7.c
 *
 *  C file with functions for computing the Taylor series approximate solution
 *  for the vector field named: pendulum
 *
 *  This file was generated by the program VFGEN (Version:2.4.0)
 *  Generated on 10-Jul-2008 at 17:56
 */

#include <math.h>
#include "pendulum_taylor7.h"

/*
 *  The vector field.
 */

void pendulum_vf(double t, const double y_[], double f_[], double params[])
    {
    double theta, v;
    double g, b, L, m;
    double *p_;

    p_ = (double *) params;

    theta      = y_[0];
    v          = y_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    f_[0] = v;
    f_[1] = -1.0/m*v/(L*L)*b-sin(theta)*g/L;

    }


/*
 *     deriv = Df(x)[v1]
 */
void pendulum_diff1(double deriv[],double x_[], double p_[],double v1_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    double vf_v = 1.0;
    deriv[0] = 0.0;
    deriv[0] += vf_v * v1_[1];
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta = -g/L*cos(theta);
    double vf_v = -1.0/m/(L*L)*b;
    deriv[1] = 0.0;
    deriv[1] += vf_theta * v1_[0];
    deriv[1] += vf_v * v1_[1];
    }

    return;
    }

/*
 *     deriv = D^2f(x)[v1,v2]
 */
void pendulum_diff2(double deriv[],double x_[], double p_[],double v1_[],double v2_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    deriv[0] = 0.0;
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta_theta = sin(theta)*g/L;
    deriv[1] = 0.0;
    deriv[1] += vf_theta_theta * v1_[0]*v2_[0];
    }

    return;
    }

/*
 *     deriv = D^3f(x)[v1,v2,v3]
 */
void pendulum_diff3(double deriv[],double x_[], double p_[],double v1_[],double v2_[],double v3_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    deriv[0] = 0.0;
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta_theta_theta = g/L*cos(theta);
    deriv[1] = 0.0;
    deriv[1] += vf_theta_theta_theta * v1_[0]*v2_[0]*v3_[0];
    }

    return;
    }

/*
 *     deriv = D^4f(x)[v1,...,v4]
 */
void pendulum_diff4(double deriv[],double x_[], double p_[],double v1_[],double v2_[],double v3_[],double v4_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    deriv[0] = 0.0;
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta_theta_theta_theta = -sin(theta)*g/L;
    deriv[1] = 0.0;
    deriv[1] += vf_theta_theta_theta_theta * v1_[0]*v2_[0]*v3_[0]*v4_[0];
    }

    return;
    }

/*
 *     deriv = D^5f(x)[v1,...,v5]
 */
void pendulum_diff5(double deriv[],double x_[], double p_[],double v1_[],double v2_[],double v3_[],double v4_[],double v5_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    deriv[0] = 0.0;
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta_theta_theta_theta_theta = -g/L*cos(theta);
    deriv[1] = 0.0;
    deriv[1] += vf_theta_theta_theta_theta_theta * v1_[0]*v2_[0]*v3_[0]*v4_[0]*v5_[0];
    }

    return;
    }

/*
 *     deriv = D^6f(x)[v1,...,v6]
 */
void pendulum_diff6(double deriv[],double x_[], double p_[],double v1_[],double v2_[],double v3_[],double v4_[],double v5_[],double v6_[])
    {
    double theta, v;
    double g, b, L, m;

    theta      = x_[0];
    v          = x_[1];

    g          = p_[0];
    b          = p_[1];
    L          = p_[2];
    m          = p_[3];

    {
    /*
     *  Partial derivatives of vf[0]. 
     *  Any derivative not listed here is zero.
     */
    deriv[0] = 0.0;
    }
    {
    /*
     *  Partial derivatives of vf[1]. 
     *  Any derivative not listed here is zero.
     */
    double vf_theta_theta_theta_theta_theta_theta = sin(theta)*g/L;
    deriv[1] = 0.0;
    deriv[1] += vf_theta_theta_theta_theta_theta_theta * v1_[0]*v2_[0]*v3_[0]*v4_[0]*v5_[0]*v6_[0];
    }

    return;
    }

/*
 *  pendulum_derivs7
 *
 *  Compute the coefficients in the Taylor polynomial at X.
 *  These are just the derivatives; they have not been scaled
 *  by the appropriate factorial.
 *
 */

void pendulum_derivs7(double Xderiv[7][2], double X[], double params[])
    {
    int i;
    double s;
    double Q[2];

    pendulum_vf(0.0,X,Xderiv[0],params);

    for (i = 0; i < 2; ++i)
        Xderiv[1][i] = 0.0;
    /*    [ 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[1][i] += Q[i];

    for (i = 0; i < 2; ++i)
        Xderiv[2][i] = 0.0;
    /*    [ 0 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[2][i] += Q[i];
    /*    [ 2]  coeff = 1.  */
    pendulum_diff2(Q,X,params,Xderiv[0],Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[2][i] += Q[i];

    for (i = 0; i < 2; ++i)
        Xderiv[3][i] = 0.0;
    /*    [ 0 0 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[3][i] += Q[i];
    /*    [ 1 1]  coeff = 3.  */
    pendulum_diff2(Q,X,params,Xderiv[0],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[3][i] += 3.*Q[i];
    /*    [ 3]  coeff = 1.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[3][i] += Q[i];

    for (i = 0; i < 2; ++i)
        Xderiv[4][i] = 0.0;
    /*    [ 0 0 0 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[3]);
    for (i = 0; i < 2; ++i)
        Xderiv[4][i] += Q[i];
    /*    [ 1 0 1]  coeff = 4.  */
    pendulum_diff2(Q,X,params,Xderiv[0],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[4][i] += 4.*Q[i];
    /*    [ 0 2]  coeff = 3.  */
    pendulum_diff2(Q,X,params,Xderiv[1],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[4][i] += 3.*Q[i];
    /*    [ 2 1]  coeff = 6.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[4][i] += 6.*Q[i];
    /*    [ 4]  coeff = 1.  */
    pendulum_diff4(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[4][i] += Q[i];

    for (i = 0; i < 2; ++i)
        Xderiv[5][i] = 0.0;
    /*    [ 0 0 0 0 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[4]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += Q[i];
    /*    [ 1 0 0 1]  coeff = 5.  */
    pendulum_diff2(Q,X,params,Xderiv[0],Xderiv[3]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += 5.*Q[i];
    /*    [ 0 1 1]  coeff = 10.  */
    pendulum_diff2(Q,X,params,Xderiv[1],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += 10.*Q[i];
    /*    [ 2 0 1]  coeff = 10.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += 10.*Q[i];
    /*    [ 1 2]  coeff = 15.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[1],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += 15.*Q[i];
    /*    [ 3 1]  coeff = 10.  */
    pendulum_diff4(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += 10.*Q[i];
    /*    [ 5]  coeff = 1.  */
    pendulum_diff5(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[5][i] += Q[i];

    for (i = 0; i < 2; ++i)
        Xderiv[6][i] = 0.0;
    /*    [ 0 0 0 0 0 1]  coeff = 1.  */
    pendulum_diff1(Q,X,params,Xderiv[5]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += Q[i];
    /*    [ 1 0 0 0 1]  coeff = 6.  */
    pendulum_diff2(Q,X,params,Xderiv[0],Xderiv[4]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 6.*Q[i];
    /*    [ 0 1 0 1]  coeff = 15.  */
    pendulum_diff2(Q,X,params,Xderiv[1],Xderiv[3]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 15.*Q[i];
    /*    [ 2 0 0 1]  coeff = 15.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[3]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 15.*Q[i];
    /*    [ 0 0 2]  coeff = 10.  */
    pendulum_diff2(Q,X,params,Xderiv[2],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 10.*Q[i];
    /*    [ 1 1 1]  coeff = 60.  */
    pendulum_diff3(Q,X,params,Xderiv[0],Xderiv[1],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 60.*Q[i];
    /*    [ 3 0 1]  coeff = 20.  */
    pendulum_diff4(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[2]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 20.*Q[i];
    /*    [ 0 3]  coeff = 15.  */
    pendulum_diff3(Q,X,params,Xderiv[1],Xderiv[1],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 15.*Q[i];
    /*    [ 2 2]  coeff = 45.  */
    pendulum_diff4(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[1],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 45.*Q[i];
    /*    [ 4 1]  coeff = 15.  */
    pendulum_diff5(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[1]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += 15.*Q[i];
    /*    [ 6]  coeff = 1.  */
    pendulum_diff6(Q,X,params,Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0],Xderiv[0]);
    for (i = 0; i < 2; ++i)
        Xderiv[6][i] += Q[i];
    }

/*
 *  pendulum_evaltaylor7
 *
 *  Use the Taylor method to approximate X(t+h) given X(t).
 *  This function uses a Taylor polynomial of order 7.
 *  pendulum_derivs7(...) must be called first to fill in the array Xderiv.
 */

void pendulum_evaltaylor7(double Xnew[], double h, double X[], double Xderiv[7][2])
    {
    int i;
    double s;
    /*  Xnew = X  */
    for (i = 0; i < 2; ++i)
        Xnew[i] = X[i];
    s = h;
    /* Add order 1 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/1.0)*Xderiv[0][i]*s;
    s = s * h;
    /* Add order 2 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/2.0)*Xderiv[1][i]*s;
    s = s * h;
    /* Add order 3 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/6.0)*Xderiv[2][i]*s;
    s = s * h;
    /* Add order 4 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/24.0)*Xderiv[3][i]*s;
    s = s * h;
    /* Add order 5 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/120.0)*Xderiv[4][i]*s;
    s = s * h;
    /* Add order 6 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/720.0)*Xderiv[5][i]*s;
    s = s * h;
    /* Add order 7 term to Xnew */
    for (i = 0; i < 2; ++i)
        Xnew[i] += (1.0/5040.0)*Xderiv[6][i]*s;
    }