#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "main.h"
#include "physics.h"
#include "atoms.h"
#include "things.h"

extern atom atoms[MAXATOMS];
extern bond bonds[MAXBONDS];
extern atomgroup agroups[MAXGROUPS];
extern int atom_num, bond_num, agroup_num;

extern float stiffness;

void DoLimitSpeed()
{
    atom *a;
    int i;
    for (i=0; i<atom_num; i++)
    {
        if (atoms[i].exists)
        {
	        a = &atoms[i];
	        if (a->grp!=256)
	        {
	            if (a->vel.y>MAXSPEED) a->vel.y = MAXSPEED;
	            if (a->vel.x>MAXSPEED) a->vel.x = MAXSPEED;
	            if (a->vel.y<-MAXSPEED) a->vel.y = -MAXSPEED;
	            if (a->vel.x<-MAXSPEED) a->vel.x = -MAXSPEED;
	        }
		}
    }
}

void DoBounds()
{
    atom *a;
    int i;
    for (i=0; i<atom_num; i++)
    {
		if (atoms[i].exists)
		{
	        a = &atoms[i];
	        if (a->pos.x<0 || a->pos.x>GameScreenWidth)
			{
				a->pos.x-=a->vel.x;
				a->vel.x = - a->vel.x*BOUNDDECAY;
			}
	        if (a->pos.y>GameScreenHeight/2.5)
			{
				a->pos.y-=a->vel.y;
				a->vel.y = - a->vel.y*BOUNDDECAY;
			}
	        if (a->pos.y<0)
			{
				a->pos.y-=a->vel.y;
				a->vel.y = - a->vel.y*BOUNDDECAY;
			}
		}
    }
}

void DoMovement()
{
    atom *a;
    int i;
    for (i=0; i<atom_num; i++)
    {
        if (atoms[i].exists)
        {
	        a = &atoms[i];
	        a->pos.x += a->vel.x;
	        a->pos.y += a->vel.y;
		}
    }
}

void DoGravity()
{
    atom *a;
    int i;

    for (i=0; i<atom_num; i++)
    {
        if (atoms[i].exists)
        {
	        a = &atoms[i];
	        a->vel.x += a->acl.x;
	        a->vel.y += a->acl.y;
		}
    }
}

void DoBonds()
{
    atom *a1, *a2;
    bond *b;

    int breakspace, deformspace;

    int i;
    float r, f;

    vector v1, v2;
    vector d;

    for (i=0; i<bond_num; i++)
    {
        b = &bonds[i];
        a1 = &atoms[b->atom1];
        a2 = &atoms[b->atom2];
        r = Vdist(&a1->pos, &a2->pos);

        breakspace = agroups[a1->grp].break_space;
        deformspace = agroups[a1->grp].deform_space;

        if (r > breakspace & b->broken==false)
        {
            b->broken = true;
            a1->nbonds--;
			if (a1->nbonds==0)
			{
				a1->exists = false;
				CreateBall(&a1->pos, &a1->vel, a1->size, a1->radius, rand()%3);
				printf("%d\n",a1->radius);
			}
            a2->nbonds--;
			if (a2->nbonds==0)
			{
				a2->exists = false;
				CreateBall(&a2->pos, &a2->vel, a2->size, a2->radius, rand()%3);
			}
        }

        if (!b->broken)
        {
            f = (r - b->balance) / stiffness;
            Vsub(&a2->pos, &a1->pos, &d);
            Vscale(&d, &d, f/b->balance);

            if (r > deformspace)
                b->balance += f * 0.75;

            Vadd(&a1->vel, &d, &a1->vel);
            Vneg(&d, &d);
            Vadd(&a2->vel,&d, &a2->vel);
        }
    }
}


static inline void TestAtomCollision(atom *a1)
{
    float r, l, l1, l2;
    vector d1, d2;
    atom *a2;

    int collidespace, r1, r2;

    int i;
    r1 = a1->radius / 4;
    for (i=0; i<atom_num; i++)
    {
        a2 = &atoms[i];
        if (a2->exists)
        {
	        r2 = a2->radius / 4;
	        collidespace = r1 + r2;
	        r = Vdist(&a2->pos, &a1->pos);
	        if (r<collidespace)
	        {
	            Vsub(&a2->pos, &a1->pos, &d1);
	            Vneg(&d1, &d2);
	            l = d1.x * d1.x + d1.y * d1.y;

	            // Project a1 on d1
	            l1 = (a1->vel.x * d1.x + a1->vel.y * d1.y) / l;

	            if (l1>=0)
	            {
	                Vscale(&d1, &d1, l1);

	                // Project a2 on d2
	                l2 = (a2->vel.x * d2.x + a2->vel.y * d2.y) / l;
	                Vscale(&d2, &d2, l2);

	                Vadd(&a1->vel, &d2, &a1->vel);
	                Vsub(&a1->vel, &d1, &a1->vel);

    	            Vadd(&a2->vel, &d1, &a2->vel);
	                Vsub(&a2->vel, &d2, &a2->vel);
				}
            }
        }
    }
}

void DoCollisions()
{
	int i;
	for (i=0; i<atom_num; i++)
		if (atoms[i].exists)
			TestAtomCollision(&atoms[i]);
}

void DoPhysics()
{
    DoBonds();
    DoGravity();
    DoBounds();
    DoCollisions();
    DoLimitSpeed();
    DoMovement();
}
