#include "bhoreal.h"
#include "Adafruit_NeoPixel.h"

#if MODEL == SLIMPRO
  const char mySSID[] = "hangar_oficines";  
  const char myPass[] = "m1cr0fug4s";
  const char *IP = "172.26.0.255";
  
//  const char mySSID[] = "Mi$Red";  
//  const char myPass[] = "FINALFANTASY";
//  const char *IP = "192.168.0.255";
  const char myAuth[] = WPA2;
  const char antenna[] = INT_ANT;
  const uint16_t outPort = 8080;
  const uint16_t localPort = 8000;   
#endif

#define MESSAGE_SIZE 36
char OSC_SEND[MESSAGE_SIZE] = { // Message template
  '/', 'b', 'h', 'o',
  'r', 'e', 'a', 'l',
  '/', 'p', 'r', 'e',   
  's', 's',  B0,  B0,
  ',', 'i', 'i', 'i', 
  B0 , B0 ,  B0,  B0,
  B0 , B0 ,  B0,  B0,
  B0 , B0 ,  B0,  B0,
  B0 , B0 ,  B0,  B0
};

byte tempR;
byte tempC;
byte lastread;
byte command = 0;
boolean ready = true;
boolean refresh_ok = false;
uint16_t IntensityMAX = 255;

// Default draw colour. Each channel can be between 0 and 4095.
int red = 0;
int green = 0;
int blue = 0;

// Auxiliary analog output definitions
#define ANALOG0 A5 //POTENCIOMETRO
#define ANALOG1 A1
boolean adc[2] = { //On or off state
  0, 0};
byte analogval[2]; //The last reported value
byte tempADC; //Temporary storage for comparison purposes

  #if (MODEL == SLIM)||(MODEL == SLIMPRO)
    // Pin definitions for the 74HC164 SIPO shift register (drives button rows high)
    #define DATAPIN     9 // aka analog pin 2 (what, you didn't know that analog pins 0-5 are also digital pins 14-19? Well, now you do!)
    #define CLOCKPIN    8
    // Pin definitions for the 74HC165 PISO shift register (reads button column state)
    #define INDATAPIN  13
    #define INCLOCKPIN  5
    #define INLOADPIN  10 // toggling this tell the 165 to read the value into its memory for reading
    
    #define FACTORY  A5 // toggling this tell the 165 to read the value into its memory for reading
    #define AWAKE  22 // AWAKE WIFLY
    #define DTR 11
    #define MUX 12
    #define BOT 7
    
    uint16_t MAX   =  8; 
    int NUM_LEDS   =  64; 
    #define PIN 6
    Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRB + NEO_KHZ800);
    #if MODEL == SLIMPRO

    #endif
  #else
    uint16_t MAX   =  4; 
    int NUM_LEDS   =  16;  
    #define PIN 11
    Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRB + NEO_KHZ800);
    byte row[4]    = {
      13, 5, 10, 9};
    byte column[4] = {
      8, 6, 12, 4};
  #endif
  
boolean pressed[8][8] = {
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,1}
};

const byte remapMini[4][4] = 
{
  { 3, 4, 11, 12 }, 
  { 2, 5, 10, 13 }, 
  { 1, 6,  9, 14 }, 
  { 0, 7,  8, 15 } 
};

byte remapSlim[8][8] = {
  {7,8,23,24,       39,40,55,56}, 
  {6,9,22,25,       38,41,54,57}, 
  {5,10,21,26,      37,42,53,58}, 
  {4,11,20,27,      36,43,52,59}, 
  {3,12,19,28,      35,44,51,60}, 
  {2,13,18,29,      34,45,50,61}, 
  {1,14,17,30,      33,46,49,62}, 
  {0,15,16,31,      32,47,48,63}, 
};

int levelR[64] = {
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0, 
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0};
int levelG[64] = {
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0, 
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0};
int levelB[64] = {
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0, 
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 0, 0, 0, 0, 0};

//////////////////////////////////////////////////////////////////////
//////////////////////       BHOREAL BEGIN      //////////////////////
//////////////////////////////////////////////////////////////////////

