#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <vector>
#include <SFML/Graphics.hpp>
#include <svector.h>
using namespace std;
using namespace skn;

typedef sf::Color Col;
typedef sf::Shape S;
typedef Vector<double,2> Charge;
typedef Vector<double,2> Vec;

static const Charge
    red  (cos(0*M_PI/3),sin(0*M_PI/3)),
    green(cos(2*M_PI/3),sin(2*M_PI/3)),
    blue (cos(4*M_PI/3),sin(4*M_PI/3));

struct Particle { Vec r, v, a; double m; Charge q; sf::Shape shape; };

int main(int argc, char ** argv)
{
    double dt = 1e-4, G = 1, t = 0;
    int skip = 1000;
    sf::RenderWindow App(sf::VideoMode(1000, 1000, 32), "Color");
    sf::View view(sf::Vector2f(0,0),sf::Vector2f(25,25));
    App.SetView(view);
    Particle p[] = {
        {Vec(-10, 0.25),Vec(0,0),Vec(0,0),1, red,  S::Circle(0,0,0.2,Col(255,  0,  0))},
        {Vec(-10,-0.25),Vec(0,0),Vec(0,0),1, green,S::Circle(0,0,0.2,Col(  0,255,  0))},
        {Vec(- 9, 0.00),Vec(0,0),Vec(0,0),1, blue, S::Circle(0,0,0.2,Col(  0,  0,255))},

        {Vec( 10,-0.60),Vec(0,0),Vec(0,0),1,-green,S::Circle(0,0,0.2,Col(255,  0,255))},
        {Vec( 10, 0.40),Vec(0,0),Vec(0,0),1,-blue, S::Circle(0,0,0.2,Col(255,255,  0))},
        {Vec(  9, 0.00),Vec(0,0),Vec(0,0),1,-red,  S::Circle(0,0,0.2,Col(  0,255,255))},

        {Vec(  0, 10.0),Vec(0,0),Vec(0,0),1, green,S::Circle(0,0,0.2,Col(  0,255,  0))},
        {Vec(  1,  9.0),Vec(0,0),Vec(0,0),1, red,  S::Circle(0,0,0.2,Col(255,  0,  0))},
        {Vec(  3,  9.0),Vec(0,0),Vec(0,0),1, blue, S::Circle(0,0,0.2,Col(  0,  0,255))},
    };
    int n = sizeof(p)/sizeof(Particle);

    for(int k = 0;; k++)
    {
        for(int i = 0; i < n; i++)
        {
            p[i].v += p[i].a * dt;
            p[i].r += p[i].v * dt;
            p[i].a = 0;
        }
        for(int i = 0; i < n; i++)
            for(int j = i+1; j < n; j++)
            {
                Vec r = p[j].r-p[i].r;
                Vec f = -r*(G*dot(p[i].q,p[j].q)*pow(abs(r),-0.5));
                p[i].a += f/p[i].m;
                p[j].a -= f/p[j].m;
            }
        if(k%skip == 0)
        {
            for(int i = 0; i < n; i++)
            {
                p[i].shape.SetPosition(p[i].r[0],p[i].r[1]);
                App.Draw(p[i].shape);
            }
            App.Display();
            sf::Sleep(0.001);
        }
    }
}