from adafruit_hcsr04 import HCSR04 as hcsr04
import board
import data_transfer_functions as dt
from adafruit_servokit import ServoKit
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
import numpy as np
import matplotlib.pyplot as plt

# Variables to control sensor
TRIGGER_PIN = board.D4
ECHO_PIN = board.D17
TIMEOUT = 0.1
MIN_DISTANCE = 4
MAX_DISTANCE = 40

# Variables to control logging.
LOG = True
SCREEN = True

# PID measurements
time = 0
integral = 0
time_prev = -1e-6
e_prev = 0


def initial():
    ...


def read_distance_sensor(fixed_file_stamp):

    with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):

        while True:
            try:
                distance = sonar.distance
                if MIN_DISTANCE < distance < MAX_DISTANCE:

                    dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
                    print("Distance: ", sonar.distance) if SCREEN else None
                else:
                    dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
                    print("Distance: ", distance) if SCREEN else None

            except RuntimeError:
                dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
                print("Timeout") if SCREEN else None

def read_setpoint():
    ...


def calculate_velocity():



def PID_calculations(Kp, Ki, Kd, setpoint, measurement):

    global cur_time, integral, prev_time, prev_error
    offset_value = 320
    # PID calculations
    error = setpoint - measurement
    P = Kp * error
    integral = integral + Ki * error * (cur_time - prev_time)
    D = Kd * (error - prev_error) / (cur_time - prev_time)
    PID_result = offset_value + P + integral + D
    prev_error = error
    prev_time = cur_time
    return PID_result


def calculate_new_servo_pos():
    ...


def send_data_to_servo():

    kit = ServoKit(channels=16)
    kit.servo[0].set_pulse_width_range(500, 2500)

    kit.servo[0].angle = 180