void Bhoreal::begin(uint32_t BAUD)
{
   #if (MODEL == SLIM)||(MODEL == SLIMPRO)
     // 165 Setup
    pinMode(INDATAPIN, INPUT);
    pinMode(INCLOCKPIN, OUTPUT);
    pinMode(INLOADPIN, OUTPUT);
  
    // 164 Setup
    pinMode(DATAPIN, OUTPUT);
    pinMode(CLOCKPIN, OUTPUT);
    
    pinMode(DTR, OUTPUT);
    pinMode(MUX, OUTPUT);
    pinMode(AWAKE, OUTPUT);
    pinMode(BOT, INPUT);
    pinMode(FACTORY, OUTPUT);
    digitalWrite(FACTORY, LOW); 
    digitalWrite(AWAKE, HIGH); 
    digitalWrite(MUX, HIGH); //Modo Wifly ON
    digitalWrite(DTR, HIGH); //Resetear atmega328 OFF
    // Start the serial port
    Serial.begin(9600); //USB inicializado a 9600
    #if MODEL == SLIMPRO
        Serial1.begin(9600); //WIFI inicializado a 9600
        if (Connect()) Serial.println("Conectado!!");
        else Serial.println("Desconectado :(");
    #endif
    strip.begin();
    strip.show();
    AttachInterrupt6(RISING);
  #else
    for(byte i = 0; i<4; i++) 
    {
      pinMode(column[i], INPUT);
      pinMode(row[i], OUTPUT);
      digitalWrite(row[i], LOW);
    }
    /* Setup the timer interrupt*/
    strip.begin();
    strip.show();
    PORTE |= B01000000;
    DDRE  |= B01000000;
  
    timer1Initialize();
    //timer3Initialize(); // Disable Serial interrupt!
   #endif

}



////////////////////////////////////////////////////////////////
//////////////////////       STARTUP      //////////////////////
////////////////////////////////////////////////////////////////

// Run this animation once at startup. Currently unfinished.
void Bhoreal::startup(){
  
  for(int x = 0; x < NUM_LEDS; ++x){ 
     #if (MODEL == SLIM)||(MODEL == SLIMPRO)
      uint32_t c = hue2rgb(x*2);
      uint8_t
      r = (uint8_t)(c >> 16),
      g = (uint8_t)(c >>  8),
      b = (uint8_t)c;
      levelR[remapSlim[x>>3][x%8]] = r;
      levelG[remapSlim[x>>3][x%8]] = g;
      levelB[remapSlim[x>>3][x%8]] = b;
    #else
      uint32_t c = hue2rgb(x*8);
      uint8_t
      r = (uint8_t)(c >> 16),
      g = (uint8_t)(c >>  8),
      b = (uint8_t)c;
      levelR[remapMini[x>>2][x%4]] = r;
      levelG[remapMini[x>>2][x%4]] = g;
      levelB[remapMini[x>>2][x%4]] = b;
    #endif
  }  
    
  for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, levelR[x], levelG[x], levelB[x]);
  strip.show();
  
}

//////////////////////////////////////////////////////////////////////
//////////////////////  SERIAL PRESS & RELEASE  //////////////////////
//////////////////////////////////////////////////////////////////////


void Bhoreal::on_press(byte r, byte c){

#if SERIAL_ENABLE
  Serial.print(1);
  Serial.print(" ");
  Serial.println( (r << 4) | c, HEX);
#endif
   #if (MODEL == SLIM)||(MODEL == SLIMPRO)
    MIDIEvent e1 = { 0x09, 0x90, ((r << 3) | c) , 64  };
    #if MODEL == SLIMPRO
      OSCSend(r, c, true);
    #endif
  #else
    MIDIEvent e1 = { 0x09, 0x90, ((r << 2) | c) , 64  };
  #endif
  MIDIUSB.write(e1);

}

