/*
Ball Physics

A code snippet for 2D sphere physics and collision against other sphere, static
line and static axis-aligned rectangle.

Author:   Adam Sawicki - adam__REMOVE__@asawicki.info - http://asawicki.info
License:  Public Domain
Version:  1.0
Released: 17 Mar 2012
*/

const vec2  GRAVITY_ACCELERATION = vec2(0.f, 98.0f);
const float VELOCITY_DAMPING     = 1.f; // Factor 0..1. 1 means no damping.
const float EXTRA_PUSHOFF        = 1e-2f;
const float DEFAULT_BOUNCE       = 0.5f;
const float PHYSICS_TIME_STEP    = 0.01f;
const float MAX_TIME_DELTA       = 0.5f;

float g_TimeDeltaAcc = 0.f;

struct Object
{
    vec2 PrevPos;
    vec2 CurrPos;
    float Radius;
    vec2 Vel;
    float Mass;
    float Bounce;
};

std::vector<Object> g_Objects;

const unsigned GRID_SIZE_X = 25;
const unsigned GRID_SIZE_Y = 20;
bool g_Grid[GRID_SIZE_X][GRID_SIZE_Y];
vec2 g_GridCellSize;

struct Line
{
    vec2 Beg, End;
};

std::vector<Line> g_Lines;

// Random number 0..1.
float rand_f();
// Linear interpolation.
vec2 lerp(const vec2& a, const vec2& b, float t);

void Init()
{
    vec2 screenSize = GetScreenSize();
    Line lines[] = {
        { vec2(0.f, 0.f), vec2(screenSize.x, 0.f) },
        { vec2(0.f, screenSize.y), vec2(screenSize.x, screenSize.y) },
        { vec2(0.f, 0.f), vec2(0.f, screenSize.y) },
        { vec2(screenSize.x, 0.f), vec2(screenSize.x, screenSize.y) },
        { vec2(screenSize.x*.5f, screenSize.y*.25f), vec2(screenSize.x*.75f, screenSize.y*.5f) },
    };
    g_Lines.assign(lines, lines + _countof(lines));

    for(size_t i = 0; i < 32; ++i)
    {
        float radius = rand_f() * 20.f + 10.f;
        vec2 pos = vec2(
            rand_f() * (screenSize.x - 2.f * radius) + radius,
            rand_f() * (screenSize.y - 2.f * radius) + radius );
        Object obj = {
            pos,            // PrevPos
            pos,            // CurrPos
            radius,         // Radius
            vec2(0.f, 0.f), // Vel
            0.f,            // Mass
            DEFAULT_BOUNCE, // Bounce
        };
        
        obj.Mass = obj.Radius * obj.Radius * 1e-4f;
        
        g_Objects.push_back(obj);
    }

    ZeroMemory(g_Grid, sizeof g_Grid);

    const unsigned FILLED_CELLS = 10;
    for(unsigned i = 0; i < FILLED_CELLS; ++i)
    {
        unsigned x = rand() % GRID_SIZE_X;
        unsigned y = rand() % GRID_SIZE_Y;
        g_Grid[x][y] = true;
    }

    g_GridCellSize = vec2(
        screenSize.x / (float)GRID_SIZE_X,
        screenSize.y / (float)GRID_SIZE_Y );
}

void Render()
{
    float alpha = g_TimeDeltaAcc / PHYSICS_TIME_STEP;

    Rendering::Begin();

    {
        for(unsigned y = 0; y < GRID_SIZE_Y; ++y)
        {
            for(unsigned x = 0; x < GRID_SIZE_X; ++x)
            {
                if(g_Grid[x][y])
                {
                    vec2 pos = vec2(
                        (float)x * g_GridCellSize.x,
                        (float)y * g_GridCellSize.y);
                    Rendering::DrawRectangle(pos, pos + g_GridCellSize, 0xFF8080FF);
                }
            }
        }
    }

    for(size_t i = 0; i < g_Lines.size(); ++i)
    {
        const float LINE_HALF_WIDTH = 3.f;
        const unsigned LINE_COLOR = 0xFFFF8080;

        const Line &line = g_Lines[i];
        Rendering::DrawLine(line.Beg, line.End, LINE_HALF_WIDTH, LINE_COLOR);
    }

    for(size_t i = 0; i < g_Objects.size(); ++i)
    {
        const Object& obj = g_Objects[i];
        vec2 pos = lerp(obj.PrevPos, obj.CurrPos, alpha);
        Rendering::DrawCircle(pos, obj.Radius, 0xFF80FF80);
    }

    Rendering::End();
}

