// PROGRAM poincare
// plot phase diagram and Poincare map for damped, driven pendulum

#include "TrueBASIC.h"
int main();
void initial(double *theta0, double *ang_vel0, double *gamma, double *A, 
             double *t0, double *dt, int *nstep, double *dA, int *IC1_, 
             int *IC2_, char* flag);
void set_up_windows(double A, int IC1_, int IC2_);
void draw_axes(double A, int IC1_, int IC2_);
void RK4(double *theta, double *ang_vel, double gamma, double A, double *t, 
         double dt);
double force(double theta, double ang_vel, double gamma, double A, double t);
void phase_space(double theta, double ang_vel, int IC9_);
void change_amplitude(double *A, double dA, int IC1_, int IC2_, 
                      char* flag);
void Poincare(double theta, double ang_vel, int IC9_);

int main()
{
    int IC1_ = 1, IC2_ = 2, istep, nstep;
    double A, ang_vel, dA, dt, gamma, t, theta;
    char flag[_LBUFF_];

    GWopen(0);

    initial(&theta, &ang_vel, &gamma, &A, &t, &dt, &nstep, &dA, &IC1_, &IC2_, 
            flag);
    set_up_windows(A, IC1_, IC2_);
    draw_axes(A, IC1_, IC2_);
    do
    {
        for(istep = 1; istep <= nstep; ++istep)
        {
            RK4(&theta, &ang_vel, gamma, A, &t, dt);
            phase_space(theta, ang_vel, IC1_);
        }
        Poincare(theta, ang_vel, IC2_);
        if(TBkeyinput())
            change_amplitude(&A, dA, IC1_, IC2_, flag);
    } while(!(strcmp(flag, "stop") == 0));

    GWquit();
    return 0;
}

void initial(double *theta0, double *ang_vel0, double *gamma, double *A, 
             double *t0, double *dt, int *nstep, double *dA, int *IC1_, 
             int *IC2_, char* flag)
{
    double T;
    char Stmp1_[_LBUFF_];

    printf("initial angle = ");
    fgets(Stmp1_, _LBUFF_, stdin);
    sscanf(Stmp1_, "%lg", theta0);
    printf("initial angular velocity = ");
    fgets(Stmp1_, _LBUFF_, stdin);
    sscanf(Stmp1_, "%lg", ang_vel0);
    (*t0) = 0;                              // initial time
    printf("damping constant = ");
    fgets(Stmp1_, _LBUFF_, stdin);
    sscanf(Stmp1_, "%lg", gamma);
    printf("amplitude of external force = ");
    fgets(Stmp1_, _LBUFF_, stdin);
    sscanf(Stmp1_, "%lg", A);
    // angular frequency of external force equals two and
    // hence period of external force equals pi
    (*nstep) = 100;             // # iterations between Poincare plot
    T = pi;                                 // period of external force
    (*dt) = T/(*nstep);                     // time step
    (*dA) = 0.01;                           // increase in amplitude of external force
    // left-side of screen for phase space plot
    GWvport(0.0f, 0.0f, 0.49f*GWaspect(1), 1.0f);
    // open second window for Poincare map
    GWsavevp(*IC1_);
    GWsetogn(0, *IC1_);
    GWvport(0.51f*GWaspect(1), 0.0f, GWaspect(1), 1.0f);
    strcpy(flag, "");
    GWsavevp(*IC2_);
    GWsetogn(0, *IC2_);
}

void set_up_windows(double A, int IC1_, int IC2_)
{
    double vmax;

    vmax = 4*A;
    GWselvp(IC1_);
    GWsetogn(0, IC1_);
    GWindow((float)(-pi), (float)(-vmax), (float)(pi), (float)(vmax));
    GWselvp(IC2_);
    GWsetogn(0, IC2_);
    GWindow((float)(-pi), (float)(-vmax), (float)(pi), (float)(vmax));
}

void draw_axes(double A, int IC1_, int IC2_)
{
    float vmax;
    char Stmp1_[_LBUFF_];

    GWclear(-1);
    GWselvp(IC1_);
    GWsetogn(0, IC1_);
    GWerase(IC1_, -1);
    GWsetpen(BLACK, -1, -1, -1);
    vmax = (float)(4*A);
    GWline(0.0f, -vmax, 0.0f, vmax);
    GWline((float)(-pi), 0.0f, (float)(pi), 0.0f);
    sprintf(Stmp1_, "A = %7.5f", A);
    GWputtxt(-3.0f, vmax*22.5f/24, Stmp1_);
    GWselvp(IC2_);
    GWsetogn(0, IC2_);
    GWerase(IC2_, -1);
    GWsetpen(BLACK, -1, -1, -1);
    GWline((float)(-pi), 0.0f, (float)(pi), 0.0f);
    GWline(0.0f, -vmax, 0.0f, vmax);
}

void RK4(double *theta, double *ang_vel, double gamma, double A, double *t, 
         double dt)
// fourth-order Runge Kutta algorithm
{
    double k1v, k1x, k2v, k2x, k3v, k3x, k4v, k4x;

    k1v = force(*theta, *ang_vel, gamma, A, *t)*dt;
    k1x = (*ang_vel)*dt;
    (*t) += 0.5*dt;
    k2v = force((*theta)+0.5*k1x, (*ang_vel)+0.5*k1v, gamma, A, *t)*dt;
    k2x = ((*ang_vel) + 0.5*k1v)*dt;
    k3v = force((*theta)+0.5*k2x, (*ang_vel)+0.5*k2v, gamma, A, *t)*dt;
    k3x = ((*ang_vel) + 0.5*k2v)*dt;
    (*t) += 0.5*dt;
    k4v = force((*theta)+k3x, (*ang_vel)+k3v, gamma, A, *t)*dt;
    k4x = ((*ang_vel) + k3v)*dt;
    (*ang_vel) += (k1v + 2*k2v + 2*k3v + k4v)/6;
    (*theta) += (k1x + 2*k2x + 2*k3x + k4x)/6;
    if((*theta) > pi)
        (*theta) -= 2*pi;
    if((*theta) < -pi)
        (*theta) += 2*pi;
}

double force(double theta, double ang_vel, double gamma, double A, double t)
{
    double force_;

    // A is amplitude of driving force
    force_ = -gamma*ang_vel - (1 +2*A*cos(2*t))*sin(theta);
    return force_;
}

void phase_space(double theta, double ang_vel, int IC9_)
{
    GWselvp(IC9_);
    GWsetogn(0, IC9_);
    GWsetpxl((float)(theta), (float)(ang_vel), RED);
}

void change_amplitude(double *A, double dA, int IC1_, int IC2_, 
                      char* flag)
{
    int k = TBgetkey();
    if(k == 's')                            // press any key to clear screen
        strcpy(flag, "stop");
    if(k == 'i')
        (*A) += dA;
    if(k == 'd')
        (*A) -= dA;
    if(k == 'i' || k == 'd')
        set_up_windows(*A, IC1_, IC2_);
    if(strcmp(flag, "") == 0)
        draw_axes(*A, IC1_, IC2_);
}

void Poincare(double theta, double ang_vel, int IC9_)
{
    // plot points and allow for change in amplitude of external force
    GWselvp(IC9_);
    GWsetogn(0, IC9_);
    GWsetpxl((float)(theta), (float)(ang_vel), BLUE);
}