/*
 * Ball Collision, simulates collision between several
 * freely moving rigid balls.
 * Uses FreeGLUT for graphics.
 * Press SPACE or ENTER to generate new circles, ESC or Q to quit.
 */
#include <cstdlib>    // for rand/srand
#include "GL/freeglut.h"  // normally should be <>
#include <ctime>      // for clock
#include <iostream>   // logging to console
#include <cmath>      // for hypot


//
// simple definitions

/// 2D vector
struct Vec2
{
  double x, y;
  
  Vec2(double nx = 0.0, double ny = 0.0)
    : x(nx), y(ny)
  {
  }

  Vec2& operator+=(const Vec2 &other)
  {
    x += other.x;
    y += other.y;
    return *this;
  }

  Vec2& operator*=(const double coef)
  {
    x *= coef;
    y *= coef;
    return *this;
  }

  double length() const
  {
    return hypot(x, y);
  }
};

inline Vec2 operator+(const Vec2 &a, const Vec2 &b)
{
  return Vec2(a.x + b.x, a.y + b.y);
}

inline Vec2 operator-(const Vec2 &a, const Vec2 &b)
{
  return Vec2(a.x - b.x, a.y - b.y);
}

inline Vec2 operator*(double coef, const Vec2 &v)
{
  return Vec2(coef * v.x, coef * v.y);
}

inline double operator*(const Vec2 &a, const Vec2 &b)
{
  return a.x * b.x + a.y * b.y;
}

std::ostream& operator<<(std::ostream &os, const Vec2 &v)
{
  return os << '(' << v.x << ", " << v.y << ')';
}


/// RGBA color
struct Color
{
  float red, green, blue, alpha;

  Color(float r = 0.0f, float g = 0.0f, float b = 0.0f, float a = 1.0f)
    : red(r), green(g), blue(b), alpha(a)
  {
  }

  void select() const
  {
    glColor4f(red, green, blue, alpha);
  }
};


/// rigid ball
struct Ball
{
  Vec2 p; ///< position
  Vec2 v; ///< velocity
  Vec2 a; ///< acceleration
  double m; ///< mass
  double r; ///< radius
  Color color;

  Ball
    (
      const Vec2 &pos = Vec2(),
      const Vec2 &vel = Vec2(),
      double mass = 1.0, 
      double rad = 10.0,
      const Color &col = Color(1.0f, 1.0f) // yellow by default
    )
      : p(pos), v(vel), m(mass), r(rad), color(col), a()
  {
  }

  /// OpenGL code to bring Ball to the frame buffer is here
  void paint() const
  {
    glLoadIdentity();
    glTranslated(p.x, p.y, 0.0);
    glScaled(r, r, r);
    color.select();
    glCallList(1);
  }

  /// calculate dynamics
  void move(double time_step)
  {
    const Vec2 ta = time_step * a;
    p += time_step * (v + 0.5 * ta);
    v += ta;
    a = Vec2();
  }
};

std::ostream& operator<<(std::ostream &os, const Ball &c)
{
  return os << "position: " << c.p
          << "\nvelocity: " << c.v
          << "\nmass: " << c.m
          << "\nradius: " << c.r << std::endl;
}


//
// constants and current (global) state
// 
/// amount of bodies
const size_t N = 20;

/// relaxation coefficient (should fall into [0, 1])
const double k = 0.9;

/// central force constant
const double G = 50000.0;

/// actual bodies
Ball C[N];

/// last frame time moment
std::clock_t t;

/// window width and height
float width = 800.0f, height = 600.0f;


//
// aux functions
//

/// returns a random number from [minv, maxv)
inline double random(double minv, double maxv)
{
  static const double anti_rand_max = 1.0 / double(RAND_MAX);
  return minv + (maxv - minv) * std::rand() * anti_rand_max;
}


/// generates randomly moving circles going to collide somewhere
void newBalls()
{
  std::cout << "\nGenerating new circles:\n";

  // target collision point
  const Vec2 cp(random(0.1 * width, 0.9 * width), random(0.1 * height, 0.9 * height));
  std::cout << "Collision point: " << cp << "\n";

  for (size_t i = 0; i < N; ++i)
  {
    Ball &b = C[i]; // setup C[i] fields
    b.p = Vec2(random(0.0, width), random(0.0, height)); // position
    b.v = 0.25 * (cp - b.p); // 4 seconds before it may reach the collision point
    b.m = random(0.1, 10.0); // mass
    b.r = random(4.0, 20.0); // radius
    b.color.red   = static_cast<float>(random(0.0, 1.0));
    b.color.green = static_cast<float>(random(0.0, 1.0));
    b.color.blue  = static_cast<float>(random(0.0, 1.0));

    std::cout << i << ")\n" << b;
  }
}


