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

// Constants for TFT display pins
#define TFT_CS    10
#define TFT_RST   9 
#define TFT_DC    8

// Screen dimensions
#define SCREEN_WIDTH  320
#define SCREEN_HEIGHT 240
#define GRID_SIZE     20

// Colors
#define FLOOR_COLOR   ILI9341_WHITE

// Initial positions of the players
int player1XPosition = 0;
int player1YPosition = SCREEN_HEIGHT - 2 * GRID_SIZE; // Start just above the bottom left floor
int player2XPosition = GRID_SIZE;                     // Start at the next grid on the bottom left floor
int player2YPosition = SCREEN_HEIGHT - 2 * GRID_SIZE; // Start just above the bottom left floor
int moveAmount = 3; // Adjust the movement amount to change speed

// Joystick pins
const int joystickXPin1 = A0;
const int joystickYPin1 = A1;
const int joystickXPin2 = A2;
const int joystickYPin2 = A3;
const int jumpButtonPin1 = 2; // Jump button pin for player 1
const int jumpButtonPin2 = 3; // Jump button pin for player 2

// Create TFT object
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Jump parameters
int jumpHeight = 40; // Jump height
int jumpDuration = 1000; // Jump duration
int jumpStep = 1; // Jump step

// Jump states for players
bool isPlayer1Jumping = false;
bool isPlayer2Jumping = false;

// Time tracking for jumps
unsigned long jumpStartTime1 = 0;
unsigned long jumpStartTime2 = 0;

// Function prototypes
void drawPlayer(int x, int y, uint16_t color);
void movePlayer(int player, int deltaX, int deltaY);
void jumpPlayer(int player, int jumpHeight, int jumpDuration, int jumpStep, int deltaX);
void drawFloor();

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

  // Initialize TFT display
  tft.begin();
  tft.setRotation(3); // Adjust display orientation

  // Clear the screen
  tft.fillScreen(ILI9341_BLACK);

  // Draw the floor
  drawFloor();

  // Display initial positions of players
  drawPlayer(player1XPosition, player1YPosition, ILI9341_RED); // Red player
  drawPlayer(player2XPosition, player2YPosition, ILI9341_BLUE); // Blue player

  // Set jump buttons as input and enable internal pull-up resistors
  pinMode(jumpButtonPin1, INPUT_PULLUP);
  pinMode(jumpButtonPin2, INPUT_PULLUP);
}

void loop() {
  // Read joystick positions
  int joystickXValue1 = analogRead(joystickXPin1);
  int joystickXValue2 = analogRead(joystickXPin2);

  // Determine movement direction based on joystick positions
  int deltaX1 = map(joystickXValue1, 0, 1023, -moveAmount, moveAmount); // Use joystick X-axis value to control X-axis movement
  int deltaY1 = 0; // Fixed Y-axis movement
  int deltaX2 = map(joystickXValue2, 0, 1023, -moveAmount, moveAmount); // Use joystick X-axis value to control X-axis movement
  int deltaY2 = 0; // Fixed Y-axis movement

  // If the jump button is pressed and player 1 is not jumping, start a jump for player 1
  if (digitalRead(jumpButtonPin1) == LOW && !isPlayer1Jumping) {
    isPlayer1Jumping = true;
    jumpStartTime1 = millis(); // Record the start time of the jump
  }

  // If player 1 is jumping, handle the jump
  if (isPlayer1Jumping) {
    jumpPlayer(1, jumpHeight, jumpDuration, jumpStep, deltaX1);
  } else {
    // Move red player
    movePlayer(1, deltaX1, deltaY1);
  }

  // If the jump button is pressed and player 2 is not jumping, start a jump for player 2
  if (digitalRead(jumpButtonPin2) == LOW && !isPlayer2Jumping) {
    isPlayer2Jumping = true;
    jumpStartTime2 = millis(); // Record the start time of the jump
  }

  // If player 2 is jumping, handle the jump
  if (isPlayer2Jumping) {
    jumpPlayer(2, jumpHeight, jumpDuration, jumpStep, deltaX2);
  } else {
    // Move blue player
    movePlayer(2, deltaX2, deltaY2);
  }

  // Delay to prevent too fast movement
  delay(50);
}

// Draw player at the given position
void drawPlayer(int x, int y, uint16_t color) {
  tft.fillRect(x, y, 15, 15, color); // Larger rectangle for bigger point
}

// Move the player to a new position
void movePlayer(int player, int deltaX, int deltaY) {
  int* playerXPosition;
  int* playerYPosition;
  uint16_t color;

  // Choose position and color based on player
  if(player == 1) {
    playerXPosition = &player1XPosition;
    playerYPosition = &player1YPosition;
    color = ILI9341_RED;
  } else if(player == 2) {
    playerXPosition = &player2XPosition;
    playerYPosition = &player2YPosition;
    color = ILI9341_BLUE;
  } else {
    return; // Unsupported player
  }

  // Clear the player's previous position
  tft.fillRect(*playerXPosition, *playerYPosition, 15, 15, ILI9341_BLACK); // Clear the player area

  // Update player position
  *playerXPosition += deltaX;
  *playerYPosition += deltaY;

  // Ensure player stays within screen boundaries
  *playerXPosition = constrain(*playerXPosition, 0, SCREEN_WIDTH - 15); // Adjusted for player size
  *playerYPosition = constrain(*playerYPosition, 0, SCREEN_HEIGHT - 15); // Limited to the screen height
  
  // Draw the player at the new position
  drawPlayer(*playerXPosition, *playerYPosition, color);
}

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

  // Choose position, color, and jump state based on player
  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; // Unsupported player
  }

  // Calculate elapsed time since the jump started
  unsigned long elapsedTime = millis() - *jumpStartTime;

  // Jump upwards if the jump is still within the duration
  if (elapsedTime <= jumpDuration) {
    // Clear previous position
    tft.fillRect(*playerXPosition, *playerYPosition, 15, 15, ILI9341_BLACK); // Clear the player area
    // Calculate jump height at this time
    float jumpProgress = (float)elapsedTime / jumpDuration;
    int currentJumpHeight = jumpHeight * (1 - 4 * (jumpProgress - 0.5) * (jumpProgress - 0.5)); // Parabolic motion
    // Move player upwards
    *playerYPosition = SCREEN_HEIGHT - 2 * GRID_SIZE - currentJumpHeight;
    *playerXPosition += currentDeltaX; // Maintain horizontal movement
    // Draw player at the new position
    drawPlayer(*playerXPosition, *playerYPosition, color);
  } else {
    // Jump duration elapsed, stop jumping
    *isJumping = false;
  }
}

// Draw the floor
void drawFloor() {
  // Third row
  for (int j = 3; j < 16; j++) {
    tft.fillRect(j * GRID_SIZE, 2 * GRID_SIZE, GRID_SIZE, GRID_SIZE, FLOOR_COLOR);
  }
  
  // Seventh row
  for (int j = 0; j < 9; j++) {
    tft.fillRect(j * GRID_SIZE, 6 * GRID_SIZE, GRID_SIZE, GRID_SIZE, FLOOR_COLOR);
  }
  
  // Bottom row
  for (int j = 0; j < 16; j++) {
    tft.fillRect(j * GRID_SIZE, 11 * GRID_SIZE, GRID_SIZE, GRID_SIZE, FLOOR_COLOR);
  }
}