#include <FastLED.h>

#define LED_PIN 6
#define WIDTH 32
#define HEIGHT 8
#define NUM_LEDS (WIDTH * HEIGHT)
CRGB leds[NUM_LEDS];

// https://pastebin.com/UzSiw8yE
int index(int x, int y) {
  int rv;
  if (x % 2 == 0)
    rv = x * HEIGHT + y;
  else
    rv = x * HEIGHT + HEIGHT - y - 1;
  return rv;
}

float ball_x = 0;
float ball_y = 0;

float ball_vx = 0;
float ball_vy = 0;

int player_1 = 0;
int player_2 = 0;
int score_1 = 0;
int score_2 = 0;

const int MAX_BR = 255;
const int X_pos_1 = 1;
const int X_pos_2 = WIDTH - 2;
const int palca = 3;

#define rezistor_1 A0
#define rezistor_2 A1

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, LED_PIN>(leds, NUM_LEDS);
  FastLED.setBrightness(50);
  pinMode(rezistor_1, INPUT);
  pinMode(rezistor_2, INPUT);
  ball_x = WIDTH / 2;
  ball_y = HEIGHT / 2;
  ball_vx = 0.1;
  ball_vy = 0.05;
}

bool intersects(float x1, float y1, float x2, float y2, float vert_x, float vert_y1, float vert_y2){
  bool case1 = !((x2 > x1) && (vert_x >= x1) && (vert_x <= x2));
  bool case2 = !((x1 > x2) && (vert_x >= x2) && (vert_x <= x1));
  if (case1 && case2)
    return false;

  float K = (y2-y1) / (x2-x1);
  float B = y1 - K*x1;
  float vert_y = K * vert_x + B;

  if((vert_y2 >= vert_y1) && (vert_y >= vert_y1) && (vert_y <= vert_y2))
    return true;
  if((vert_y1 > vert_y2) && (vert_y >= vert_y2) && (vert_y <= vert_y1))
    return true;

  return false;
}

void loop() {
  player_1 = map(analogRead(rezistor_1), 0, 1020, 0, HEIGHT - palca);
  player_2 = map(analogRead(rezistor_2), 0, 1020, 0, HEIGHT - palca);
  Serial.print("p1=");
  Serial.print(player_1);
  Serial.print(" p2=");
  Serial.print(player_2);
  Serial.println();

  float ball_x_new = ball_x + ball_vx;
  float ball_y_new = ball_y + ball_vy;
  if (ball_y_new < 0 || ball_y_new >= HEIGHT) {
    ball_vy = -ball_vy;
    ball_y_new = ball_y + ball_vy;
  }
  else if (ball_x_new < 0){
    score_2 = score_2 + 1;
    ball_x_new = X_pos_1 + 2;
    ball_y_new = player_1 + palca / 2.0;
    ball_vx = 0.1;
    ball_vy = 0;
  }
  else if (ball_x_new >= WIDTH){
    score_1 = score_1 + 1;
    ball_x_new = X_pos_2 - 2;
    ball_y_new = player_2 + palca / 2.0;
    ball_vx = -0.1;
    ball_vy = 0;
  }
  else if(intersects(ball_x, ball_y, ball_x_new, ball_y_new, X_pos_1+1, player_1, player_1 + palca - 1)){
    ball_vx = -ball_vx;
    ball_x_new = ball_x + ball_vx;
  }
  else if(intersects(ball_x, ball_y, ball_x_new, ball_y_new, X_pos_2, player_2, player_2 + palca - 1)){
    ball_vx = -ball_vx;
    ball_x_new = ball_x + ball_vx;
  }
  ball_x = ball_x_new;
  ball_y = ball_y_new;

  // Serial.print(ball_x);
  // Serial.print(" ");
  // Serial.print(ball_y);
  // Serial.println();

  for (int i = 0; i < NUM_LEDS; i++)
    leds[i].setRGB(0, 0, 0);

  for (int i = 0; i < palca; i++) {
    leds[index(X_pos_1, i + player_1)].setRGB(0, 0, MAX_BR);
    leds[index(X_pos_2, i + player_2)].setRGB(MAX_BR, 0, 0);
  }

  leds[index(ball_x, ball_y)].setRGB(MAX_BR, MAX_BR, MAX_BR);
  FastLED.show();
  delay(10);
}