void Bhoreal::on_release(byte r, byte c){
  
#if SERIAL_ENABLE
  Serial.print(0);
  Serial.print(" ");
  Serial.println( (r << 4) | c, HEX); 
#endif
   #if (MODEL == SLIM)||(MODEL == SLIMPRO)
    MIDIEvent e1 = { 0x09, 0x90, ((r << 3) | c) , 0  };
    #if MODEL == SLIMPRO
      OSCSend(r, c, false);
    #endif
  #else
    MIDIEvent e1 = { 0x09, 0x90, ((r << 2) | c) , 0  };
  #endif

  MIDIUSB.write(e1);

}



///////////////////////////////////////////////////////////////
//////////////////////   CHECK BUTTONS   //////////////////////
///////////////////////////////////////////////////////////////

boolean bot_state = true;
boolean state_ok = false;

void Bhoreal::checkButtons(){
   #if (MODEL == SLIM)||(MODEL == SLIMPRO)
    if ((!bot_state)&&(!state_ok))
    {
      detachInterrupt6();
      //for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, 0, 0, 0);
      strip.show();
      #if (MODEL == SLIMPRO)
        sleep();
        digitalWrite(AWAKE, LOW);
      #endif
      state_ok = true;
      AttachInterrupt6(RISING);
      //sleepNow();
    }
    else if ((bot_state)&&(state_ok))
    {
      detachInterrupt6();
      #if (MODEL == SLIMPRO)
        digitalWrite(AWAKE, HIGH);
      #endif
      //for(int x = 0; x < NUM_LEDS; ++x) strip.setPixelColor(x, levelR[x], levelG[x], levelB[x]);
      strip.show();
      AttachInterrupt6(RISING);
      state_ok = false;
    }
    
    digitalWrite(CLOCKPIN,LOW);
    digitalWrite(DATAPIN, HIGH);
    
     for(byte c = 0; c < MAX; c++){
      digitalWrite(CLOCKPIN, HIGH);
  
      digitalWrite(INLOADPIN, LOW); // read into register
      digitalWrite(INLOADPIN, HIGH); // done reading into register, ready for us to read
  
      for(int r= MAX/2; r < MAX; r++){ // read each of the 165's 8 inputs (or its snapshot of it rather)
  
        // tell the 165 to send the first inputs pin state
        digitalWrite(INCLOCKPIN, LOW);
        // read the current output
        //int tempvalue = digitalRead(INDATAPIN);
        //Serial.print(tempvalue,DEC);
  
        if(pressed[c][r] != digitalRead(INDATAPIN)){ // read the state
          pressed[c][r] = digitalRead(INDATAPIN);
          if(!pressed[c][r]){
            on_press(c, r);
          }
          else {
            on_release(c, r);
          }
        }
      // tell the 165 we are done reading the state, the next inclockpin=0 will output the next input value
      digitalWrite(INCLOCKPIN, 1);
      }
      
      for(int r= 0; r < MAX/2; r++){ // read each of the 165's 8 inputs (or its snapshot of it rather)
  
        // tell the 165 to send the first inputs pin state
        digitalWrite(INCLOCKPIN, LOW);
        // read the current output
        //int tempvalue = digitalRead(INDATAPIN);
        //Serial.print(tempvalue,DEC);
  
        if(pressed[c][r] != digitalRead(INDATAPIN)){ // read the state
          pressed[c][r] = digitalRead(INDATAPIN);
          if(!pressed[c][r]){
            on_press(c, r);
          }
          else {
            on_release(c, r);
          }
        }
      // tell the 165 we are done reading the state, the next inclockpin=0 will output the next input value
      digitalWrite(INCLOCKPIN, 1);
      }
      digitalWrite(CLOCKPIN, LOW);
      digitalWrite(DATAPIN, LOW);
      
  
     }
  #else
    for(byte c = 0; c < MAX; c++)
    { 
      digitalWrite(row[c],HIGH);
      for(int r= MAX - 1; r >= 0; r--)
      {
        if(pressed[c][r] != digitalRead(column[r]))
        { // read the state
          delay(1); // Antirebotes!!!
          pressed[c][r] = digitalRead(column[r]);
          if(pressed[c][r]) on_press(c, r);
          else on_release(c, r);
        }
      }
      digitalWrite(row[c],LOW);
    } 
  #endif
}