bool CircleToLineCollision(
	vec2& circleCenter, vec2& circleVel, float circleRadius, float circleBounce,
    const vec2& lineBeg, const vec2& lineEnd)
{
    vec2 lineDir = lineEnd - lineBeg;
    float lineLen = lineDir.length();
    if(lineLen > 0.f)
        lineDir *= (1.f / lineLen);
    float p = (circleCenter - lineBeg).dot(lineDir);
                
    vec2 contactPoint;
    if(p <= 0.f)
        contactPoint = lineBeg;
    else if(p >= lineLen)
        contactPoint = lineEnd;
    else
        contactPoint = lineBeg + lineDir * p;

    vec2 norm = circleCenter - contactPoint;
    float distSq = norm.dot(norm);
    if(distSq < circleRadius * circleRadius)
    {
		float dist = sqrtf(distSq);
        if(dist > 0.f)
            norm *= (1.f / dist);
        circleCenter = contactPoint + norm * (circleRadius + EXTRA_PUSHOFF);
        float impact = -circleVel.dot(norm);
        if(impact > 0.f)
            circleVel += norm * impact * (1.f + circleBounce);

        return true;
    }
    else
        return false;
}

inline bool CircleToCircleCollision(
	vec2& circle1Center, vec2& circle1Vel, float circle1Radius, float circle1Bounce,
	vec2& circle2Center, vec2& circle2Vel, float circle2Radius, float circle2Bounce)
{
    vec2 collisionNormal = circle2Center - circle1Center;
    float distSq = collisionNormal.dot(collisionNormal);
    float minDistSq = (circle1Radius + circle2Radius) * (circle1Radius + circle2Radius);
    if(distSq < minDistSq)
    {
        float minDist = sqrtf(minDistSq);
        float dist    = sqrtf(distSq);
        collisionNormal.normalize();
        vec2 collisionTangent = vec2(collisionNormal.y, -collisionNormal.x);

        float v1Normal  =  circle1Vel.dot(collisionNormal);
        float v1Tangent =  circle1Vel.dot(collisionTangent);
        float v2Normal  = -circle2Vel.dot(collisionNormal);
        float v2Tangent =  circle2Vel.dot(collisionTangent);

        float pushoff = (minDist + EXTRA_PUSHOFF - dist) * .5f;
        circle1Center -= collisionNormal * pushoff;
        circle2Center += collisionNormal * pushoff;
		
        if(v1Normal + v2Normal > 0.f)
		{
            std::swap(v1Normal, v2Normal);
            v1Normal *= -circle1Bounce;
            v2Normal *= -circle2Bounce;
		}

        circle1Vel = collisionNormal*( v1Normal) + collisionTangent*v1Tangent;
		circle2Vel = collisionNormal*(-v2Normal) + collisionTangent*v2Tangent;

		return true;
    }

	return false;
}

bool CircleToRectangleCollision(
	vec2& circleCenter, vec2& circleVel, float circleRadius, float circleBounce,
	const vec2& rectMin, const vec2& rectMax)
{
	vec2 c = vec2(
		std::max(rectMin.x, std::min(circleCenter.x, rectMax.x)),
		std::max(rectMin.y, std::min(circleCenter.y, rectMax.y)));
	vec2 d = circleCenter - c;
	float distSq = d.dot(d);
	if(distSq >= circleRadius * circleRadius) return false;
	if(distSq == 0.f) distSq = 1e-9f;
	float dist = sqrtf( distSq );

	vec2 n = d / dist;
	float vp = circleVel.dot(n);
	circleVel -= n * (vp * (1+0.3f));
	
	circleCenter = c + n * (circleRadius + EXTRA_PUSHOFF);

	return true;
}

