#include <SPI.h>
#include "Adafruit_Soundboard.h"
#include <WiFi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
//#include <OSCBundle.h>
#include <OSCData.h>

#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"

#define SFX_RST   21
#define SFX_ACT   26 // A0
#define RELE      13 

#define CE_PIN   25 // A1
#define CSN_PIN  4

#define B_VERDE  33  // VERDE
#define B_ROJO   27  // ROJO
#define B_NEGRO  12  // NEGRO

#define  UDP_RX_PACKET_MAX_SIZE  8  // SUBIR EN CASO DE APURO

int radioId; 
uint32_t CMD[ 8 ]; // CMD + Ch + 2
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
const uint64_t pipeNode = 0xE8E8F0F0E1LL; // Define the transmit pipe
//int ch = 0x4C ; // 76 por defecto es el canal 76 (showroom)

Adafruit_Soundboard sfx = Adafruit_Soundboard(&Serial1, NULL, SFX_RST);

char* ssid = "REGIDOR-AP";          // your network SSID (name)
char* pass = "123456789";    // your network password

// A UDP instance to let us send and receive packets over   UDP
WiFiUDP Udp;

const unsigned int outPort = 9999;          // remote port (not needed for receive)
const unsigned int localPort = 8888;        // local port to listen for UDP packets (here's where we send the packets)

OSCErrorCode error;
unsigned int ledState = LOW;              // LOW means led is *on*

unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 3000;           // interval at which to blink (milliseconds)

int last_button_rojo = 0;     // previous state of the button
int last_button_verde = 0;     // previous state of the button
int last_button_negro = 0;     // previous state of the button


#ifndef BUILTIN_LED
#ifdef LED_BUILTIN
#define BUILTIN_LED LED_BUILTIN
#else
#define BUILTIN_LED 13
#endif
#endif

String translateEncryptionType(wifi_auth_mode_t encryptionType) {
 
  switch (encryptionType) {
    case (WIFI_AUTH_OPEN):
      return "Open";
    case (WIFI_AUTH_WEP):
      return "WEP";
    case (WIFI_AUTH_WPA_PSK):
      return "WPA_PSK";
    case (WIFI_AUTH_WPA2_PSK):
      return "WPA2_PSK";
    case (WIFI_AUTH_WPA_WPA2_PSK):
      return "WPA_WPA2_PSK";
    case (WIFI_AUTH_WPA2_ENTERPRISE):
      return "WPA2_ENTERPRISE";
  }
}
 
void scanNetworks() {
 
  int numberOfNetworks = WiFi.scanNetworks();
 
  Serial.print("Number of networks found: ");
  Serial.println(numberOfNetworks);
 
  for (int i = 0; i < numberOfNetworks; i++) {
 
    Serial.print("Network name: ");
    Serial.println(WiFi.SSID(i));
 
    Serial.print("Signal strength: ");
    Serial.println(WiFi.RSSI(i));
 
    Serial.print("MAC address: ");
    Serial.println(WiFi.BSSIDstr(i));
 
    Serial.print("Encryption type: ");
    String encryptionTypeDescription = translateEncryptionType(WiFi.encryptionType(i));
    Serial.println(encryptionTypeDescription);
    Serial.println("-----------------------");
 
  }
}


void flushInput() {
  // Read all available serial input to flush pending data.
  uint16_t timeoutloop = 0;
  while (timeoutloop++ < 40) {
    while(Serial1.available()) {
      Serial1.read();
      timeoutloop = 0;  // If char was received reset the timer
    }
    delay(1);
  }
}

void led(OSCMessage &msg)   // ACTIVA EL LED Y EL RELE 
{
  ledState = (int)msg.getFloat(0);
  //ledState = msg.getInt(0);
  digitalWrite(BUILTIN_LED, ledState);
  Serial.print("led: ");
  Serial.println(ledState);
}

void RF_scene(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
  
  //if (msg.isFloat(0)){        
  //  CMD[0] = msg.getFloat(0);
  //}
  
  CMD[0] = 1;
  if (msg.isFloat(0)){        
    CMD[1] = msg.getFloat(0);
  } 
  
  Serial.print("Radio scene: ");
  Serial.print(CMD[0]); Serial.print("/"); Serial.println(CMD[1]); 
  
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
  
  if(CMD[1] == 0)
    playtrackButton(17); // disparo audio remoto test
  else
    safeStop();
    

}

