#include <FastLED.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_DotStarMatrix.h>
#include <Adafruit_DotStar.h>
#include <SPI.h>
#include <SparkFun_APDS9960.h>
#undef WAIT  // Prevent macro conflict with FastLED

// ============ PIN DEFINITIONS ============
// PublicI2C (slave) pins
#define PUBLIC_I2C_SDA  21
#define PUBLIC_I2C_SCL  22
#define PUBLIC_I2C_ADDR 0x42  // Example slave address

// LocalI2C (master) pins
#define LOCAL_I2C_SDA   12
#define LOCAL_I2C_SCL   11

// OLED display settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1

// Joystick pins
#define JOY_X_PIN      1   // ADC1_CH0
#define JOY_Y_PIN      2   // ADC1_CH1
#define JOY_BTN_PIN    3   // Digital input

// FastLED (NeoPixel) settings
#define NUM_LEDS       16
#define LED_DATA_PIN   5   // Example GPIO for LED data

// DotStar HD 8x8 Grid settings
#define DOTSTAR_WIDTH   8
#define DOTSTAR_HEIGHT  8
#define DOTSTAR_DATA    6  // Example GPIO for DotStar data
#define DOTSTAR_CLK     7  // Example GPIO for DotStar clock

// ============ I2C BUS OBJECTS ============
TwoWire PublicI2C = TwoWire(0); // I2C0 (slave)
TwoWire LocalI2C  = TwoWire(1); // I2C1 (master)

// ============ DEVICE OBJECTS =============
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &LocalI2C, OLED_RESET);
// Use a SparkFun_APDS9960 library that supports TwoWire
SparkFun_APDS9960 apds(&LocalI2C);
CRGB leds[NUM_LEDS];
Adafruit_DotStarMatrix dotstar = Adafruit_DotStarMatrix(
  DOTSTAR_WIDTH, DOTSTAR_HEIGHT, DOTSTAR_DATA, DOTSTAR_CLK,
  DS_MATRIX_TOP + DS_MATRIX_LEFT + DS_MATRIX_ROWS + DS_MATRIX_PROGRESSIVE,
  DOTSTAR_BRG
);

// ============ DATA STRUCTURES ============
struct SensorData {
  int8_t joystickDir;   // -1=none, 0=up, 1=down, 2=left, 3=right
  bool   joystickBtn;
  uint16_t colorR, colorG, colorB, colorC;
};
volatile SensorData latestData;

// ============ I2C SLAVE DATA HANDLERS =========
void receiveEvent(int numBytes) {
  // Not used in this application, but required by Wire
}

void requestEvent() {
  // Copy volatile to local to avoid race conditions
  SensorData copy;
  noInterrupts();
  copy = latestData;
  interrupts();
  // Send as a binary buffer
  PublicI2C.write((uint8_t*)&copy, sizeof(SensorData));
}

// ============ JOYSTICK INTERPRETATION =========
#define JOY_THRESHOLD_HIGH 3000
#define JOY_THRESHOLD_LOW  1000

void updateJoystick(SensorData &data) {
  int joyX = analogRead(JOY_X_PIN);
  int joyY = analogRead(JOY_Y_PIN);
  bool joyBtn = digitalRead(JOY_BTN_PIN) == LOW;

  // Default: no direction
  data.joystickDir = -1;

  if (joyY > JOY_THRESHOLD_HIGH) data.joystickDir = 0; // Up
  else if (joyY < JOY_THRESHOLD_LOW) data.joystickDir = 1; // Down
  else if (joyX < JOY_THRESHOLD_LOW) data.joystickDir = 2; // Left
  else if (joyX > JOY_THRESHOLD_HIGH) data.joystickDir = 3; // Right

  data.joystickBtn = joyBtn;
}

// ============ COLOR SENSOR READING =========
void updateColorSensor(SensorData &data) {
  uint16_t r=0, g=0, b=0, c=0;
  if (apds.readAmbientLight(c) &&
      apds.readRedLight(r) &&
      apds.readGreenLight(g) &&
      apds.readBlueLight(b)) {
    data.colorR = r;
    data.colorG = g;
    data.colorB = b;
    data.colorC = c;
  }
}

