#include "DebugDraw.h"

#include <SDL2/SDL.h>
#include <SDL2/SDL2_gfxPrimitives.h>
#include <cstdio>
#include <cstdarg>
#include <cstring>
using namespace std;

#include "Window.h"
#include "RUBEScene.h"

void DebugDraw::DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    Sint16 xs[8];
    Sint16 ys[8];

    int32 i;
    for (i = 0; i < vertexCount; ++i) {
        SDL_Point p = scene->worldToPixel( vertices[i] );
        xs[i] = p.x;
        ys[i] = p.y;
    }

    polygonRGBA(renderer, xs, ys, vertexCount, (uint8)(color.r*255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
}

void DebugDraw::DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    Sint16 xs[8];
    Sint16 ys[8];

    int32 i;
    for (i = 0; i < vertexCount; ++i) {
        SDL_Point p = scene->worldToPixel( vertices[i] );
        xs[i] = p.x;
        ys[i] = p.y;
    }

    filledPolygonRGBA(renderer, xs, ys, vertexCount, (uint8)(color.r * 128), (uint8)(color.g * 128), (uint8)(color.b * 128), 128);
    polygonRGBA(renderer, xs, ys, vertexCount, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
}

void DebugDraw::DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    const float32 k_segments = 16.0f;
    const float32 k_increment = 2.0f * b2_pi / k_segments;
    float32 theta = 0.0f;

    Sint16 xs[16];
    Sint16 ys[16];

    int32 i;
    for (i = 0; i < k_segments; ++i) {
        b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
        SDL_Point p = scene->worldToPixel( v );
        xs[i] = p.x;
        ys[i] = p.y;
        theta += k_increment;
    }

    polygonRGBA(renderer, xs, ys, (int)k_segments, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
}

void DebugDraw::DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    const float32 k_segments = 16.0f;
    const float32 k_increment = 2.0f * b2_pi / k_segments;
    float32 theta = 0.0f;

    Sint16 xs[16];
    Sint16 ys[16];

    int32 i;
    for (i = 0; i < k_segments; ++i) {
        b2Vec2 v = center + radius * b2Vec2(cosf(theta), sinf(theta));
        SDL_Point p = scene->worldToPixel( v );
        xs[i] = p.x;
        ys[i] = p.y;
        theta += k_increment;
    }

    filledPolygonRGBA(renderer, xs, ys, (int)k_segments, (uint8)(color.r * 128), (uint8)(color.g * 128), (uint8)(color.b * 128), 128);
    polygonRGBA(renderer, xs, ys, (int)k_segments, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);

    b2Vec2 p = center + radius * axis;
    SDL_Point p1i = scene->worldToPixel(p);
    SDL_Point p2i = scene->worldToPixel(center);
    SDL_SetRenderDrawColor(renderer, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
    SDL_RenderDrawLine(renderer, p1i.x, p1i.y, p2i.x, p2i.y);
}

void DebugDraw::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    SDL_Point p1i = scene->worldToPixel(p1);
    SDL_Point p2i = scene->worldToPixel(p2);

    SDL_SetRenderDrawColor(renderer, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
    SDL_RenderDrawLine(renderer, p1i.x, p1i.y, p2i.x, p2i.y);
}

void DebugDraw::DrawTransform(const b2Transform& xf)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    b2Vec2 p1 = xf.p, p2;
    const float32 k_axisScale = 0.4f;
    p2 = p1 + k_axisScale * xf.q.GetXAxis();

    SDL_Point p1i = scene->worldToPixel(p1);
    SDL_Point p2i = scene->worldToPixel(p2);

    SDL_SetRenderDrawColor(renderer, 255, 0, 0, SDL_ALPHA_OPAQUE);
    SDL_RenderDrawLine(renderer, p1i.x, p1i.y, p2i.x, p2i.y);

    p2 = p1 + k_axisScale * xf.q.GetYAxis();
    p2i = scene->worldToPixel(p2);
    SDL_SetRenderDrawColor(renderer, 0, 255, 0, SDL_ALPHA_OPAQUE);
    SDL_RenderDrawLine(renderer, p1i.x, p1i.y, p2i.x, p2i.y);
}

void DebugDraw::DrawPoint(const b2Vec2& p, float32 size, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    SDL_Point pi = scene->worldToPixel(p);

    SDL_SetRenderDrawColor(renderer, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
    SDL_RenderDrawPoint(renderer, pi.x, pi.y);
}

void DebugDraw::DrawAABB(b2AABB* aabb, const b2Color& color)
{
    RUBEScene* scene = getCurrentScene();
    SDL_Renderer* renderer = getRenderer();

    SDL_Point lowerBound = scene->worldToPixel(aabb->lowerBound);
    SDL_Point upperBound = scene->worldToPixel(aabb->upperBound);

    Sint16 xs[4];
    Sint16 ys[4];

    xs[0] = lowerBound.x;
    ys[0] = lowerBound.y;

    xs[1] = upperBound.x;
    ys[1] = lowerBound.y;

    xs[2] = upperBound.x;
    ys[2] = upperBound.y;

    xs[3] = lowerBound.x;
    ys[3] = upperBound.y;

    polygonRGBA(renderer, xs, ys, 4, (uint8)(color.r * 255), (uint8)(color.g * 255), (uint8)(color.b * 255), SDL_ALPHA_OPAQUE);
}
