#include "draw.h"

Color convert_color(const b2HexColor c, const float alpha) {
  return {
    static_cast<uint8_t>(c >> 16 & 0xFF), static_cast<uint8_t>(c >> 8 & 0xFF), static_cast<uint8_t>(c & 0xFF),
    static_cast<uint8_t>(0xFF * alpha)
  };
}

void dd_solid_polygon(const b2Transform transform, const b2Vec2* vertices, const int vertexCount, float radius,
                             const b2HexColor color, void* context) {
  Vector2 points[B2_MAX_POLYGON_VERTICES + 1];

  for (int i = 0; i < vertexCount; ++i) {
    points[i].x = transform.p.x + vertices[i].x * transform.q.c - vertices[i].y * transform.q.s;
    points[i].y = transform.p.y + vertices[i].x * transform.q.s + vertices[i].y * transform.q.c;
    points[i].y *= -1.0f;

    points[i].x *= PIXELS_PER_METER;
    points[i].y *= PIXELS_PER_METER;
  }

  points[vertexCount] = points[0];

  // DrawTriangleFan(&points[0], vertexCount + 1, b2HexColorToColor(color, 1.0f));
  DrawLineStrip(points, vertexCount + 1, convert_color(color, 1.0f));
}

void dd_solid_circle(const b2Transform transform, const float radius, const b2HexColor color, void* context) {
  const Vector2 center = {
    transform.p.x * PIXELS_PER_METER,
    -transform.p.y * PIXELS_PER_METER
  };

  const float scaled_radius = radius * PIXELS_PER_METER;

  DrawCircleLines(center.x, center.y, scaled_radius, convert_color(color, 1.0f));

  const float angle = atan2f(transform.q.s, transform.q.c);
  const float end_x = center.x + scaled_radius * cosf(angle);
  const float end_y = center.y - scaled_radius * sinf(angle);

  DrawLine(center.x, center.y, end_x, end_y, convert_color(color, 1.0f));
}

void dd_draw_point(const b2Vec2 p, const float size, const b2HexColor color, void* context) {
  DrawCircle(p.x * PIXELS_PER_METER, -p.y * PIXELS_PER_METER, size, convert_color(color, 1.0f));
}