////////////////////////////////////////////////////////////////
//////////////////////    REFRESH LED     //////////////////////
////////////////////////////////////////////////////////////////

unsigned long time = 0;

void Bhoreal::refresh(){ 
  if (refresh_ok) 
  {
    strip.show();
    refresh_ok=false;
  }
}



////////////////////////////////////////////////////////////////
////////////////////// REFRESH MIDI & LED  /////////////////////
////////////////////////////////////////////////////////////////


void Bhoreal::midiRefresh(){ 
  while(MIDIUSB.available() > 0) 
  {
    MIDIEvent e;
    e = MIDIUSB.read();

#if SERIAL_ENABLE
    if(MIDI_DEBUG)
    {
      if(e.type != 0x0F) // timestamp 1 BYTE
      {
        Serial.print("Midi Packet: ");
        Serial.print(e.type);
        Serial.print("\t");
        Serial.print(e.m1);
        Serial.print("\t");
        Serial.print(e.m2);
        Serial.print("\t");
        Serial.println(e.m3);
      }
    }
#endif
    #if (MODEL == SLIM)||(MODEL == SLIMPRO)
    if((e.type == 0x09) && (e.m3))  // mensaje de NoteON con velocidad mayor que cero
    {  
      uint32_t c = hue2rgb(e.m3);
      uint8_t
        r = (uint8_t)(c >> 16),
        g = (uint8_t)(c >>  8),
        b = (uint8_t)c;

      strip.setPixelColor(remapSlim[e.m2>>3][e.m2%8], r, g, b);
      strip.show();
    }
    else if( (e.type == 0x08) || ((e.type == 0x09) && !e.m3) ) // mensaje de NoteOFF
    {  
      strip.setPixelColor(remapSlim[e.m2>>3][e.m2%8], 0, 0, 0);
      strip.show();
    }  
    #else
    if((e.type == 0x09) && (e.m3))  // mensaje de NoteON con velocidad mayor que cero
    {  
      uint32_t c = hue2rgb(e.m3);
      uint8_t
        r = (uint8_t)(c >> 16),
        g = (uint8_t)(c >>  8),
        b = (uint8_t)c;

      strip.setPixelColor(remapMini[e.m2>>2][e.m2%4], r, g, b);
      strip.show();
    }
    else if( (e.type == 0x08) || ((e.type == 0x09) && !e.m3) ) // mensaje de NoteOFF
    {  
      strip.setPixelColor(remapMini[e.m2>>2][e.m2%4], 0, 0, 0);
      strip.show();
    }
    #endif
    MIDIUSB.flush(); // delete it???
  
  }
}

////////////////////////////////////////////////////////////////
//////////////////////  CHECK ADC INPUTS  //////////////////////
////////////////////////////////////////////////////////////////


void Bhoreal::checkADC(){
  // For all of the ADC's which are activated, check if the analog value has changed,
  // and send a message if it has.
  if(adc[0]){
    tempADC = (analogRead(ANALOG0) >> 2);
    if(abs((int)analogval[0] - (int)tempADC) > 3 ){
      analogval[0] = tempADC;
#if SERIAL_ENABLE
      Serial.write(14 << 4);
      Serial.write(analogval[0]);
#endif
    }
  }
  if(adc[1]){
    if(analogval[1] != (analogRead(ANALOG1) >> 2)){
      analogval[1] = (analogRead(ANALOG1) >> 2);
#if SERIAL_ENABLE
      Serial.write(14 << 4 | 1);
      Serial.write(analogval[1]);
#endif
    }
  }
}

///////////////////////////////////////////////////////////////
//////////////////////  TIMERS SETTINGS  //////////////////////
///////////////////////////////////////////////////////////////



#define RESOLUTION 65536    // Timer1 is 16 bit
unsigned int pwmPeriod;
unsigned char clockSelectBits;
char oldSREG;					// To hold Status 

