from OpenGL.images import returnFormat
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
import statistics as st

# 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
DEBUG = False

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

# PID values
P_value = 2
I_value = 0
D_value = 0

sample_array = []

def initial():
    ...


def read_distance_sensor(fixed_file_stamp):

    with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
        samples = 0
        max_samples = 10
        timestamp_last = 0.0
        timestamp_first = 0.0
        while samples != max_samples:
            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: ", distance) if SCREEN else None

                    sample_array.append(distance)
                    if samples == 0: timestamp_first, _ = dt.timestamper()
                    if samples == max_samples - 1: timestamp_last, _ = dt.timestamper()

                    timestamp_first_float = float(timestamp_first)
                    timestamp_last_float = float(timestamp_last)
                    samples = samples + 1
                    median_distance = st.median(sample_array)
                    mean_timestamp  = st.mean([timestamp_first_float, timestamp_last_float])
                    print(median_distance) if LOG else None
                    print(mean_timestamp)

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

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

    return median_distance, mean_timestamp

def read_setpoint():
    ...


def calculate_velocity():
    ...

def PID_calculations(setpoint):

    global I_result, previous_time, previous_error
    offset_value = 320
    measurement, current_time = read_distance_sensor
    error = setpoint - measurement

    if previous_time is None:
        previous_error: float = 0.0
        previous_time: float = current_time
        I_result: float = 0.0
        error_sum: float = error * 0.008 # sensor sampling number approximation.

    error_sum: float = error_sum +  (error * (current_time - previous_time))
    P_result: float = P_value * error
    I_result: float =  I_value * error_sum
    D_result: float = D_value * ((error - previous_error) / (current_time - previous_time))
    PID_result: float = offset_value + P_result + I_result + D_result
    previous_error: float = error
    previous_time: float = current_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