void UpdatePhysics()
{
    for (size_t i = 0; i < g_Objects.size(); ++i)
    {
        Object& obj = g_Objects[i];
       
        // Calculate forces
        vec2 force = vec2(0.f, 0.f);

        // Add some additional forces to force here...

        // Integrate
        vec2 acceleration = force / obj.Mass;
        acceleration += GRAVITY_ACCELERATION;
        
        obj.PrevPos = obj.CurrPos;
        obj.CurrPos += obj.Vel * PHYSICS_TIME_STEP;

        obj.Vel += acceleration * PHYSICS_TIME_STEP;

        if(VELOCITY_DAMPING < 1.f)
            obj.Vel *= powf(VELOCITY_DAMPING, PHYSICS_TIME_STEP); // TODO optimize: for fixed time step this can be precalculated.
    } // foreach obj in g_Objects

    // Collisions
    bool modified = true;
    for(size_t tryIndex = 0; modified && tryIndex < 3; ++tryIndex)
    {
        modified = false;

        for (size_t i = 0; i < g_Objects.size(); ++i)
        {
            Object& objI = g_Objects[i];

            for (size_t j = 0; j < g_Objects.size(); ++j)
            {
                if(j == i) continue;

                Object& objJ = g_Objects[j];

				if(CircleToCircleCollision(
					objI.CurrPos, objI.Vel, objI.Radius, objI.Bounce,
					objJ.CurrPos, objJ.Vel, objJ.Radius, objJ.Bounce))
				{
					modified = true;
				}
            } // foreach objJ in g_Objects

            for(size_t j = 0, lineCount = g_Lines.size(); j < lineCount; ++j)
            {
                const Line& line = g_Lines[j];

                if(CircleToLineCollision(
					objI.CurrPos, objI.Vel, objI.Radius, objI.Bounce,
                    line.Beg, line.End))
                {
                    modified = true;
                }
            } // foreach line in g_Lines

			{
				int minX = std::max<int>(0, (int)floorf((objI.CurrPos.x - objI.Radius)/g_GridCellSize.x));
				int minY = std::max<int>(0, (int)floorf((objI.CurrPos.y - objI.Radius)/g_GridCellSize.y));
				int maxX = std::min<int>(GRID_SIZE_X-1, (int)ceilf((objI.CurrPos.x + objI.Radius)/g_GridCellSize.x));
				int maxY = std::min<int>(GRID_SIZE_Y-1, (int)ceilf((objI.CurrPos.y + objI.Radius)/g_GridCellSize.y));

				for(int y = minY; y <= maxY; ++y)
				{
					for(int x = minX; x <= maxX; ++x)
					{
						if(g_Grid[x][y])
						{
							vec2 cellPos = vec2(
								(float)x * g_GridCellSize.x,
								(float)y * g_GridCellSize.y);
							if(CircleToRectangleCollision(
								objI.CurrPos, objI.Vel, objI.Radius, objI.Bounce,
								cellPos, cellPos + g_GridCellSize))
							{
								modified = true;
							}
						}
					}
				}
			}

        } // foreach objI in g_Objects
    } // for tryIndex
}

void Update()
{
    const float dt = std::min<float>(MAX_TIME_DELTA, GetTimeDelta());
    g_TimeDeltaAcc += dt;

    size_t stepCount = 0;
    while(g_TimeDeltaAcc > PHYSICS_TIME_STEP)
    {
        UpdatePhysics();
        g_TimeDeltaAcc -= PHYSICS_TIME_STEP;
        ++stepCount;
    }
}