// PROGRAM pendula
// 線形振り子と非線形振り子のアニメーション

#include "TrueBASIC.h"
#include "csgraphics.h"

int main();
void initial(int *IC1_, int *IC2_, double *theta1, double *v1, double *theta2, 
             double *v2, double *omega2, double *t, double *dt, double *dt_2, 
             int ball[], double *r);
void linear(double *theta, double *v, double omega2, double dt, double dt_2);
void nonlinear(double *theta, double *v, double omega2, double dt, double dt_2);
void animation(int IC9_, double theta, int ball);

int main()
{
    int ball[2], IC1_ = 1, IC2_ = 2;
    double dt, dt_2, omega2, r, t, theta1, theta2, v1, v2;

    GWopen(0);

    initial(&IC1_, &IC2_, &theta1, &v1, &theta2, &v2, &omega2, &t, &dt, &dt_2, 
            ball, &r);
    do
    {
//        int i;
//        for(i = 0; i < 4; ++i) {
            linear(&theta1, &v1, omega2, dt, dt_2);
            nonlinear(&theta2, &v2, omega2, dt, dt_2);
            t += dt;
//        }
        animation(IC1_, theta1, ball[0]);
        animation(IC2_, theta2, ball[1]);
    } while(!TBkeyinput());

    GWquit();
    return 0;
}

void initial(int *IC1_, int *IC2_, double *theta1, double *v1, double *theta2, 
             double *v2, double *omega2, double *t, double *dt, double *dt_2, 
             int ball[], double *r)
{
    float xmax, xwin, ymin, ywin;

    (*t) = 0;
    (*theta1) = 0.25;                       // 線形振り子の位置(ラジアン)
    (*theta2) = (*theta1);                  // 非線形振り子の位置
    (*v1) = 0;
    (*v2) = (*v1);
    (*omega2) = 9;                          // g/L の比
    (*dt) = 0.01;
    (*dt_2) = 0.5*(*dt);
    xmax = (float)(*theta1);                // ウィンドウの大きさ
    GWvport(0.0f, 0.0f, GWaspect(1), 0.5f);
                                            // スクリーンの下半面
    compute_aspect_ratio(xmax, &xwin, &ywin);
    GWindow(-xwin, -ywin, xwin, ywin);
    GWputtxt(-xwin*0.9f, -ywin*0.9f, "linear oscillator");
    // 円の描画
    GWsetpen(RED, -1, -1, -1);
    (*r) = 0.1;                             // 円の半径
    ball[0] = GWcmbmrk(0, 6, (float)(2*(*r)), RED, -1, 4);
    GWsavevp(*IC1_);
    GWvport(0.0f, 0.5f, GWaspect(1), 1.0f);
                                            // スクリーンの上半面
    compute_aspect_ratio(xmax, &xwin, &ymin);
    GWindow(-xwin, -ywin, xwin, ywin);
    GWputtxt(-xwin*0.9f, -ywin*0.9f, "nonlinear oscillator");
    GWsetpen(BLUE, -1, -1, -1);
    ball[1] = GWcmbmrk(0, 6, (float)(2*(*r)), BLUE, -1, 4);
    GWsavevp(*IC2_);
    GWanchor(1);
}

void linear(double *theta, double *v, double omega2, double dt, double dt_2)
{
    double a, amid, theta_mid, vmid;

    a = -omega2*(*theta);
    vmid = (*v) + a*dt_2;
    theta_mid = (*theta) + (*v)*dt_2;
    amid = -omega2*theta_mid;
    (*v) += amid*dt;
    (*theta) += vmid*dt;
}

void nonlinear(double *theta, double *v, double omega2, double dt, double dt_2)
{
    double a, amid, theta_mid, vmid;

    a = -omega2*sin((*theta));
    vmid = (*v) + a*dt_2;
    theta_mid = (*theta) + (*v)*dt_2;
    amid = -omega2*sin(theta_mid);
    (*v) += amid*dt;
    (*theta) += vmid*dt;
}

void animation(int IC9_, double theta, int ball)
{
    GWselvp(IC9_);
    GWsetogn(0, -IC9_*10);
    GWputcmb(ball, (float)(theta), 0.0f, 0.0f, 0.0f, 0);
    GWflush(-IC9_*10);
}