#include <GSM.h>

// PIN Number
#define PINNUMBER ""
#define ARM_COMMAND 1
#define DISARM_COMMAND 2
#define STATUS_COMMAND 3

// initialize the library instance
GSM gsmAccess; // include a 'true' parameter for debug enabled
GSMVoiceCall vcs;
uint8_t keyBufferPnt = 0;
const uint8_t passLen = 4;
const uint8_t keyBufferLen = 5;
const uint8_t passCode[] = {1,2,3,4};
uint8_t keyBuffer[5];
char numtel[20];           // buffer for the incoming call
uint16_t codeTimeOutInMillis = 2000;
unsigned long lastCodeMillis = 0;
bool codeTimeOutStarted = false;

bool isArmed = false;


void setup()
{
  // initialize serial communications
  Serial.begin(9600);
  Serial.println("Receive Voice Call");

  // connection state
  boolean notConnected = true;

  // Start GSM shield
  // If your SIM has PIN, pass it as a parameter of begin() in quotes
  while(notConnected)
  {
    if(gsmAccess.begin(PINNUMBER)==GSM_READY)
      notConnected = false;
    else
    {
      Serial.println("Not connected");
      delay(1000);
    }
  }

  //Enable dtmf
  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QTONEDET=1", true); // Send command
  bool resp;
  delay(1000); // Wait for response
  theGSM3ShieldV1ModemCore.genericParse_rsp(resp);
  if(resp)Serial.println("DTMF Enabled");else Serial.println("DTMF NOT Enabled"); //Enable tone only

  //Select MIC1 channel to send DTMF to remote peer.
  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QAUDCH=0", true); // Send command
  delay(1000); // Wait for response
  theGSM3ShieldV1ModemCore.genericParse_rsp(resp);
  if(resp)Serial.println("QAUDCH Enabled");
  else Serial.println("QAUDCH NOT Enabled");

  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QSFR=7", true); // Send command
  delay(1000); // Wait for response
  theGSM3ShieldV1ModemCore.genericParse_rsp(resp);
  if(resp)Serial.println("QSFR Enabled");
  else Serial.println("QSFR NOT Enabled");

  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QULDLSPH=0,0", true); // Send command
  delay(1000); // Wait for response
  theGSM3ShieldV1ModemCore.genericParse_rsp(resp);
  if(resp)Serial.println("ToneOnly Enabled");
  else Serial.println("ToneOnly NOT Enabled");

  // This makes sure the modem notifies correctly incoming events
  vcs.hangCall();

  Serial.println("Waiting Call");
}

void loop()
{
  // Check the status of the voice call
  switch (vcs.getvoiceCallStatus())
  {
    case IDLE_CALL: // Nothing is happening

      break;

    case CALLING: // This should never happen, as we are not placing a call

      Serial.println("CALLING");
      break;

    case RECEIVINGCALL: // Yes! Someone is calling us

      Serial.println("RECEIVING CALL");

      // Retrieve the calling number
      vcs.retrieveCallingNumber(numtel, 20);

      // Print the calling number
      Serial.print("Number:");
      Serial.println(numtel);

      // Answer the call, establish the call
      vcs.answerCall();
      break;

    case TALKING:  // In this case the call would be established

      Serial.println("TALKING. Enter line to interrupt.");
      uint8_t charToneSum = 0;
      const char* strToneSearch = "QTONEDET: ";
      uint8_t len = strlen(strToneSearch);

      while(Serial.read()!='\n'){
        int toneNum = detectTone(&charToneSum , len, strToneSearch);
        if (isCodeTimedout()) keyTimeout();
        else{
          if(toneNum != -1){
            resetCodeTimeout();
            Serial.print("Tone detected:");Serial.println(toneNum);
            if(keyBufferPnt < keyBufferLen){
              keyBuffer[keyBufferPnt] = toneNum;
              keyBufferPnt++;
            }
            if(keyBufferPnt>= keyBufferLen){
              if(checkPass())correctPass();
              else badPass();
            }
          }
        }
      }

      vcs.hangCall();
      Serial.println("HANG. Waiting Call.");
      break;
  }
  delay(1000);
}

int detectTone(uint8_t* sum , uint8_t len, const char* strSearch){
  char c = theGSM3ShieldV1ModemCore.theBuffer().read();
  *sum = (c==strSearch[*sum]) ? *sum+1 : 0;
  if(*sum == len){
    char cc[2];
    cc[0] = (char)theGSM3ShieldV1ModemCore.theBuffer().read();
    cc[1] = (char)theGSM3ShieldV1ModemCore.theBuffer().read();
    int n;
    n = atoi(cc);
    *sum = 0;
    return n - 48;
  }
  return -1;
}

bool isCodeTimedout(){
  if(codeTimeOutStarted){
    if( (unsigned long) millis() - lastCodeMillis > codeTimeOutInMillis){
      codeTimeOutStarted = false;
      return true;
    }
  }
  return false;
}

void resetCodeTimeout(){
  lastCodeMillis = millis();
  codeTimeOutStarted = true;
}

bool checkPass(){
  codeTimeOutStarted = false;
  for(int i = 0; i < passLen; i++){
    if(passCode[i] != keyBuffer[i]) return false;
  }
  return true;
}

void keyTimeout(){
  Serial.println("Key timeout");
  keyBufferPnt = 0;
  const char* str = "AT+QWDTMF=7, 0, \"G, 300, 300, G, 300, 300\"";
  Serial.print("Sending|:");Serial.println(str);
  theGSM3ShieldV1ModemCore.genericCommand_rqc(str, true);
  theGSM3ShieldV1ModemCore.theBuffer().debugBuffer();
}

void correctPass(){
  Serial.println("Correct pass");
  keyBufferPnt = 0;
  const char* str = "AT+QWDTMF=7, 0, \"G, 500, 500\"";
  Serial.print("Sending|:");Serial.println(str);
  theGSM3ShieldV1ModemCore.genericCommand_rqc(str, true);
  theGSM3ShieldV1ModemCore.theBuffer().debugBuffer();
  runCommand(keyBuffer[4]);
}

void badPass(){
  Serial.println("Wrong pass");
  keyBufferPnt = 0;
  const char* str = "AT+QWDTMF=7, 0, \"G, 300, 300, G, 300, 300\"";
  Serial.print("Sending|:");Serial.println(str);
  theGSM3ShieldV1ModemCore.genericCommand_rqc(str, true);
  theGSM3ShieldV1ModemCore.theBuffer().debugBuffer();
}

void runCommand(int commandCode){
  switch (commandCode) {
    case ARM_COMMAND:
      armCom();
      break;
    case DISARM_COMMAND:
      disarmCom();
      break;
    case STATUS_COMMAND:
      statusCom();
      break;
    default:
      Serial.println("Uknown command");
      break;
  }
}

void armCom(){
  isArmed = true;
  Serial.println("Armed");
}

void disarmCom(){
  isArmed = false;
  Serial.println("Disarmed");
}

void statusCom(){
  if(isArmed) Serial.println("Is armed");
  else Serial.println("Is disarmed");
}