void setPeriodTimer1(long microseconds)		// AR modified for atomic access
{

  long cycles = (F_CPU / 2000000) * microseconds;                                // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2
  if(cycles < RESOLUTION)              clockSelectBits = _BV(CS10);              // no prescale, full xtal
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11);              // prescale by /8
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10);  // prescale by /64
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12);              // prescale by /256
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12) | _BV(CS10);  // prescale by /1024
  else        cycles = RESOLUTION - 1, clockSelectBits = _BV(CS12) | _BV(CS10);  // request was out of bounds, set as maximum

  oldSREG = SREG;				
  cli();							// Disable interrupts for 16 bit register access
  ICR1 = pwmPeriod = cycles;                                          // ICR1 is TOP in p & f correct pwm mode
  SREG = oldSREG;

  TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
  TCCR1B |= clockSelectBits;                                          // reset clock select register, and starts the clock
}


void Bhoreal::timer1Initialize()
{
  TCCR1A = 0;                 // clear control register A 
  TCCR1B = _BV(WGM13);        // set mode 8: phase and frequency correct pwm, stop the timer
  setPeriodTimer1(5); 
  TIMSK1 = _BV(TOIE1);                                  
}
 
boolean flag = false;
 
ISR(TIMER1_OVF_vect)
{
       if (flag) { PORTE |= B01000000; flag=0;}
       else if (!flag) { PORTE &= B10111111; flag=1;}
 
}


void setPeriodTimer3(long microseconds)		// AR modified for atomic access
{

  long cycles = (F_CPU / 2000000) * microseconds;                                // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2
  if(cycles < RESOLUTION)              clockSelectBits = _BV(CS30);              // no prescale, full xtal
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS31);              // prescale by /8
  else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS31) | _BV(CS30);  // prescale by /64
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS32);              // prescale by /256
  else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS32) | _BV(CS30);  // prescale by /1024
  else        cycles = RESOLUTION - 1, clockSelectBits = _BV(CS32) | _BV(CS30);  // request was out of bounds, set as maximum

  oldSREG = SREG;				
  cli();							// Disable interrupts for 16 bit register access
  ICR3 = pwmPeriod = cycles;                                          // ICR1 is TOP in p & f correct pwm mode
  SREG = oldSREG;

  TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
  TCCR3B |= clockSelectBits;                                          // reset clock select register, and starts the clock
}

void Bhoreal::timer3Initialize()
{
  TCCR3A = 0;                 // clear control register A 
  TCCR3B = _BV(WGM33);        // set mode 8: phase and frequency correct pwm, stop the timer
  setPeriodTimer3(10000); 
  TIMSK3 = _BV(TOIE3);    

  //    TCCR3A = 0;
  //    TCCR3B = 0<<CS32 | 0<<CS31 | 1<<CS30;
  //    //Timer1 Overflow Interrupt Enable
  //    TIMSK3 = 1<<TOIE3;
}


///////////////////////////////////////////////////////////////
//////////////////////     HUE -> RGB    //////////////////////
///////////////////////////////////////////////////////////////

uint32_t Bhoreal::hue2rgb(uint16_t hueValue)
{
  
  uint8_t r;
  uint8_t g;
  uint8_t b;
  hueValue<<= 3;
  
  if (hueValue < 341)  { // Lowest third of the potentiometer's range (0-340)
    hueValue = (hueValue * 3) / 4; // Normalize to 0-255

    r = 255 - hueValue;  // Red from full to off
    g = hueValue;        // Green from off to full
    b = 1;             // Blue off
  }
  else if (hueValue < 682) { // Middle third of potentiometer's range (341-681)
    hueValue = ( (hueValue-341) * 3) / 4; // Normalize to 0-255

    r = 1;            // Red off
    g = 255 - hueValue; // Green from full to off
    b = hueValue;       // Blue from off to full
  }
  else  { // Upper third of potentiometer"s range (682-1023)
    hueValue = ( (hueValue-683) * 3) / 4; // Normalize to 0-255

    r = hueValue;       // Red from off to full
    g = 1;            // Green off
    b = 255 - hueValue; // Blue from full to off
  }
  
  return ((uint32_t)r << 16) | ((uint32_t)g <<  8) | b;

}