// ============ DISPLAY LOGIC =========
void updateDisplays(const SensorData &data) {
  // DotStar: show direction as arrow or color as background
  dotstar.fillScreen(0);
  if (data.joystickDir == 0) dotstar.drawPixel(3, 0, dotstar.Color(0,0,255)); // Up
  else if (data.joystickDir == 1) dotstar.drawPixel(3, 7, dotstar.Color(0,0,255)); // Down
  else if (data.joystickDir == 2) dotstar.drawPixel(0, 3, dotstar.Color(0,0,255)); // Left
  else if (data.joystickDir == 3) dotstar.drawPixel(7, 3, dotstar.Color(0,0,255)); // Right
  else dotstar.fillScreen(dotstar.Color(data.colorR>>2, data.colorG>>2, data.colorB>>2)); // Show color
  dotstar.show();

  // FastLED: color bar with button brightness
  uint8_t brightness = data.joystickBtn ? 255 : 64;
  for (int i = 0; i < NUM_LEDS; i++) {
    leds[i] = CRGB(data.colorR>>4, data.colorG>>4, data.colorB>>4);
    leds[i].nscale8_video(brightness);
  }
  FastLED.show();

  // OLED: show values
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("Dir: ");
  switch (data.joystickDir) {
    case 0: display.println("Up"); break;
    case 1: display.println("Down"); break;
    case 2: display.println("Left"); break;
    case 3: display.println("Right"); break;
    default: display.println("None"); break;
  }
  display.print("Btn: "); display.println(data.joystickBtn ? "Pressed" : "Released");
  display.print("R: "); display.println(data.colorR);
  display.print("G: "); display.println(data.colorG);
  display.print("B: "); display.println(data.colorB);
  display.print("C: "); display.println(data.colorC);
  display.display();
}

// ============ SETUP FUNCTION =============
void setup() {
  Serial.begin(115200);

  // --- Initialize PublicI2C as Slave ---
  PublicI2C.begin(PUBLIC_I2C_ADDR, PUBLIC_I2C_SDA, PUBLIC_I2C_SCL, 100000);
  PublicI2C.onReceive(receiveEvent);
  PublicI2C.onRequest(requestEvent);
  Serial.println("PublicI2C (slave) initialized.");

  // --- Initialize LocalI2C as Master ---
  LocalI2C.begin(LOCAL_I2C_SDA, LOCAL_I2C_SCL, 100000);
  Serial.println("LocalI2C (master) initialized.");

  // --- Initialize OLED Display ---
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 allocation failed");
    while (1);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("OLED Ready!");
  display.display();

  // --- Initialize APDS-9960 (SparkFun) ---
  if (!apds.init()) { // Uses LocalI2C if library supports it
    Serial.println("APDS-9960 initialization failed");
    display.println("No APDS-9960!");
    display.display();
    while (1);
  }
  display.println("APDS-9960 ready!");
  display.display();

  // Enable color sensing
  apds.enableLightSensor(false);

  // --- Initialize Joystick Pins ---
  pinMode(JOY_BTN_PIN, INPUT_PULLUP); // Button is active LOW

  // --- Initialize FastLED (NeoPixel) ---
  FastLED.addLeds<NEOPIXEL, LED_DATA_PIN>(leds, NUM_LEDS);
  FastLED.clear();
  FastLED.show();

  // --- Initialize DotStar Matrix ---
  dotstar.begin();
  dotstar.setBrightness(32); // Adjust as needed
  dotstar.fillScreen(0);
  dotstar.show();
}

// ============ MAIN LOOP ==================
void loop() {
  SensorData tempData;
  updateJoystick(tempData);
  updateColorSensor(tempData);

  // Copy to shared struct for I2C
  noInterrupts();
  latestData = tempData;
  interrupts();

  updateDisplays(tempData);

  delay(50); // Adjust as needed
}