#include <SPI.h>
#include <Ethernet.h>
#include <EthernetUdp.h>

#define serialSpeed 9600
#define ledPin A3
#define buzzerPin A1
#define wiegandLowPin 4
#define wiegandHighPin 5
#define ledHighThreshold 1000
#define ledLowThreshold 500
#define ledHeartbeatPulseWidth 100
#define ledPulseWidth 108
#define buzzerPulseWidth 68

struct PacketHeader
{
  unsigned char Number[2];
  unsigned char CardNumberBits;
  unsigned char Body[];
};

/* NETWORKING SETTINGS */
byte ip[] = {192, 168, 40, 100};
byte mac[] = {0xDC, 0xAA, 0xBE, 0xCD, 0xFA, 0xFC};
byte subnet[] = {255, 255, 0, 0};
byte ns[] = {192, 168, 0, 1};
byte gateway[] = {192, 168, 0, 1};

unsigned int localPort = 8888;
char packetBuffer[UDP_TX_PACKET_MAX_SIZE];

EthernetUDP Udp;

int ledAnalog = 0;
unsigned long ledHigh = 0;
unsigned long ledLow = 0;
unsigned short ledCount = 0;
unsigned short heartbeatCount = 0;
unsigned long heartbeatHigh = 0;
int buzzerAnalog = 0;
unsigned long buzzerLow = 0;
unsigned short buzzerCount = 0;
unsigned int counter = 0;
unsigned long timestamp = 0;
unsigned long ledHighReset = 0;
unsigned long ledLowReset = 0;

void setup() {
  Ethernet.begin(mac, ip, ns, gateway, subnet);
  Udp.begin(localPort);
  Serial.begin(serialSpeed);
  pinMode(ledPin, INPUT);
  pinMode(buzzerPin, INPUT);
  pinMode(wiegandLowPin, OUTPUT);
  pinMode(wiegandHighPin, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(ledPin, HIGH);
  digitalWrite(buzzerPin, HIGH);
  digitalWrite(wiegandLowPin, HIGH);
  digitalWrite(wiegandHighPin, HIGH);
}

inline int GetBit(unsigned char buffer[], int bitNum)
{
  return ( buffer[bitNum / 8] & ( 1 << bitNum % 8 ) ) != 0 ? 1 : 0;
}

void ProcessPacket(struct PacketHeader& ph)
{
    for ( int i = ph.CardNumberBits-1; i >= 0; i-- )
    {
      int bit = GetBit(ph.Body, i);
      digitalWrite(bit == 1 ? wiegandHighPin : wiegandLowPin, LOW);
      delayMicroseconds(100);     // Weigand TPW
      digitalWrite(bit == 1 ? wiegandHighPin : wiegandLowPin, HIGH);
      delayMicroseconds(1000);    // Weigand TPI
    }  
}

void loop() 
{
  MonitorUDP();
  timestamp = millis();
  MonitorBuzzer();
  MonitorLED();
  counter++;
//  if (counter % 10000 == 0)
//  {
//    Serial.print("ANALOG VALUES (B/L):");
//    Serial.print(buzzerAnalog);
//    Serial.print("/");
//    Serial.println(ledAnalog);
//  }
}

void MonitorUDP()
{
  int packetSize = Udp.parsePacket();

  if ( packetSize )
  {
    Serial.print("Received packet of size ");
    Serial.println(packetSize);
    
    // read the packet into packetBufffer
    Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
    
    Serial.println("Processing packet...");
    
    PacketHeader* ph = (PacketHeader*)&packetBuffer;
     
    Serial.print("Bits: "); Serial.println(ph->CardNumberBits);
    for ( int i = ph->CardNumberBits-1; i >= 0; i-- )
     Serial.print(GetBit(ph->Body, i));
    Serial.println();

//    buzzerCount = 0;
//    ledCount = 0;
//    buzzerLow = 0;
//    ledLow = 0;
//    ledHigh = 0;
    
   ProcessPacket(*ph);
   
   // send a reply, to the IP address and port that sent us the packet we received
//   Serial.print("Responding with packet number...");
//   Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());     
//   Udp.write(packetBuffer, sizeof(PacketHeader::Number));
//   Udp.endPacket();
  }
}

void MonitorBuzzer()
{
  buzzerAnalog = analogRead(buzzerPin);

  if (buzzerAnalog <= ledLowThreshold) // LO
  {
    if (buzzerLow == 0)
      buzzerLow = timestamp;
  }
  else // Baseline
  {
    if (buzzerLow > 0)
    {
      if (timestamp - buzzerLow >= buzzerPulseWidth)
      {
//        Serial.print("BEEP:");
        buzzerCount++;
//        Serial.print(buzzerCount);
//        Serial.print(":");
//        Serial.println(timestamp - buzzerLow);           
      }
      
      buzzerLow = 0;
    }
  }
}

void MonitorLED()
{
  ledAnalog = analogRead(ledPin);

  if (ledAnalog >= ledHighThreshold) // HI
  {
    if (ledHigh == 0)
    {
      digitalWrite(LED_BUILTIN, HIGH);
      ledHigh = timestamp;      
    }
  }
  else if (ledAnalog <= ledLowThreshold) // LOW
  {
    if (ledLow == 0)
      ledLow = timestamp;
  }
  else // Baseline
  {
    if (ledHigh > 0)
    {
      if (timestamp - ledHigh >= ledHeartbeatPulseWidth)
      {
        if (timestamp - ledHigh >= ledPulseWidth)
        {  
//          Serial.print("LEDH:");
          ledCount++;
//          Serial.print(ledCount);
//          Serial.print(":");
          ledHighReset = timestamp;

          if (buzzerCount > 1) // Access Denied
          {
            Serial.print("ACCESS DENIED:");
            Serial.print(buzzerCount);
            Serial.print(":");
            Serial.println(timestamp - buzzerLow);            
      
            Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());     
      
            char response[] = "DENIED";
            
            Udp.write(response, 6);
            Udp.endPacket();

            Serial.print("SENT MESSAGE:");
            Serial.print(Udp.remoteIP());
            Serial.print(":");
            Serial.println(Udp.remotePort());
            
            ledCount = 0;
            buzzerCount = 0;
          }
        }
        else  
        {
//          Serial.print("HBH:");
          heartbeatCount++;       
//          Serial.print(heartbeatCount);
//          Serial.print(":");
          digitalWrite(LED_BUILTIN, LOW);
        } 
        
//        Serial.println(timestamp - ledHigh);           
      }

      ledHigh = 0;
    }

    if (ledLow > 0)
    {
      if (timestamp - ledLow >= ledPulseWidth)
      {
//        Serial.print("LEDL:");

        ledCount++;
//        Serial.print(ledCount);
//        Serial.print(":");

//        Serial.println(timestamp - ledLow);     
        if (buzzerCount >= 1) // Access Granted
        {
          Serial.print("ACCESS GRANTED:");
          Serial.print(buzzerCount);
          Serial.print(":");
          Serial.println(timestamp - ledLow);

          Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());     

          char response[] = "GRANTED";
          
          Udp.write(response, 7);
          Udp.endPacket();

          Serial.print("SENT MESSAGE:");
          Serial.print(Udp.remoteIP());
          Serial.print(":");
          Serial.println(Udp.remotePort());
          
          ledCount = 0;
          buzzerCount = 0;
        }
      }

      ledLow = 0;
    }
  }
}