///////////////////////////////////////////////////////////////
//////////////////////     WIFI    //////////////////////
///////////////////////////////////////////////////////////////

 boolean FindInResponse(const char *toMatch,
                                    unsigned int timeOut = 1000) {
  int byteRead;

  unsigned long timeOutTarget; // in milliseconds

  for (unsigned int offset = 0; offset < strlen(toMatch); offset++) {
    timeOutTarget = millis() + timeOut; // Doesn't handle timer wrapping
    while (!Serial1.available()) {
      // Wait, with optional time out.
      if (timeOut > 0) {
        if (millis() > timeOutTarget) {
          return false;
        }
      }
      delay(1); // This seems to improve reliability slightly
    }
    byteRead = Serial1.read();
    //Serial.print((char)byteRead);
    delay(1); // Removing logging may affect timing slightly

    if (byteRead != toMatch[offset]) {
      offset = 0;
      // Ignore character read if it's not a match for the start of the string
      if (byteRead != toMatch[offset]) {
        offset = -1;
      }
      continue;
    }
  }

  return true;
}

boolean SendCommand(const __FlashStringHelper *command,
                                 boolean isMultipartCommand = false,
                                 const char *expectedResponse = "AOK") {
  Serial1.print(command);
  delay(20);
  if (!isMultipartCommand) {
    Serial1.flush();
    Serial1.println();

    // TODO: Handle other responses
    //       (e.g. autoconnect message before it's turned off,
    //        DHCP messages, and/or ERR etc)
    if (!FindInResponse(expectedResponse, 3000)) {
      return false;
    }
    //sckFindInResponse(expectedResponse);
  }
  return true;
}

boolean SendCommand(const char *command,
                                 boolean isMultipartCommand = false,
                                 const char *expectedResponse = "AOK") {
  Serial1.print(command);
  delay(20);
  if (!isMultipartCommand) {
    Serial1.flush();
    Serial1.println();

    // TODO: Handle other responses
    //       (e.g. autoconnect message before it's turned off,
    //        DHCP messages, and/or ERR etc)
    if (!FindInResponse(expectedResponse, 3000)) {
      return false;
    }
    //findInResponse(expectedResponse);
  }
  return true;
}

#define COMMAND_MODE_ENTER_RETRY_ATTEMPTS 2

#define COMMAND_MODE_GUARD_TIME 250 // in milliseconds

boolean EnterCommandMode() {
    for (int retryCount = 0; retryCount < COMMAND_MODE_ENTER_RETRY_ATTEMPTS; retryCount++) 
     {
      delay(COMMAND_MODE_GUARD_TIME);
      Serial1.print(F("$$$"));
      delay(COMMAND_MODE_GUARD_TIME);
      Serial1.println();
      Serial1.println();
      if (FindInResponse("\r\n<", 1000))
      {
        return true;
      }
    }
    return false;
}

boolean Reset() {
      EnterCommandMode();
      SendCommand(F("factory R"), false, "Set Factory Defaults"); // Store settings
      SendCommand(F("save"), false, "Storing in config"); // Store settings
      SendCommand(F("reboot"), false, "*READY*");
}

boolean ExitCommandMode() {
    for (int retryCount = 0; retryCount < COMMAND_MODE_ENTER_RETRY_ATTEMPTS; retryCount++) 
     {
      if (SendCommand(F("exit"), false, "EXIT")) 
      {
      return true;
      }
    }
    return false;
}

void SkipRemainderOfResponse(unsigned int timeOut) {
  unsigned long time = millis();
  while (((millis()-time)<timeOut))
  {
    if (Serial1.available())
      { 
        byte temp = Serial1.read();
        //Serial.write(temp);
        time = millis();
      }
  }
}

static char buffer[10];

char* itoa(int32_t number)
  {
   byte count = 0;
   uint32_t temp;
   if (number < 0) {temp = number*(-1); count++;} 
   while ((temp/10)!=0) 
   {
     temp = temp/10;
     count++;
   }
   int i;
   if (number < 0) {temp = number*(-1);} 
   else temp = number;
   for (i = count; i>=0; i--) 
   { 
     buffer[i] = temp%10 + '0'; 
     temp = temp/10; 
   }
   if (number < 0) {buffer[0] = '-';} 
   buffer[count + 1] = 0x00;
   return buffer;   
  }
  
