#include <stdio.h>
#include <string.h>
#include <math.h>
#include <float.h>

#define FLG_CONTACT_LESS (0)
#define FLG_A_TOUCH_ON_B (1)
#define FLG_A_IS_IN_B    (2)

#define SIMULATION_SECONDS (20.0)

typedef struct {
    /* 球の物理的な値 */
    double r; /* 球の半径(m) */
    double m; /* 球の質量(kg) */

    /* 球の初期状態を表す変数 */
    double x0; /* 球のx軸上の初期位置 (m) */
    double y0; /* 球のy軸上の初期位置 (m) */
    double vx0; /* 球のx軸上の初期速度 (m/s) */
    double vy0; /* 球のy軸上の初期速度 (m/s) */

    /* 球の現在の状態を表す変数 */
    double x; /* 球の中心座標のx軸上の現在地 (m) */
    double y; /* 球の中心座標のy軸上の現在地 (m) */
    double vx; /* x軸方向の現在の速度 (m/s) */
    double vy; /* y軸方向の現在の速度 (m/s) */

} sphere_object_t;

/* 正負は数学の平面を表すxy軸と一致する。
   実験1:
   物体a: 半径 1cm, 100g, 初期位置右に 10cm , 上に 0cm の状態で、
          左に 1 cm/sec の速度で進む。
   物体b: 半径 1cm, 100g, 初期位置左に 10cm , 上に 0cm の状態で、
          右に 1 cm/sec の速度で進む。
   10 秒後に物体a,b が衝突する。

   実験2: 現在は適当。
   物体a  半径 2cm, 100g, 初期位置右に 10cm , 上に 0cm の状態で、
          左に 1 cm/sec の速度で進む。
   物体b: 半径 10cm, 100g, 初期位置左に 10cm , 上に 0cm の状態で、
          右に 1 cm/sec の速度で進む。
 */

int so_init(sphere_object_t *so,
            double r, double m,
            double x0, double y0,
            double vx0, double vy0)
{
    /* 球の物理的な値 */
    so->r = r; /* 球の半径(m) */
    so->m = m; /* 球の質量(kg) */

    /* 球の初期状態を表す変数 */
    so->x0 = x0; /* 球のx軸上の初期位置 (m) */
    so->y0 = y0; /* 球のy軸上の初期位置 (m) */
    so->vx0 = vx0; /* 球のx軸上の初期速度 (m/s) */
    so->vy0 = vy0; /* 球のy軸上の初期速度 (m/s) */

    /* 球の現在の状態を表す変数 */
    so->x = x0; /* 球の中心座標のx軸上の現在地 (m) */
    so->y = y0; /* 球の中心座標のy軸上の現在地 (m) */
    so->vx = vx0; /* x軸方向の現在の速度 (m/s) */
    so->vy = vy0; /* y軸方向の現在の速度 (m/s) */

    return 0;
}

int so_view_current(sphere_object_t *so)
{
    fprintf(stdout, "                       so = %p\n", so);

    /* 球の現在の状態 */
    fprintf(stdout, " so->(r, x, y, m, vx, vy) = (%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n", so->r, so->x, so->y, so->m, so->vx, so->vy);

    return 0;
}

int so_view(sphere_object_t *so)
{
    fprintf(stdout, "                    so = %p\n", so);
    /* 球の物理的な値 */
    fprintf(stdout, "            so->(r, m) = (%.3f, %.3f)\n", so->r, so->m);

    /* 球の初期状態を表す変数 */
    fprintf(stdout, "so->(x0, y0, vx0, vy0) = (%.3f, %.3f, %.3f, %.3f)\n", so->x0, so->y0, so->vx0, so->vy0);

    /* 球の現在の状態を表す変数 */
    fprintf(stdout, "    so->(x, y, vx, vy) = (%.3f, %.3f, %.3f, %.3f)\n", so->x, so->y, so->vx, so->vy);


    return 0;
}

int so_copy(sphere_object_t *dst, sphere_object_t *src)
{
    memcpy(dst, src, sizeof(sphere_object_t));

    return 0;
}

