#include <Adafruit_NeoPixel.h>
#include <Servo.h>
#include <ArduinoJson.h>
#include "AITINKR_SHIELDS.h"

// Define the pin connected to the NeoPixel data input.
#define LED_PIN     4

// Define the total number of LEDs on your strip.
#define NUM_LEDS    16

// Create an instance of the Adafruit_NeoPixel object.
Adafruit_NeoPixel pixels(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);

struct ColorEntry {
  const char* name;
  uint8_t r, g, b;
};

const ColorEntry colorPalette[] = {
  // Add color palette here...
};

const int NUM_COLORS = sizeof(colorPalette) / sizeof(colorPalette[0]);

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
motor Motor;

void setup() {
  pixels.begin();
  pixels.show();

  // Attach servos to their pins
  servo1.attach(3);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);

  // Set all servos to closed position
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);

  // Initialize serial and motor
  Serial.begin(9600);
  Motor.init();
  Motor.setSpeed(255); // default speed

  Serial.println("Ready! Send JSON like {\"1\":\"o\", \"3\":\"c\", \"d\":\"f\", \"s\":200, \"l\":\"red\"}");
  Serial.println("o=open=90°, c=close=0° | d=direction(f,b,l,r,s), s=speed 0-255, l=color");
}

void loop() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    StaticJsonDocument<256> doc;
    DeserializationError error = deserializeJson(doc, input);

    if (error) {
      Serial.print("JSON Error: ");
      Serial.println(error.c_str());
      return;
    }

    // ---- Servo Handling ----
    for (int i = 1; i <= 4; i++) {
      const char* val = doc[String(i)];
      if (val != nullptr) {
        int angle = 0;
        if (strcmp(val, "o") == 0) angle = 90;
        else if (strcmp(val, "c") == 0) angle = 0;
        else {
          Serial.print("Invalid servo command for servo ");
          Serial.println(i);
          continue;
        }

        switch (i) {
          case 1: servo1.write(angle); break;
          case 2: servo2.write(angle); break;
          case 3: servo3.write(angle); break;
          case 4: servo4.write(angle); break;
        }

        Serial.print("Servo ");
        Serial.print(i);
        Serial.print(" set to ");
        Serial.print(angle);
        Serial.println(" degrees");
      }
    }

    // ---- Motor Handling ----
    const char* dir = doc["d"];
    int speed = doc["s"];

    if (!isnan(speed) && speed >= 0 && speed <= 255) {
      Motor.setSpeed(speed);
      Serial.print("Speed set to: ");
      Serial.println(speed);
    } else if (doc.containsKey("s")) {
      Serial.println("Invalid speed value.");
    }

    if (dir != nullptr) {
      if (strcmp(dir, "f") == 0) {
        Motor.forward();
        Serial.println("Moving forward.");
      } else if (strcmp(dir, "b") == 0) {
        Motor.backward();
        Serial.println("Moving backward.");
      } else if (strcmp(dir, "l") == 0) {
        Motor.left();
        Serial.println("Turning left.");
      } else if (strcmp(dir, "r") == 0) {
        Motor.right();
        Serial.println("Turning right.");
      } else if (strcmp(dir, "s") == 0) {
        Motor.stop();
        Serial.println("Stopping motors.");
      } else {
        Serial.println("Unknown direction command.");
      }
    }

    // ---- NeoPixel Handling ----
    const char* colorName = doc["l"];
    if (colorName != nullptr) {
      for (int i = 0; i < NUM_COLORS; i++) {
        if (strcmp(colorName, colorPalette[i].name) == 0) {
          uint32_t color = pixels.Color(colorPalette[i].r, colorPalette[i].g, colorPalette[i].b);
          for (int j = 0; j < NUM_LEDS; j++) {
            pixels.setPixelColor(j, color);
          }
          pixels.show();
          Serial.print("Color set to: ");
          Serial.println(colorName);
          return;
        }
      }

      Serial.print("Color not found: ");
      Serial.println(colorName);
    }
  }
  delay(10);
}