//arduino8
#include "HUSKYLENS.h"
#include "SoftwareSerial.h"
#include <Wire.h>



HUSKYLENS huskylens;

// Define the pins for the outputs
const int PIN_RIGHT = 6;
const int PIN_CENTER = 7;
const int PIN_LEFT = 8;

// Define center threshold (e.g., +/- 30 pixels from center x=160)
const int CENTER_THRESHOLD = 30; 
const int FRAME_CENTER_X = 160; // HuskyLens X resolution is 320

void setup() {
  Serial.begin(115200);
  Wire.begin();
  if (!huskylens.begin(Wire)) {
    Serial.println(F("Can't connect to HuskyLens!"));
    while (true); // Halt if connection fails
  }
  
  // Set algorithm to Face Recognition (or ALGORITHM_OBJECT_RECOGNITION if you used object learning)
  huskylens.writeAlgorithm(ALGORITHM_OBJECT_RECOGNITION); 

  // Initialize pins as outputs and ensure they are LOW initially
  pinMode(PIN_RIGHT, OUTPUT);
  pinMode(PIN_CENTER, OUTPUT);
  pinMode(PIN_LEFT, OUTPUT);
  digitalWrite(PIN_RIGHT, LOW);
  digitalWrite(PIN_CENTER, LOW);
  digitalWrite(PIN_LEFT, LOW);
  delay (5000);
}

void loop() {      // Turn off all pins initially
        digitalWrite(PIN_RIGHT, LOW);
        digitalWrite(PIN_CENTER, LOW);
        digitalWrite(PIN_LEFT, LOW);



  // Request data from HuskyLens
  if (huskylens.requestLearned()) {

    if (huskylens.available() >0) {
    // Iterate through all detected items (blocks/arrows)
    for (int i = 0; i < huskylens.countBlocks(); i++) {
      HUSKYLENSResult result = huskylens.getBlock(i);

      // Check if the detected item is ID1
      if (result.ID == 1) {
        

        // Determine position based on the center X coordinate (result.x)
        if (result.xCenter > FRAME_CENTER_X + CENTER_THRESHOLD) {
          // Person is right of center (further right means larger X value in HuskyLens coords)
          digitalWrite(PIN_RIGHT, HIGH);
          Serial.println("ID1 Right");
        } else if (result.xCenter < FRAME_CENTER_X - CENTER_THRESHOLD) {
          // Person is left of center
          digitalWrite(PIN_LEFT, HIGH);
          Serial.println("ID1 Left");
        } else {
          // Person is centered within the threshold
          digitalWrite(PIN_CENTER, HIGH);
          Serial.println("ID1 Centered");
        }
        
        // Since we found ID1, we can stop checking other results this frame
       break; 
      }
    }Serial.println("I'm just before else");
  

  } else {
    // If no data is available or no object is detected, ensure all pins are LOW
    digitalWrite(PIN_RIGHT, LOW);
    digitalWrite(PIN_CENTER, LOW);
    digitalWrite(PIN_LEFT, LOW);
    
  }
  }

  // Small delay to prevent overwhelming the Arduino with requests
  delay(100); 
}