#include <MySignals.h>
#include <Wire.h>
#include "SPI.h"



void setup()
{
  //Write here you correct baud rate
  Serial.begin(9600);

  MySignals.begin();

  /*
    expanderState is initialized with B10100001

    Expansor pin library names:

    EXP_3G_POWER
    EXP_ESP8266_POWER
    EXP_BT_POWER
    EXP_BT_KEY
    EXP_ADC_CS
    EXP_BLE_FLOW_CONTROL
    EXP_BLE_POWER
    EXP_ROVING_POWER
  */

  //Enable expansion board power -> bit7:0
  bitClear(MySignals.expanderState, EXP_ROVING_POWER);
  MySignals.expanderWrite(MySignals.expanderState);

  MySignals.initSensorUART();
  MySignals.enableSensorUART(EXPANSION);


  delay(2000);

  if (enterConfig(2000))
  {
    MySignals.println("XBee OK");
    delay(1000);

    int8_t answer = sendCommand("AT", "OK", 2000);
    if (answer == 1)
    {
      MySignals.println("AT OK");
    }
    else
    {
      MySignals.println("AT no OK");
    }


  }
  else
  {
    MySignals.println("XBee no OK");
  }
}

void loop()
{
  delay(5000);
}




int8_t sendCommand(const char* Command, const char* expected_answer, unsigned int timeout)
{
  char response[300];
  uint8_t x = 0,  answer = 0;
  unsigned long previous;

  memset(response, 0, sizeof(response));    // Initialize the string

  delay(100);

  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer

  Serial.println(Command);    // Send Command

  x = 0;
  previous = millis();

  // this loop waits for the answer
  do
  {
    if (Serial.available() != 0)
    {
      // if there are data in the UART input buffer, reads it and checks for the asnwer
      response[x] = Serial.read();
      x++;
      // check if the desired answer  is in the response of the module
      if (strstr(response, expected_answer) != NULL)
      {
        answer = 1;
      }
    }
  }
  // Waits for the asnwer with time out
  while ((answer == 0) && ((millis() - previous) < timeout));

  return answer;
}


int8_t enterConfig(unsigned int timeout)
{
  char response[100];
  uint8_t x = 0,  answer = 0;
  unsigned long previous;

  memset(response, 0, 100);    // Initialize the string

  delay(100);

  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer

  Serial.print("+++");    // Send Command


  x = 0;
  previous = millis();

  // this loop waits for the answer
  do
  {
    if (Serial.available() != 0)
    {
      // if there are data in the UART input buffer, reads it and checks for the asnwer
      response[x] = Serial.read();
      x++;
      // check if the desired answer  is in the response of the module
      if (strstr(response, "OK") != NULL)
      {
        answer = 1;
      }
    }
  }
  // Waits for the asnwer with time out
  while ((answer == 0) && ((millis() - previous) < timeout));

  return answer;
}