#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SPI.h>

#define TFT_CS    10
#define TFT_RST   9 
#define TFT_DC    8

#define SCREEN_WIDTH  320
#define SCREEN_HEIGHT 240
#define GRID_SIZE     20

#define FLOOR_COLOR   ILI9341_WHITE

int player1XPosition = 0;
int player1YPosition = SCREEN_HEIGHT - 2 * GRID_SIZE + 5;
int player2XPosition = GRID_SIZE;
int player2YPosition = SCREEN_HEIGHT - 2 * GRID_SIZE + 5;
int moveAmount = 3;

const int joystickXPin1 = A0;
const int joystickYPin1 = A1;
const int joystickXPin2 = A2;
const int joystickYPin2 = A3;
const int jumpButtonPin1 = 2;
const int jumpButtonPin2 = 3;

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

int jumpHeight = 40;
int jumpDuration = 1000;
int jumpStep = 1;
bool isPlayer1Jumping = false;
bool isPlayer2Jumping = false;
unsigned long jumpStartTime1 = 0;
unsigned long jumpStartTime2 = 0;

#define MAX_RECORDS 100

struct Position {
  int x;
  int y;
};

Position playerPositions[MAX_RECORDS];
int currentRecordIndex = 0;

#define MAX_OBSTACLES 10

struct Obstacle {
  int x;
  int y;
  int width;
  int height;
  uint16_t color;
};

Obstacle obstacles[MAX_OBSTACLES];
int obstacleCount = 0;

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

  tft.begin();
  tft.setRotation(3); 

  tft.fillScreen(ILI9341_BLACK);

  drawFloor();

  drawPlayer(player1XPosition, player1YPosition, ILI9341_RED);
  drawPlayer(player2XPosition, player2YPosition, ILI9341_BLUE);

  pinMode(jumpButtonPin1, INPUT_PULLUP);
  pinMode(jumpButtonPin2, INPUT_PULLUP);

  // Initialize obstacles
  obstacles[0] = {50, SCREEN_HEIGHT - 4 * GRID_SIZE, 40, GRID_SIZE, ILI9341_WHITE};
  obstacles[1] = {150, SCREEN_HEIGHT - 6 * GRID_SIZE, 40, GRID_SIZE, ILI9341_WHITE};
  obstacleCount = 2;

  // Draw obstacles
  for (int i = 0; i < obstacleCount; i++) {
    tft.fillRect(obstacles[i].x, obstacles[i].y, obstacles[i].width, obstacles[i].height, obstacles[i].color);
  }
}

void loop() {
  int joystickXValue1 = analogRead(joystickXPin1);
  int joystickXValue2 = analogRead(joystickXPin2);

  int deltaX1 = map(joystickXValue1, 0, 1023, moveAmount, -moveAmount);
  int deltaY1 = 0;
  int deltaX2 = map(joystickXValue2, 0, 1023, moveAmount, -moveAmount);
  int deltaY2 = 0;

  if (digitalRead(jumpButtonPin1) == LOW && !isPlayer1Jumping) {
    isPlayer1Jumping = true;
    jumpStartTime1 = millis();
  }

  if (isPlayer1Jumping) {
    jumpPlayer(1, jumpHeight, jumpDuration, jumpStep, deltaX1);
  } else {
    movePlayer(1, deltaX1, deltaY1);
  }

  if (digitalRead(jumpButtonPin2) == LOW && !isPlayer2Jumping) {
    isPlayer2Jumping = true;
    jumpStartTime2 = millis();
  }

  if (isPlayer2Jumping) {
    jumpPlayer(2, jumpHeight, jumpDuration, jumpStep, deltaX2);
  } else {
    movePlayer(2, deltaX2, deltaY2);
  }
}

void drawPlayer(int x, int y, uint16_t color) {
  tft.fillRect(x, y, 15, 15, color);
}

void drawFloor() {
  tft.fillRect(0, SCREEN_HEIGHT - GRID_SIZE, SCREEN_WIDTH, GRID_SIZE, FLOOR_COLOR);
}

void recordPosition(int x, int y) {
  if (currentRecordIndex < MAX_RECORDS) {
    playerPositions[currentRecordIndex].x = x;
    playerPositions[currentRecordIndex].y = y;
    currentRecordIndex++;
  }
}

