/*
 * SMART COLOR - ESP32 DevKit V1
 * Sistema de Controle de Motor de Passo para Separação de Tampas
 * 
 * Hardware:
 * - ESP32 DevKit V1
 * - Motor de Passo (ex: 28BYJ-48 com driver ULN2003 ou NEMA 17 com A4988)
 * - 7 posições para cores: Branco, Preto, Amarelo, Verde, Azul, Vermelho, Laranja
 * 
 * Conexões (exemplo com driver A4988):
 * - STEP_PIN (GPIO 26) -> STEP do driver
 * - DIR_PIN (GPIO 27) -> DIR do driver
 * - ENABLE_PIN (GPIO 25) -> ENABLE do driver
 * 
 * Comunicação Serial: 115200 baud
 * Formato JSON: {"color":"AZUL","position":4}
 */

#include <ArduinoJson.h>
#include <AccelStepper.h>

// ========== CONFIGURAÇÕES ==========

// Pinos do motor de passo (A4988/DRV8825)
#define STEP_PIN 26
#define DIR_PIN 27
#define ENABLE_PIN 25

// Configurações do motor
#define STEPS_PER_REVOLUTION 200  // Motor NEMA 17: 200 passos/revolução (1.8° por passo)
#define MICROSTEPS 16             // Microstepping do driver (1, 2, 4, 8, 16)
#define TOTAL_STEPS (STEPS_PER_REVOLUTION * MICROSTEPS)  // 3200 passos/revolução com 1/16

// Configurações de movimento
#define MAX_SPEED 2000            // Velocidade máxima (passos/segundo)
#define ACCELERATION 1000         // Aceleração (passos/segundo²)
#define POSITIONS 7               // Número de posições (cores)

// LED de status
#define LED_PIN 2                 // LED interno do ESP32

// ========== VARIÁVEIS GLOBAIS ==========

// Motor de passo
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Posições das cores (em passos)
long colorPositions[POSITIONS];

// Posição atual
int currentPosition = 0;

// Mapeamento de cores
const char* colorNames[POSITIONS] = {
  "BRANCO",   // Posição 0
  "PRETO",    // Posição 1
  "AMARELO",  // Posição 2
  "VERDE",    // Posição 3
  "AZUL",     // Posição 4
  "VERMELHO", // Posição 5
  "LARANJA"   // Posição 6
};

// Buffer serial
String inputBuffer = "";

// Estatísticas
unsigned long totalProcessed = 0;
unsigned long colorCount[POSITIONS] = {0};

// ========== FUNÇÕES ==========

void setup() {
  // Inicializa serial
  Serial.begin(115200);
  while (!Serial) delay(10);
  
  Serial.println("\n========================================");
  Serial.println("SMART COLOR - ESP32 Controller");
  Serial.println("Sistema de Separacao de Tampas PET");
  Serial.println("========================================\n");
  
  // Configura pinos
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);  // Habilita motor (LOW = habilitado)
  
  // Configura motor de passo
  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCELERATION);
  stepper.setCurrentPosition(0);
  
  // Calcula posições das cores (distribuídas uniformemente em 360°)
  calculatePositions();
  
  // Teste inicial
  Serial.println("Iniciando teste de posicoes...");
  testPositions();
  
  Serial.println("\nSistema pronto!");
  Serial.println("Aguardando comandos via serial...\n");
  
  // Pisca LED
  blinkLED(3, 200);
}

void loop() {
  // Lê dados da serial
  while (Serial.available() > 0) {
    char c = Serial.read();
    
    if (c == '\n') {
      // Processa comando
      processCommand(inputBuffer);
      inputBuffer = "";
    } else {
      inputBuffer += c;
    }
  }
  
  // Atualiza motor
  stepper.run();
}

void calculatePositions() {
  /*
   * Calcula as posições angulares para cada cor
   * Distribui 7 posições em 360° (aproximadamente 51.4° entre cada)
   */
  Serial.println("Calculando posicoes das cores:");
  
  for (int i = 0; i < POSITIONS; i++) {
    // Ângulo em graus
    float angle = (360.0 / POSITIONS) * i;
    
    // Converte para passos
    colorPositions[i] = (long)((angle / 360.0) * TOTAL_STEPS);
    
    Serial.print("  ");
    Serial.print(colorNames[i]);
    Serial.print(" (Pos ");
    Serial.print(i);
    Serial.print("): ");
    Serial.print(angle, 1);
    Serial.print("° = ");
    Serial.print(colorPositions[i]);
    Serial.println(" passos");
  }
  Serial.println();
}

