#include "ultrasonic.h"
#include "ledstrip.h"

#define MAX_DELTA 5         // maximum delta per iteration in cm
#define MIN_DISTANCE 0      // minimum distance in cm
#define MAX_DISTANCE 400    // maximum distance value in cm
#define MIN_LED 0           // minimum LED position 
#define MAX_LED 300         // maximum LED position
#define LEDS_PER_CM 0.75    // LEDs per strip cm (e.g, 300 LEDs / 400cm)

enum direction {
  NONE = 0,
  LEFT = -1,
  RIGHT = 1
};

enum direction travelDirection = NONE;  // direction measured object travels
int measuredDistance = 0;   // normalized measured distance in cm
int prevDistance = 0;       // previous distance in cm for LED
int curDistance = 0;        // current distance in cm for LED
int delta = 0;              // difference between previous and current distance
int activeLED = 0;          // index of active led

enum direction getTravelDirection(int distance) {
    bool isOutOfBounds = distance < MIN_DISTANCE || distance > MAX_DISTANCE;

    if (isOutOfBounds) {
        return travelDirection;
    }

    if (distance < prevDistance) {
        return LEFT;
    }
    
    if (distance > prevDistance) {
        return RIGHT;
    }

    return NONE;
}

// returns a valid X when the sensor value is out of range
int normalizeDistance(int measuredDistance) {
    if (measuredDistance >= MIN_DISTANCE && measuredDistance <= MAX_DISTANCE) {
        return measuredDistance;
    }
    
    if (travelDirection > 0) {
        return MAX_DISTANCE;
    }

    return MIN_DISTANCE;
}

int getCurrentDistance(int distance) {
    delta = abs(prevDistance - distance);
    
    if (delta > MAX_DELTA) {
        delta = MAX_DELTA;
    }

    return prevDistance + (delta * travelDirection);
}

void setActiveLED(int distance) {
   int activeLED = round(LEDS_PER_CM * curDistance);

    // set leds if it's in range
    if (activeLED >= MIN_LED && activeLED < MAX_LED) {
        prevDistance = curDistance;
        setLEDs(activeLED);
    }
}

void logValues() {
    Serial.print("Meassured Distance: ");
    Serial.print(measuredDistance);

    Serial.print(" prevDistance: ");
    Serial.print(prevDistance);

    Serial.print(" curDistance: ");
    Serial.print(curDistance);

    Serial.print(" Direction:");
    if (travelDirection == LEFT) {
        Serial.println(" LEFT ");
    } else if (travelDirection == RIGHT) {
        Serial.println(" RIGHT ");
    } else {
        Serial.println(" NONE ");
    }
}

void setup() {
    Serial.begin(9600);
    setupUltraSonic();
    setupLEDStrip(); 
    delay(500);
}

void loop() {
    // get sensor value
    measuredDistance = getDistance();
    travelDirection = getTravelDirection(measuredDistance);
    curDistance = getCurrentDistance(normalizeDistance(measuredDistance));
    setActiveLED(curDistance);
  
    // logValues();
}