void RF_m1(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
  
  //if (msg.isFloat(0)){        
  //  CMD[0] = msg.getFloat(0);
  //}
  
  CMD[0] = 2;
  if (msg.isFloat(0)){        
    CMD[1] = msg.getFloat(0); // Valores entre 0 y 1023 de posicion
  } 
  
  Serial.print("Radio M1: ");
  Serial.print(CMD[0]); Serial.print("/"); Serial.println(CMD[1]); 
  
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

}

void RF_m2(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
  
  //if (msg.isFloat(0)){        
  //  CMD[0] = msg.getFloat(0);
  //}
  
  CMD[0] = 3;
  if (msg.isFloat(0)){        
    CMD[1] = msg.getFloat(0); // Valores entre 0 y 1023 de posicion
  } 
  
  Serial.print("Radio M2: ");
  Serial.print(CMD[0]); Serial.print("/"); Serial.println(CMD[1]); 
  
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

}


void RF_m3(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
  
  //if (msg.isFloat(0)){        
  //  CMD[0] = msg.getFloat(0);
  //}
  
  CMD[0] = 4;
  if (msg.isFloat(0)){        
    CMD[1] = msg.getFloat(0); // Valores entre 0 y 1023 de posicion
  } 
  
  Serial.print("Radio M3: ");
  Serial.print(CMD[0]); Serial.print("/"); Serial.println(CMD[1]); 
  
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

}

void RF_m4(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
  
  //if (msg.isFloat(0)){        
  //  CMD[0] = msg.getFloat(0);
  //}
  
  CMD[0] = 5;
  if (msg.isFloat(0)){        
    CMD[1] = msg.getFloat(0); // Valores entre 0 y 1023 de posicion
  } 
  
  Serial.print("Radio M4: ");
  Serial.print(CMD[0]); Serial.print("/"); Serial.println(CMD[1]); 
  
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

}


void RF_r1(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
}

void RF_r2(OSCMessage &msg)   // FUNCION POR OSC PARA CAMBIO DE ESCENA 
{
}

void safeStop() {
  int isPlaying = digitalRead(SFX_ACT);
  if (isPlaying == LOW) {
    Serial.println("Stopping...");
    sfx.stop();
  }
}

void playtrack(OSCMessage &msg) 
{

  flushInput() ;
  safeStop();
  
  uint8_t track = (int)msg.getFloat(0);
  Serial.print("\nPlaying track #"); Serial.println(track);
  if (! sfx.playTrack((uint8_t)track) ) 
  {
    Serial.println("Failed to play track?");
  }
}

void playtrackButton( uint8_t track)  // Para disparar el audio desde los botones
{

  flushInput() ;
  safeStop();
  
  Serial.print("\nPlaying track #"); Serial.println(track);
  if (! sfx.playTrack((uint8_t)track) ) 
  {
    Serial.println("Failed to play track?");
  }
}


void setup() 
{
  pinMode(BUILTIN_LED, OUTPUT);
  digitalWrite(BUILTIN_LED, ledState);    // turn *on* led
  
  pinMode(SFX_ACT, INPUT);

  Serial.begin(115200);
  Serial1.begin(9600);
  
  if (!sfx.reset()) {
    Serial.println("Not found");
    while (1);
  }
  Serial.println("SFX board found");
  //scanNetworks();

// Configures static IP address
//  if (!WiFi.config(local_IP, gateway, subnet)) {
//    Serial.println("STA Failed to configure");
//  }

  WiFi.softAP(ssid, pass);
  Serial.println();
  Serial.print("IP address: ");
  Serial.println(WiFi.softAPIP());

  WiFi.setSleep(false); // IMPORTANTE PARA QUE EL OSC VAYA FINO!

  Serial.println("Starting UDP");
  Udp.begin(localPort);
  Serial.print("Local port: ");
  Serial.println(localPort);

  radio.begin();
  radio.setDataRate( RF24_250KBPS );
  radio.setPALevel( RF24_PA_MAX );
  radio.setAutoAck(0);
  radio.setPayloadSize(8);
  radio.setChannel(76); 
  radio.openWritingPipe(pipeNode);
  radio.printDetails();
  //printf_begin();

  pinMode(B_ROJO, INPUT_PULLUP);    // BOTON 1
  pinMode(B_VERDE, INPUT_PULLUP);  // BOTON 2
  pinMode(B_NEGRO, INPUT_PULLUP);  // BOTON 3
  
//  pinMode(33, INPUT_PULLUP);  // VERDE
//  pinMode(27, INPUT_PULLUP);  // ROJO
//  pinMode(12, INPUT_PULLUP);  // NEGRO

  
}