/// gravity-like force, pulling to the screen center
inline void applyCentralForce(Ball &ball)
{
  Vec2 r(0.5 * width - ball.p.x, 0.5 * height - ball.p.y);
  const double dist = (std::max)(r.length(), 1.0);
  r *= G / (dist * dist * dist);
  ball.a += r;
}

//
// GLUT callbacks
//

/// redraw our scene
void repaint()
{
  glClear(GL_COLOR_BUFFER_BIT);
  
  // paint all balls
  for (size_t i = 0; i < N; ++i)
    C[i].paint();

  // finish and present our frame to the user
  glFinish();
  glutSwapBuffers();
}


/// calculate the next frame
void frameMove()
{
  static const double anti_clocks_per_sec = 1.0 / double(CLOCKS_PER_SEC);

  // check time
  std::clock_t t1 = std::clock();
  if(t1 != t)
  {
    const double h = anti_clocks_per_sec * (t1 - t);
    t = t1;
    
    // move
    for (size_t i = 0; i < N; ++i)
    {
      applyCentralForce(C[i]);
      C[i].move(h);
    }
    
    // check for intersection
    for (size_t i = 0; i < N; ++i)
    {
      for (size_t j = i + 1; j < N; ++j)
      {
        const Vec2 dp = C[i].p - C[j].p;
        const double dplen = dp.length();
        const double delta = dplen - (C[i].r + C[j].r);

        if (delta < 0.0) // are overlapping?
        {
          const Vec2 r = 1.0 / dplen * dp;
          if ( (C[i].v - C[j].v) * r < 0.0) // are approaching?
          {
            // collision detected -- calculate new velocities
            const Vec2 u(-r.y, r.x);
            // old velocities in (r, u) basis
            const double
              v1r = C[i].v * r,
              v2r = C[j].v * r,
              v1u = C[i].v * u,
              v2u = C[j].v * u,
            // new velocities
              v1n = k / (C[i].m + C[j].m) * ((C[i].m - C[j].m) * v1r + 2.0 * C[j].m * v2r),
              v2n = k / (C[i].m + C[j].m) * (2.0 * C[i].m * v1r + (C[j].m - C[i].m) * v2r);

            // calculate resulting velocities in original coordinates
            C[i].v = v1n * r + v1u * u;
            C[j].v = v2n * r + v2u * u;
          }
        }
      }
    }
  

    // check if all disks flew out of viewport
    bool new_round = true;
    for (size_t i = 0; new_round && i < N; ++i)
    {
      new_round = new_round &&
        (C[i].p.x < -C[i].r || C[i].p.y < -C[i].r ||
          C[i].p.x > width + C[i].r || C[i].p.y > height + C[i].r);
    }

    if (new_round)
      newBalls();

    // say GLUT that our frame has changed
    glutPostRedisplay();
  }
}


/// work out a keypress
void keyPressed(unsigned char key, int x, int y)
{
  switch (key)
  {
  case 27: // ESC ASCII code
  case 'q':
  case 'Q': // if ASCII fails ;)
    std::cout << "Exiting\n";
    exit(0); // quit

  case ' ':
  case '\r':
  case '\n':
    newBalls();
  
  default:
    ; // ignore it, suppress compiler's possible warning
  }
}


void reshape(int w, int h)
{
  width = static_cast<float>(w);
  height = static_cast<float>(h);

  // setup projection
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(0.0, w, 0.0, h, -100.0, 100.0);
  glMatrixMode(GL_MODELVIEW);

  // setup viewport
  glViewport(0, 0, w, h);
}


//
// main
//

int main(int argc, char **argv)
{
  // initialization
  glutInit(&argc, argv);
  std::srand(static_cast<unsigned>(std::time(nullptr)));

  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_MULTISAMPLE);
  glutInitWindowPosition(100, 100);
  glutInitWindowSize(width, height);
  glutCreateWindow("Circle collision simulation");

  // setup some OpenGL state
  glClearColor(0.0, 0.0, 0.0, 1.0); // default one (black), just to show glClearColor
  glEnable(GL_COLOR_MATERIAL);
  glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
  
  // create a disk image
  glNewList(1, GL_COMPILE);
  glutSolidSphere(1.0, 16, 4);
  glEndList();
  
  // register callbacks
  glutDisplayFunc(repaint);
  glutKeyboardFunc(keyPressed);
  glutReshapeFunc(reshape);
  glutIdleFunc(frameMove);

  // run
  std::cout << "Starting...\n" << "k = " << k << "\n";
  newBalls();
  t = std::clock();
  glutMainLoop();
}