boolean Bhoreal::sleep() {
      EnterCommandMode();
      SendCommand(F("sleep"));
}

boolean Ready()
{
  if(!EnterCommandMode())
    {
      Serial1.begin(115200);
      if(EnterCommandMode()) Reset();
      Serial1.begin(9600);
    }
  if (EnterCommandMode())
    {
      Serial1.println(F("join"));
      if (FindInResponse("Associated!", 8000)) 
      {
        SkipRemainderOfResponse(3000);
        ExitCommandMode();
        return(true);
      }
   } 
  else return(false);
}

boolean Bhoreal::Connect()
  {
    //if (!Ready())
    if (true)
    {
      if(EnterCommandMode())
        {    
            SendCommand(F("set wlan join 1")); // Disable AP mode
            SendCommand(F("set ip dhcp 1")); // Enable DHCP server
            SendCommand(F("set ip proto 1")); //Modo UDP
            //SendCommand(F("set ip proto 2")); //Modo TCP
  
            SendCommand(F("set ip host "), true);
            SendCommand(IP);
            SendCommand(F("set ip localport "), true);
            SendCommand(itoa(localPort));
            SendCommand(F("set ip remote "), true);
            SendCommand(itoa(outPort));
            
            SendCommand(F("set wlan auth "), true);
            SendCommand(myAuth);
            boolean mode = true;
            if ((myAuth==WEP)||(myAuth==WEP64)) mode=false;
            Serial.print(myAuth);
            SendCommand(F("set wlan ssid "), true);
            SendCommand(mySSID);
            Serial.print(F(" "));
            Serial.print(mySSID);
            if (mode) SendCommand(F("set wlan phrase "), true);  // WPA1, WPA2, OPEN
            else SendCommand(F("set wlan key "), true);
            SendCommand(myPass);
            Serial.print(F(" "));
            Serial.print(myPass);
            SendCommand(F("set wlan ext_antenna "), true);
            SendCommand(antenna);
            Serial.print(F(" "));
            Serial.println(antenna);
            SendCommand(F("save"), false, "Storing in config"); // Store settings
            SendCommand(F("reboot"), false, "*READY*");
            if (Ready()) return true;
        }
        return false;   
    }
     else return true;  
  }
  
  boolean Bhoreal::OSCSend(byte r, byte c, boolean state)
  {
      OSC_SEND[27] = r;
      OSC_SEND[31] = c;
      OSC_SEND[35] = state;
      for (int i = 0; i<MESSAGE_SIZE; i++)  Serial1.write(OSC_SEND[i]);
  }
  
///////////////////////////////////////////////////////////////
//////////////////////    Interrupccion 6    //////////////////////
///////////////////////////////////////////////////////////////  

//Esta interrupcion no esta soportada por la libreria arcore

void Bhoreal::AttachInterrupt6(int mode)
{
    EICRB = (EICRB & ~((1<<ISC60) | (1<<ISC61))) | (mode << ISC60);
    EIMSK |= (1<<INT6);
}

void Bhoreal::detachInterrupt6()
{
    EIMSK &= ~(1<<INT6);
}

void Bhoreal::sleepNow()         // here we put the arduino to sleep
{
    AttachInterrupt6(RISING);
    
    delay(100);
    
    set_sleep_mode(SLEEP_MODE_PWR_DOWN);   // sleep mode is set here
 
    sleep_enable();          // enables the sleep bit in the mcucr register
                             // so sleep is possible. just a safety pin
  
    sleep_mode();            // here the device is actually put to sleep!!
                             // THE PROGRAM CONTINUES FROM HERE AFTER WAKING UP
 
    //sleep_disable();         // first thing after waking from sleep:
                             // disable sleep...
 
}

unsigned long inttime = 0;

ISR(INT6_vect) {
 if ((millis()-inttime)>=400)
  {
    inttime = millis();
    bot_state=!bot_state;
  }
}