void processCommand(String command) {
  /*
   * Processa comando JSON recebido via serial
   * Formato: {"color":"AZUL","position":4}
   */
  
  // Parse JSON
  StaticJsonDocument<200> doc;
  DeserializationError error = deserializeJson(doc, command);
  
  if (error) {
    Serial.print("Erro ao parsear JSON: ");
    Serial.println(error.c_str());
    return;
  }
  
  // Extrai dados
  const char* color = doc["color"];
  int position = doc["position"];
  
  // Valida posição
  if (position < 0 || position >= POSITIONS) {
    Serial.print("Erro: Posicao invalida: ");
    Serial.println(position);
    return;
  }
  
  // Confirma recebimento
  Serial.print("Comando recebido: ");
  Serial.print(color);
  Serial.print(" -> Posicao ");
  Serial.println(position);
  
  // Move para a posição
  moveToPosition(position);
  
  // Atualiza estatísticas
  colorCount[position]++;
  totalProcessed++;
  
  // Envia confirmação
  Serial.print("OK - Tampa processada. Total: ");
  Serial.println(totalProcessed);
}

void moveToPosition(int position) {
  /*
   * Move o motor para a posição especificada
   */
  
  if (position == currentPosition) {
    Serial.println("Ja na posicao correta");
    return;
  }
  
  // Acende LED
  digitalWrite(LED_PIN, HIGH);
  
  // Calcula movimento
  long targetSteps = colorPositions[position];
  long currentSteps = colorPositions[currentPosition];
  long delta = targetSteps - currentSteps;
  
  // Otimiza movimento (menor caminho)
  if (abs(delta) > TOTAL_STEPS / 2) {
    if (delta > 0) {
      delta -= TOTAL_STEPS;
    } else {
      delta += TOTAL_STEPS;
    }
  }
  
  Serial.print("Movendo de ");
  Serial.print(colorNames[currentPosition]);
  Serial.print(" para ");
  Serial.print(colorNames[position]);
  Serial.print(" (");
  Serial.print(delta);
  Serial.println(" passos)");
  
  // Move motor
  stepper.move(delta);
  
  // Aguarda conclusão
  while (stepper.distanceToGo() != 0) {
    stepper.run();
  }
  
  // Atualiza posição
  currentPosition = position;
  
  // Apaga LED
  digitalWrite(LED_PIN, LOW);
  
  Serial.println("Movimento concluido!");
}

void testPositions() {
  /*
   * Testa todas as posições sequencialmente
   */
  Serial.println("Testando todas as posicoes...");
  
  for (int i = 0; i < POSITIONS; i++) {
    Serial.print("Posicao ");
    Serial.print(i);
    Serial.print(" - ");
    Serial.println(colorNames[i]);
    
    moveToPosition(i);
    delay(500);
  }
  
  // Retorna à posição inicial
  Serial.println("Retornando a posicao inicial...");
  moveToPosition(0);
  
  Serial.println("Teste concluido!\n");
}

void blinkLED(int times, int delayMs) {
  /*
   * Pisca o LED
   */
  for (int i = 0; i < times; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(delayMs);
    digitalWrite(LED_PIN, LOW);
    delay(delayMs);
  }
}

void printStatistics() {
  /*
   * Imprime estatísticas de processamento
   */
  Serial.println("\n========== ESTATISTICAS ==========");
  Serial.print("Total processado: ");
  Serial.println(totalProcessed);
  Serial.println("\nPor cor:");
  
  for (int i = 0; i < POSITIONS; i++) {
    Serial.print("  ");
    Serial.print(colorNames[i]);
    Serial.print(": ");
    Serial.print(colorCount[i]);
    
    if (totalProcessed > 0) {
      float percentage = (colorCount[i] * 100.0) / totalProcessed;
      Serial.print(" (");
      Serial.print(percentage, 1);
      Serial.print("%)");
    }
    Serial.println();
  }
  
  Serial.println("==================================\n");
}

// Comandos adicionais via Serial Monitor
void serialEvent() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    
    if (cmd == "STATS") {
      printStatistics();
    } else if (cmd == "HOME") {
      Serial.println("Retornando a posicao inicial...");
      moveToPosition(0);
    } else if (cmd == "TEST") {
      testPositions();
    } else if (cmd.startsWith("POS")) {
      int pos = cmd.substring(3).toInt();
      if (pos >= 0 && pos < POSITIONS) {
        moveToPosition(pos);
      }
    }
  }
}