# =============================================================================
# FeelBeam: Haptic Distance Navigator for Vision Loss Adaptation
# Project: https://hackaday.io/project/204343-feelbeam-haptic-distance-navigator
# Website: https://feelbeam.app
# Author: Vasilii Belykh (Berlin, Germany)
# Contact: info@feelbeam.app
# License: CC-BY-SA 4.0
# =============================================================================
#
# This code is part of the FeelBeam open-source assistive technology project.
# It runs on Raspberry Pi 5 and controls a TF-Luna LiDAR sensor and servo motor
# to provide proportional mechanical haptic feedback via a sliding lever.
#
# Target users: Individuals adapting to vision loss (especially newly blind).
# Current status: Wired prototype. Final design: battery-powered, ESP32-S3 based.
#
# =============================================================================
# CC-BY-SA 4.0 License Summary:
# - You may use, modify, and distribute this code
# - You must give credit to the original author (Vasilii / FeelBeam)
# - You must share modifications under the same license
# Full license: https://creativecommons.org/licenses/by-sa/4.0/
# =============================================================================

import smbus
import time
import RPi.GPIO as GPIO
import statistics

SERVO_MIN_PULSE = 500
SERVO_MAX_PULSE = 2500

SERVO_START_ANGLE = 45
SERVO_WIDE_ANGLE = 90
SERVO_MAX_DISTANCE = 400

ServoPin = 18
STRENGTH_THRESHOLD = 100  # Threshold for reliable signal strength
STABILITY_THRESHOLD = 20  # Stability threshold in cm for servo update

def getLidarData(addr, cmd, previous_distance):
    # Send command to get data
    bus.write_i2c_block_data(addr, 0x00, cmd)
    time.sleep(0.01)
    # Read 9 bytes of data
    data = bus.read_i2c_block_data(addr, 0x00, 9)
    distance = data[0] | (data[1] << 8)
    strength = data[2] | (data[3] << 8)
    temperature = (data[4] | (data[5] << 8)) / 100 
    print('distance = %5d cm, strength = %5d, temperature = %5.2f °C' % (distance, strength, temperature))
    # If signal strength is weak, return previous distance
    if strength < STRENGTH_THRESHOLD:
        return previous_distance
    return distance

def map_value(value, inMin, inMax, outMin, outMax):
    # Map value from one range to another
    return (outMax - outMin) * (value - inMin) / (inMax - inMin) + outMin

def setup():
    global p, bus
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(ServoPin, GPIO.OUT)
    GPIO.output(ServoPin, GPIO.LOW)
    p = GPIO.PWM(ServoPin, 50)
    p.start(0)
    bus = smbus.SMBus(1)  # I2C bus

def setAngle(angle):
    # Set servo to specific angle (0-180 degrees)
    angle = max(0, min(180, angle))
    pulse_width = map_value(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE)
    pwm = map_value(pulse_width, 0, 20000, 0, 100)
    p.ChangeDutyCycle(pwm)

def destroy():
    # Cleanup GPIO and stop PWM
    p.stop()
    GPIO.cleanup()

if __name__ == '__main__':
    setup()
    address = 0x10  # Default TF-Luna address
    getLidarDataCmd = [0x5A, 0x05, 0x00, 0x01, 0x60]  # Command to get distance
    distance1 = distance2 = distance3 = 0
    previous_distance = 0  # Initial previous distance
    try:
        while True:
            distance3 = distance2
            distance2 = distance1
            # Get new distance, handling weak signal
            distance1 = getLidarData(address, getLidarDataCmd, previous_distance)
            if distance1 > SERVO_MAX_DISTANCE:
                distance1 = SERVO_MAX_DISTANCE
            distance = (distance1 + distance2 + distance3) / 3
            previous_distance = distance  # Update previous for next iteration
            # Check stability: update servo only if std dev < threshold
            distances = [distance1, distance2, distance3]
            if statistics.stdev(distances) < STABILITY_THRESHOLD:
                angle = SERVO_START_ANGLE + (SERVO_WIDE_ANGLE * distance / SERVO_MAX_DISTANCE)
                setAngle(angle)
            time.sleep(0.3)  # Increased delay for smoother operation
    except KeyboardInterrupt:
        destroy()