void loop() 
{


  //////////////////////////////////////////////////////////////////
  // RUTINA OSC POR WIFI
  //////////////////////////////////////////////////////////////////
 
  OSCMessage msg;
  int size = Udp.parsePacket();

  if (size > 0) {
    while (size--) {
      //msg.fill(Udp.read());
      uint8_t packetBuffer[UDP_RX_PACKET_MAX_SIZE];
      Udp.read(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
      msg.fill(packetBuffer, UDP_RX_PACKET_MAX_SIZE);
    }
    if (!msg.hasError()) {
      msg.dispatch("/led", led); // LED + RELE
      msg.dispatch("/track", playtrack);
      msg.dispatch("/RF/scene", RF_scene);
      msg.dispatch("/RF/motor1", RF_m1); 
      msg.dispatch("/RF/motor2", RF_m2);
      msg.dispatch("/RF/hmotor1", RF_m3); 
      msg.dispatch("/RF/hmotor2", RF_m4);
      msg.dispatch("/RF/rele1", RF_r1);
      msg.dispatch("/RF/rele2", RF_r2);
    }    
    else {
      error = msg.getError();
      Serial.print("error: ");
      Serial.println(error);
    }
  }

  
/*    
////////////////////////////////////////////////////////////////////////////
// TEST CONTADOR 
////////////////////////////////////////////////////////////////////////////
  CMD[0]++;
  if(CMD[0]>255)
    CMD[0] = 0;

  Serial.println(CMD[0]);
  radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
  
  delay(30);



  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) 
  {
    // save the last time you blinked the LED
    previousMillis = currentMillis;
    CMD[0] = 1; // comando escena
    CMD[1]++;
    if(CMD[1]>3)
      CMD[1] = 0;

    Serial.println(CMD[1]);
    radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA

  }
*/

///////////////////////////////////////////////////
// RUTINA DE BOTONES DE ESCENAS
///////////////////////////////////////////////////


  int button_rojo = digitalRead(B_ROJO);
  int button_verde = digitalRead(B_VERDE);
  int button_negro = digitalRead(B_NEGRO);

///////////////////////////////////////////////////
///// BOTON ROJO
///////////////////////////////////////////////////

  if (button_rojo != last_button_rojo) 
  {
    if (button_rojo == LOW) {
      Serial.println("Rojo on");
      CMD[0] = 1; // comando escena
      CMD[1] = 0; // ESCENA ROJO
      radio.write( CMD, sizeof(CMD) );   
      playtrackButton(17); // disparo audio remoto test
    } else {
      Serial.println("Rojo off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  last_button_rojo = button_rojo;

///////////////////////////////////////////////////
///// BOTON VERDE
///////////////////////////////////////////////////

  if (button_verde != last_button_verde) 
  {
    if (button_verde == LOW) {
      Serial.println("Verde on");
      CMD[0] = 1; // comando escena
      CMD[1] = 1; // ESCENA VERDE
      radio.write( CMD, sizeof(CMD) ); 
      //playtrackButton(17); // disparo audio remoto
      safeStop();
    } else {
      Serial.println("Verde off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  last_button_verde = button_verde;

///////////////////////////////////////////////////
///// BOTON NEGRO
///////////////////////////////////////////////////

  if (button_negro != last_button_negro) 
  {
    if (button_negro == LOW) {
      Serial.println("Negro on");
      CMD[0] = 1; // comando escena
      CMD[1] = 2; // ESCENA AZUL
      radio.write( CMD, sizeof(CMD) ); 

      //playtrackButton(17); // disparo audio remoto
    } else {
      Serial.println("Negro off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  last_button_negro = button_negro;


  
/*
  if (button1 == LOW) {
    delay(10); 
    if(digitalRead(B1) == LOW)
    {
      CMD[0] = 1; // comando escena
      CMD[1]++;
      if(CMD[1]>3)
        CMD[1] = 0;

    Serial.println(CMD[1]);
    radio.write( CMD, sizeof(CMD) ); // STREAM SIN CONTROL DE LLEGADA
    
    }
    
  } 
*/
  
}