#define SigPin 7
#define Node1Pin 6
#define Node2Pin 8
#define Transducer1 13
#define Transducer2 9
#define PacketPin 2
#define LEDC 3
#define LEDA 4

int beaconNumber;

void findBeaconNumber() {
  pinMode(SigPin,OUTPUT);
  pinMode(Node1Pin,INPUT);
  pinMode(Node2Pin,INPUT);
  digitalWrite(Node1Pin,HIGH); // activate pullup
  digitalWrite(Node2Pin,HIGH); // activate pullup
  digitalWrite(SigPin,LOW);

  // need to check code below
  if (digitalRead(Node1Pin)==LOW) beaconNumber=1; // must be connected to SigPin
  else if (digitalRead(Node2Pin)==LOW) beaconNumber=2; // must be connected to SigPin
  else beaconNumber=0;
}

void setup() {
  pinMode(LEDC,OUTPUT);
  pinMode(LEDA,OUTPUT);
  digitalWrite(LEDC,LOW);
  digitalWrite(LEDA,LOW);
  pinMode(Transducer1,OUTPUT);
  pinMode(Transducer2,OUTPUT);
  digitalWrite(Transducer1,LOW);
  digitalWrite(Transducer2,LOW);  
  findBeaconNumber();
  Serial.begin(2400);
}

void PulseTransducer() {  // 42Khz pulsing
    for (int i=0;i<13;i++) {
      digitalWrite(Transducer1,HIGH);
      delayMicroseconds(5);
      digitalWrite(Transducer2,HIGH);
      digitalWrite(Transducer1,LOW);
      delayMicroseconds(5);
      digitalWrite(Transducer2,LOW);
    }
}

void loop() {
  if (Serial.available()) {
    char frame=Serial.read();
//    Serial.write('|');
    while (!Serial.available());
    if (frame==0x5a) {
      while (!Serial.available()); // Wait for second byte
      Serial.write('-');
      char request=Serial.read();
      if (request==beaconNumber) {
        PulseTransducer();
        digitalWrite(LEDA,HIGH);
      } else {
        digitalWrite(LEDA,LOW);
      }
    }
  }
}