#include "devicelib.h"

#include <cmath>
#include <hardware/clocks.h>
#include <pico/bootrom.h>
#include <tusb.h>

using namespace lib;

namespace
{

Display display(DISPLAY_SCK, DISPLAY_TX, DISPLAY_CS, DISPLAY_DC,
                100 * 1000 * 1000);
Flash flash(FLASH_SCK, FLASH_TX, FLASH_RX, FLASH_CS, 25 * 1000 * 1000);

void draw_rectangle(float x, float y, float w, float h, uint8_t r, uint8_t g,
                    uint8_t b)
{
  int wi = w;
  int hi = h;
  display.set_update_area(x - w / 2, y - h / 2, wi, hi);

  const uint8_t buf[24] = {
    r, g, b, r, g, b, r, g, b, r, g, b, r, g, b, r, g, b, r, g, b, r, g, b,
  };

  auto pixels = display.pixels();
  for (int i = 0; i < (wi + 1) * (hi + 1); i += 8)
  {
    pixels.write(buf, sizeof(buf));
  }
}

}

int main()
{
  set_sys_clock_khz(150 * 1000, false);

  gpio_set_function(FLASH_IO_2, GPIO_FUNC_SIO);
  gpio_set_function(FLASH_IO_3, GPIO_FUNC_SIO);

  gpio_set_dir(FLASH_IO_2, GPIO_OUT);
  gpio_set_dir(FLASH_IO_3, GPIO_OUT);

  gpio_put(FLASH_IO_2, 1);
  gpio_put(FLASH_IO_3, 1);

  gpio_set_function(BUTTON_LEFT, GPIO_FUNC_SIO);
  gpio_set_function(BUTTON_MIDDLE, GPIO_FUNC_SIO);
  gpio_set_function(BUTTON_RIGHT, GPIO_FUNC_SIO);

  gpio_set_dir(BUTTON_LEFT, GPIO_IN);
  gpio_set_dir(BUTTON_MIDDLE, GPIO_IN);
  gpio_set_dir(BUTTON_RIGHT, GPIO_IN);

  gpio_pull_up(BUTTON_LEFT);
  gpio_pull_up(BUTTON_MIDDLE);
  gpio_pull_up(BUTTON_RIGHT);

  tusb_rhport_init_t dev_init
    = { .role = TUSB_ROLE_DEVICE, .speed = TUSB_SPEED_AUTO };
  tud_init(0);

  absolute_time_t previous_time = get_absolute_time();
  float time_since_last_display = 0;

  constexpr float SPEED = 75;
  float paddle_x = 120;
  constexpr float paddle_y = 280;

  constexpr float BALL_SPEED = 125;
  float ball_x = 120;
  float ball_y = 160;

  float INITIAL_BALL_ANGLE = -3.1415926f / 4.0f;
  float ball_dx = cosf(INITIAL_BALL_ANGLE);
  float ball_dy = sinf(INITIAL_BALL_ANGLE);

  uint8_t left_history = 0;
  uint8_t right_history = 0;

  while (true)
  {
    tud_task();
    absolute_time_t current = get_absolute_time();
    float dt = absolute_time_diff_us(previous_time, current) / 1000000.0f;
    previous_time = current;

    float old_ball_pos_x = ball_x;
    float old_ball_pos_y = ball_y;

    ball_x += ball_dx * BALL_SPEED * dt;
    ball_y += ball_dy * BALL_SPEED * dt;

    if (ball_x - 2 <= 0 || ball_x + 2 >= 240)
    {
      ball_dx = -ball_dx;
      ball_x += ball_dx * 2;
    }
    if (ball_y - 2 <= 0 || ball_y + 2 >= 320)
    {
      ball_dy = -ball_dy;
      ball_y += ball_dy * 2;
    }

    if (ball_x + 2 >= paddle_x - 20 && ball_x - 2 <= paddle_x + 20
        && ball_y + 3 >= paddle_y - 2 && ball_y - 3 <= paddle_y - 2)
    {
      float ball_angle = atan2f(ball_dy, ball_dx);
      float diff = paddle_x - ball_x;
      ball_angle += diff / 50.0f;
      ball_dx = cosf(ball_angle);
      ball_dy = -sinf(ball_angle);
      ball_y = paddle_y - 6;
    }

    float old_pos = paddle_x;

    left_history = (left_history << 1) | !gpio_get(BUTTON_LEFT);
    right_history = (right_history << 1) | !gpio_get(BUTTON_RIGHT);

    if (left_history == 0xFF)
    {
      paddle_x -= SPEED * dt;
    }

    if (right_history == 0xFF)
    {
      paddle_x += SPEED * dt;
    }

    time_since_last_display += dt;

    if (time_since_last_display > 0.02f)
    {
      // ball
      draw_rectangle(old_ball_pos_x, old_ball_pos_y, 12, 12, 0, 0, 0);
      draw_rectangle(ball_x, ball_y, 4, 4, 0xFF, 0xFF, 0xFF);

      // player paddle
      draw_rectangle(old_pos - 20, paddle_y, 6, 7, 0, 0, 0);
      draw_rectangle(old_pos + 20, paddle_y, 6, 7, 0, 0, 0);
      draw_rectangle(paddle_x, paddle_y, 40, 5, 0xFF, 0xFF, 0xFF);
      time_since_last_display = 0;
    }
  }
}

void tud_vendor_rx_cb(uint8_t itf, const uint8_t* buffer, uint16_t bufsize)
{
  if (buffer[0] == 0x99)
  {
    rom_reset_usb_boot(0, 0);
  }
}