#include <Adafruit_Arcada.h>

Adafruit_Arcada arcada;

GFXcanvas16* g_canvas;

uint8_t g_width = 160;
uint8_t g_height = 128;

class Ball {
    public:
        float x;
        float y;

        float dx;
        float dy;

        float radius;

        Ball(float x, float y, float dx, float dy, float radius) {
            this->x = x;
            this->y = y;
            this->dx = dx;
            this->dy = dy;
            this->radius = radius;
        }

        void update(float dt) {
            x += dx * dt;
            y += dy * dt;

            if (x <= -100) {
                x = g_width / 2;
                y = g_height / 2;
            }

            if (x >= g_width - radius) {
                dx = -dx;
                x = g_width - radius;
            }

            if (y <= this->radius) {
                dy = -dy;
                y = this->radius;
            }

            if (y >= g_height - radius) {
                dy = -dy;
                y = g_height - radius;
            }
        }

        void draw() {
            g_canvas->fillCircle(x, y, radius, 0xFFFF);
        }
};

class Paddle
{
public:
    float x;
    float y;

    int width;
    int height;

    Paddle(float x, float y, int width, int height)
    {
        this->x = x;
        this->y = y;
        this->width = width;
        this->height = height;
    }

    void update(float dt)
    {
        // joystick inputs range from -512 to 512
        // we want to map this to the screen size
        // but account for the size of the paddle
        int padY = arcada.readJoystickY();
        // Ensure that the paddle stays within the screen bounds
        this->y = map(padY, -512, 512, 0, g_height - height);
    }

    void draw()
    {
        g_canvas->fillRect(x, y, width, height, 0xFFFF);
    }
};

#include <cmath>

// Helper function to clamp value between min and max
float clamp(float value, float min, float max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
}

class Collision {
public:
    // Check for collision between ball and paddle and resolve if needed
    static bool checkAndResolveCollision(Ball& ball, Paddle& paddle) {
        // Calculate the ball's next position
        float ballNextX = ball.x + ball.dx;
        float ballNextY = ball.y + ball.dy;

        // Find the closest point on the paddle to the ball's next position
        float closestX = clamp(ballNextX, paddle.x, paddle.x + paddle.width);
        float closestY = clamp(ballNextY, paddle.y, paddle.y + paddle.height);

        // Calculate the distance from the closest point to the ball's center
        float distanceX = ballNextX - closestX;
        float distanceY = ballNextY - closestY;
        float distanceSquared = distanceX * distanceX + distanceY * distanceY;

        // Check if the distance is less than the ball's radius squared
        if (distanceSquared <= ball.radius * ball.radius) {
            // Collision detected, now resolve it

            // Calculate how much the ball has penetrated into the paddle
            float overlap = ball.radius - std::sqrt(distanceSquared);

            // Normalize the distance vector
            float distance = std::sqrt(distanceSquared);
            float normalX = distanceX / distance;
            float normalY = distanceY / distance;

            // Move the ball out of the paddle by the overlap amount
            ball.x += normalX * overlap;
            ball.y += normalY * overlap;

            // Reflect the ball's velocity
            float dotProduct = ball.dx * normalX + ball.dy * normalY;
            ball.dx -= 2 * dotProduct * normalX;
            ball.dy -= 2 * dotProduct * normalY;

            return true; // Collision resolved
        }

        return false; // No collision detected
    }
};

Ball ball = Ball(g_width / 2, g_height / 2, .1, .2, 4);
Paddle paddle = Paddle(5, 100, 3, 25);

Collision collision = Collision();

void setup(void) {
  Serial.begin(9600);

  // Start TFT and fill black
  if (!arcada.arcadaBegin()) {
    Serial.print("Arcada failed to begin!");
    while (1) delay(10);
  }

  arcada.displayBegin();
  
  // Turn on backlight
  arcada.setBacklight(255); 

  if (arcada.createFrameBuffer(g_width, g_height)) {
    g_canvas = arcada.getCanvas();
  }
}

int lastTime = 0;

void loop() {
  int now = millis();
  int deltaTime = lastTime - now;
  lastTime = now;

  // Clear screen
  g_canvas->fillScreen(0x0000);

  // Draw sprite
  ball.update(deltaTime);
  paddle.update(deltaTime);

  collision.checkAndResolveCollision(ball, paddle);

  ball.draw();
  paddle.draw();

  arcada.blitFrameBuffer(0, 0);
}