/*
 * ball-bound-glsc.cpp --- Eigen を使ってはずむボールのシミュレーション
 *   http://nalab.mind.meiji.ac.jp/~mk/program/misc/ball-bound-glsc.cpp
 *   cglsc++ とか作れば良いのだけど
 *   c++ -I ~/include -I/opt/X11/include ball-bound-glsc.cpp -L ~/lib -lglscd -L/opt/X11/lib -lX11
 *   c++ -I/opt/X11/include ball-bound-glsc.cpp -lglscd -L/opt/X11/lib -lX11
 *
 *   ./a.out
 */

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
using namespace Eigen;

// GLSCのヘッダーを読み込む
#ifndef G_DOUBLE
#define G_DOUBLE
#endif
extern "C" {
#include <glsc.h>
};

double m, g, Gamma, e;

VectorXd f(double t, VectorXd x)
{
  VectorXd y(4);
  y(0) = x(2);
  y(1) = x(3);
  y(2) = - Gamma / m * x(2);
  y(3) = - g - Gamma / m * x(3);
  return y;
}

int main(void)
{
  int n, N;
  double tau, Tmax, t,pi;
  VectorXd x(4),k1(4),k2(4),k3(4),k4(4);

  pi = 4 * atan(1.0);
  m = 100;
  g = 9.8;
  Gamma = 1.0;
  e = 1.0;

  Tmax = 20;
  N = 1000;
  tau = Tmax / N;
  x << 0,0,50*cos(pi*50/180),50*sin(pi*50/180);

 /* グラフィックス・ライブラリィ GLSC の呼び出し */
  g_init((char *) "Meta", 250.0, 160.0);
  g_device(G_BOTH);
  g_def_scale(0, 0.0, 600.0, 0.0, 100.0, 10.0, 10.0, 230.0, 140.0);
  g_def_line(0, G_BLACK, 0, G_LINE_SOLID);
  g_sel_scale(0);
  g_sel_line(0);
  g_cls();
  g_move(x(0),x(1));

  for (n = 0; n < N; n++) {
    t = n * tau;
    k1 = tau * f(t, x);
    k2 = tau * f(t+tau/2, x+k1/2);
    k3 = tau * f(t+tau/2, x+k2/2);
    k4 = tau * f(t+tau, x+k3);
    x = x + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
    if (x(1)<0) {
      x(1) = - x(1);
      x(3) = - x(3);
    }
    std::cout << x(0) << " " << x(1) << std::endl;
    g_plot(x(0),x(1));
  }
  g_sleep(-1.0);
  g_term();
  return 0;
}