void movePlayer(int player, int deltaX, int deltaY) {
  int* playerXPosition;
  int* playerYPosition;
  uint16_t color;

  if(player == 1) {
    playerXPosition = &player1XPosition;
    playerYPosition = &player1YPosition;
    color = ILI9341_RED;
  } else if(player == 2) {
    playerXPosition = &player2XPosition;
    playerYPosition = &player2YPosition;
    color = ILI9341_BLUE;
  } else {
    return;
  }

  tft.fillRect(*playerXPosition, *playerYPosition, 15, 15, ILI9341_BLACK);

  *playerXPosition += deltaX;
  *playerYPosition += deltaY;

  *playerXPosition = constrain(*playerXPosition, 0, SCREEN_WIDTH - 15);
  *playerYPosition = constrain(*playerYPosition, 0, SCREEN_HEIGHT - 15);

  // Check collision with obstacles
  for (int i = 0; i < obstacleCount; i++) {
    if (*playerXPosition < obstacles[i].x + obstacles[i].width &&
        *playerXPosition + 15 > obstacles[i].x &&
        *playerYPosition < obstacles[i].y + obstacles[i].height &&
        *playerYPosition + 15 > obstacles[i].y) {
      // Collision detected
      *playerYPosition = obstacles[i].y - 15;
      break;
    }
  }

  drawPlayer(*playerXPosition, *playerYPosition, color);
  
  // Record the new position
  recordPosition(*playerXPosition, *playerYPosition);
}

void jumpPlayer(int player, int jumpHeight, int jumpDuration, int jumpStep, int currentDeltaX) {
  int* playerXPosition;
  int* playerYPosition;
  uint16_t color;
  bool* isJumping;
  unsigned long* jumpStartTime;

  if(player == 1) {
    playerXPosition = &player1XPosition;
    playerYPosition = &player1YPosition;
    color = ILI9341_RED;
    isJumping = &isPlayer1Jumping;
    jumpStartTime = &jumpStartTime1;
  } else if(player == 2) {
    playerXPosition = &player2XPosition;
    playerYPosition = &player2YPosition;
    color = ILI9341_BLUE;
    isJumping = &isPlayer2Jumping;
    jumpStartTime = &jumpStartTime2;
  } else {
    return;
  }

  unsigned long elapsedTime = millis() - *jumpStartTime;
  tft.fillRect(*playerXPosition, *playerYPosition, 15, 15, ILI9341_BLACK);

  if (elapsedTime <= jumpDuration) {
    float jumpProgress = constrain((float)elapsedTime / jumpDuration, 0.0, 1.0);
    int currentJumpHeight = jumpHeight * (1 - 4 * (jumpProgress - 0.5) * (jumpProgress - 0.5));

    *playerYPosition = SCREEN_HEIGHT - 2 * GRID_SIZE - currentJumpHeight + 5;
    *playerXPosition += currentDeltaX;

    *playerXPosition = constrain(*playerXPosition, 0, SCREEN_WIDTH - 15);
    *playerYPosition = constrain(*playerYPosition, 0, SCREEN_HEIGHT - 15);

    // Check collision with obstacles
    for (int i = 0; i < obstacleCount; i++) {
      if (*playerXPosition < obstacles[i].x + obstacles[i].width &&
          *playerXPosition + 15 > obstacles[i].x &&
          *playerYPosition < obstacles[i].y + obstacles[i].height &&
          *playerYPosition + 15 > obstacles[i].y) {
        // Collision detected
        *playerYPosition = obstacles[i].y - 15;
        *isJumping = false;
        break;
      }
    }

    drawPlayer(*playerXPosition, *playerYPosition, color);
  } else {
    *isJumping = false;
    *playerYPosition = SCREEN_HEIGHT - 2 * GRID_SIZE + 5;

    // Check collision with obstacles
    for (int i = 0; i < obstacleCount; i++) {
      if (*playerXPosition < obstacles[i].x + obstacles[i].width &&
          *playerXPosition + 15 > obstacles[i].x &&
          *playerYPosition < obstacles[i].y + obstacles[i].height &&
          *playerYPosition + 15 > obstacles[i].y) {
        // Collision detected
        *playerYPosition = obstacles[i].y - 15;
        break;
      }
    }

    drawPlayer(*playerXPosition, *playerYPosition, color);
  }

  // Record the new position
  recordPosition(*playerXPosition, *playerYPosition);
}