#include "Arduino.h"
#include "McKenzie.h"
#include "TargetDetection.h"

TargetDetection::TargetDetection (int inPin, int hitLevel, int hitInterval, int hitCycles, int hitResetTime){ 
  this->inPin = inPin;
  this->hitLevel = hitLevel;
  this->hitInterval = hitInterval;
  this->hitCycles = hitCycles;
  this->hitResetTime = hitResetTime;
  oneShot1 = false;
  oneShot2 = false;
  pulseDetect = false; 
  detectTime.preset = hitResetTime;

}

bool TargetDetection::check() {
  //Read infrared photo resistor levels (0-1023) 
  //0 is no detection
  targetLevel = analogRead(inPin);
  int bandUpper = hitInterval + intervalBand;
  int bandLower = hitInterval - intervalBand;
  
  /*
  Serial.print ("Lower:");
  Serial.print (bandLower);
  Serial.print ("  Upper:");
  Serial.println (bandUpper);
  */

  if ((targetLevel > (lastTargetLevel + hitLevel)) && !oneShot1 ) {
    oneShot1 = true;
    onTimes[detectIndex] = micros();
    onLevel = targetLevel;
    detectReset.reset();
    detectReset.start();
  } 
  
  if ((targetLevel < onLevel) && oneShot1){
    offTimes[detectIndex] = micros();
    if (detectIndex > 2) {
      onInterval = (onTimes[detectIndex-1] - onTimes[detectIndex-2])/1000;
      offInterval = (offTimes[detectIndex-1] - offTimes[detectIndex-2])/1000; 
      if ((onInterval > bandLower) && (onInterval < bandUpper) || (offInterval > bandLower) && (offInterval < bandUpper)) {
        detectCount++;
      }
    }
    detectIndex++;
    if (detectIndex > 790){detectIndex = 0;}
    oneShot1 = false;
  }
  
  //Reset the index after 2 seconds after first hit
  detectReset.check();
  if (detectReset.dn) {
    oneShot1 = false;
    detectIndex = 0;
  }

  //Reset the good pulse counter 
  detectTime.check();
  if ((detectCount>0)&&!oneShot2){
    detectTime.reset();
    detectTime.start();
    oneShot2 = true;
  }
  
  if (detectTime.dn){
    detectCount = 0;
    detectTime.reset();
    oneShot2=false;
  }
  lastTargetLevel = targetLevel;
  if (detectCount > hitCycles) {
    detectCount=0;
    oneShot2 = false;
    pulseDetect = true;
  }
  else {pulseDetect = false;}  
  return pulseDetect;
}