#include <GSM.h>

// PIN Number
#define PINNUMBER ""

// initialize the library instance
GSM gsmAccess; // include a 'true' parameter for debug enabled
GSMVoiceCall vcs;
uint8_t passPnt = 0;
uint8_t passLen = 4;
uint8_t passCode[] = {1,2,3,4};
uint8_t passBuffer[4];
char numtel[20];           // buffer for the incoming call
uint16_t codeTimeOutInMillis = 2000;
unsigned long lastCodeMillis = 0;
bool codeTimeOutStarted = 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
  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()) badPass();
        else{
          if(toneNum != -1){
            resetCodeTimeout();
            Serial.print("Tone detected:");Serial.println(toneNum);
            if(passPnt < 4){
              passBuffer[passPnt] = toneNum;
              passPnt++;
            }
            if(passPnt>= 4){
              passPnt = 0;
              if(checkPass()) {
                Serial.println("Correct pass");
                correctPass();
              }
              else {
                Serial.println("Wrong pass");
                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(){
  for(int i = 0; i < passLen; i++){
    if(passCode[i] != passBuffer[i]) return false;
  }
  return true;
}

void correctPass(){
  passPnt = 0;
  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QWDTMF=7, 0, \"G, 500, 500\"", true);
}

void badPass(){
  passPnt = 0;
  theGSM3ShieldV1ModemCore.genericCommand_rqc("AT+QWDTMF=7, 0, \"G, 300, 300, G, 300, 300\"", true);
}