int so_collision_detect(sphere_object_t *a, sphere_object_t *b)
{
    double distance_of_a_and_b, sum_of_a_r_and_b_r;
    double dx, dx2, dy, dy2;
    int hit_flg = FLG_CONTACT_LESS;

    sum_of_a_r_and_b_r = a->r + b->r;

    dx = a->x - b->x;
    dx2 = dx * dx;
    dy = a->y - b->y;
    dy2 = dy * dy;
    distance_of_a_and_b = sqrt(dx2 + dy2);

    fprintf(stdout, " sum_of_a_r_and_b_r = %f\n", sum_of_a_r_and_b_r);
    fprintf(stdout, "distance_of_a_and_b = %f\n", distance_of_a_and_b);
    if (fabs(distance_of_a_and_b - sum_of_a_r_and_b_r) < DBL_EPSILON){
        fprintf(stdout, "/* a touch on b */\n");
        hit_flg |= FLG_A_TOUCH_ON_B;
    }
    if (distance_of_a_and_b < sum_of_a_r_and_b_r) {
        fprintf(stdout, "/* a is in b */\n");
        hit_flg |= FLG_A_IS_IN_B;
    }

    return hit_flg;
}

/* sphere_object の何秒後の状態を知りたいか、sec引数にて指定する。*/
int so_update(sphere_object_t *dst, sphere_object_t *src, double sec)
{
    sphere_object_t tmp_, *tmp = &tmp_;

    so_copy(tmp, src);
    tmp->x += tmp->vx * sec;
    tmp->y += tmp->vy * sec;
    so_copy(dst, tmp);

    return 0;
}

int so_clash(double *v1_, double *v2_,
             double m1, double v1,
             double m2, double v2,
             double e)
{
    /*
    1.   m1 * v1 + m2 + v2 = m1 * v1' + m2 * v2'
    2.   v1' - v2' = -e * (v1 - v2)
                 e = - ((v1' - v2') / (v1 - v2))
    2-1. v2' = v1' + e * (v1 - v2)
    2-2. v1' = v2' - e * (v1 - v2)
    1. に 2-1. を代入して、
         m1 * v1 + m2 + v2 = m1 * v1' + m2 * (v1' + e * (v1 - v2))
                           = v1' * (m1 + m2) + e * m2 * (v1 - v2)
         v1' * (m1 + m2) = m1 * v1 + m2 * v2 - (e * m2 * (v1 - v2))

               m1 * v1 + m2 * v2 - m2 * (e * (v1 - v2))
         v1' = ----------------------------------------
               m1 + m2

    1. に 2-2. を代入して、
               m1 * v1 + m2 * v2 + m1 * (e * (v1 - v2))
         v2' = ----------------------------------------
               m1 + m2

    */
    *v1_ = (m1 * v1 + m2 * v2 - m2 * (e * (v1 - v2))) / (m1 + m2);
    *v2_ = (m1 * v1 + m2 * v2 + m1 * (e * (v1 - v2))) / (m1 + m2);

    return 0;
}

int so_change_mv(sphere_object_t *a, sphere_object_t *b, double e)
{
    /* v1 as a->vx, v2 as b->vx */
    so_clash(&a->vx, &b->vx, a->m, a->vx, b->m, b->vx, e);
    so_clash(&a->vy, &b->vy, a->m, a->vy, b->m, b->vy, e);

    return 0;
}

/* 完全弾性衝突の実験 */
void experiment_1(double e)
{
    sphere_object_t a_, b_, *a = &a_, *b = &b_;
    double sec, delta;
    int collision_flg;

    /*             r,    m,     x0,    y0,    vx0,   vy0 */
    so_init(a, 0.010, 0.10,  0.100, 0.000, -0.010, 0.000);
    so_init(b, 0.010, 0.10, -0.100, 0.000,  0.010, 0.000);

    delta = 1.0;
    for (sec=0.0;sec<SIMULATION_SECONDS + 1.0;sec+=delta) {
        fprintf(stdout, "現在は実験開始から %.1f 秒後の状態です。\n", sec);
        so_view_current(a);
        so_view_current(b);
        collision_flg = so_collision_detect(a, b);
        if (collision_flg) {
            fprintf(stdout, "a detect collision of b.(collision_flg=%d)\n", collision_flg);
            so_change_mv(a, b, e);
        }
        fprintf(stdout, "\n");

        so_update(a, a, delta);
        so_update(b, b, delta);
    }
}

int main(void)
{
    /* 完全弾性衝突の実験 */
    experiment_1(1.0);

    return 0;
}