from adafruit_hcsr04 import HCSR04 as hcsr04        # Ultrasound sensor
import board                                        # General board pin mapper
from adafruit_servokit import ServoKit              # Servo libraries for PWM driver board
import adafruit_pcf8591.pcf8591 as PCF              # AD/DA converter board for potentiometer
from adafruit_pcf8591.analog_in import AnalogIn     # Analogue in pin library
from adafruit_pcf8591.analog_out import AnalogOut   # Analogue out pin library
import statistics as st                             # Mean and median calculations
import csv                                          # CSV handling
from datetime import datetime                       # Date and time formatting
from time import sleep                              # Sleep/pause
import pandas as pd

# Variables to control sensor
TRIGGER_PIN = board.D4      # GPIO pin xx
ECHO_PIN = board.D17        # GPIO pin xx
PIN_TIMEOUT: float = 0.1    # Timeout for echo wait -- don't change
RUN_TIMEOUT: float = 0.0    # Sleep time in function
MIN_DISTANCE: int = 4       # Minimum sensor distance to be considered valid (1 on bar)
MAX_DISTANCE: int = 39      # Maximum sensor distance to be considered valid (35 on bar)

# Variables to control servo
KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE: int = 400        # Defines angle 80, for current PID setup
MAX_PULSE: int = 2500       # Defines angle 100, for current PID setup
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)

# Variables to control logging.
LOG: bool   = True         # Log data to files
SCREEN: bool = True         # Log data to screen
DEBUG: bool = True         # More data to display
TWIN_MODE: bool = False

# Control the number of samples for single distance measurement (average from burst)
MAX_SAMPLES: int = 10

# Control the potentiometer
# Description:
# POT_MIN = min_scaled: 0.012890821698329136 (0.01V)
# POT_MAX = max_scaled: 3.28715953307393000 (3.29V)
# POT_RNG = range_scaled: 3.274268711375600864 (3.28V) -> POT_MAX - POT_MIN
# POT_ARM = usable_arm_range: 35cm
# POT_PCM = 35 / 3.274268711375600864 = 10.689409784359341315326937965383 -> POT_ARM / POT_RNG
PCF_VAL: int = 65535
POT_MIN: float = 0.012890821698329136
POT_MAX: float = 3.287159533073930000
POT_RNG: float = 3.274268711375600864
POT_ARM: int = 35
POT_PCM: float = 10.689409784359341315326937965383
POT_INT: float = 0.1

# Pin control potentiometer board
i2c = board.I2C()
pcf = PCF.PCF8591(i2c)
pcf_in_0 = AnalogIn(pcf, PCF.A0)
pcf_out = AnalogOut(pcf, PCF.OUT)
pcf_out.value = PCF_VAL

# Variables to control PID values (PID formula tweaks)
p_value: float = 2.0
i_value: float = 0.0
d_value: float = 0.0

# Initial variables, used in pid_calculations()
i_result: float = 0.0
previous_time: float = 0.0
previous_error: float = 0.0

# Variables to assist pid_calculations()
current_time: float = 0
integral: float = 0

# Init array, used in read_distance_sensor()
sample_array: list = []

# Error sum array
error_sum_array: list = []
error_sum_counter: int = 0
error_sum_max: int = 100

# Digital twin
previous_speed:float = 0.0
start_loop = True
previous_measurement: float = 0.0

# Write data to any of the logfiles
def log_data(data_file: str, data_line: str, remark: str|None):
    log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]

    with open("pid-balancer_" + "time_file.txt", "r") as time_file:
        file_stamp: str = time_file.readline()

    with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file:
        data_writer  = csv.writer(data_file)
        data_writer.writerow([log_stamp,data_line, remark])

def read_distance_sensor():

    # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long a distance)
    # Return the mean timestamp and median distance.
    with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=PIN_TIMEOUT) as sonar:
        samples: int = 0
        max_samples: int = MAX_SAMPLES
        timestamp_last: float   = 0.0
        timestamp_first: float = 0.0

        while samples != max_samples:
            sleep(RUN_TIMEOUT)
            try:
                distance: float = sonar.distance
                if MIN_DISTANCE < distance < MAX_DISTANCE:

                    log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
                    print("Distance_in_range: ", distance) if SCREEN else None

                    sample_array.append(distance)
                    if samples == 0: timestamp_first  = float(datetime.strftime(datetime.now(),
                                                                                '%Y%m%d%H%M%S.%f')[:-3])
                    if samples == max_samples - 1: timestamp_last = float(datetime.strftime(datetime.now(),
                                                                                '%Y%m%d%H%M%S.%f')[:-3])

                    timestamp_first_float: float = float(timestamp_first)
                    timestamp_last_float: float = float(timestamp_last)
                    samples: int = samples + 1
                    median_distance: float = st.median(sample_array)
                    mean_timestamp: float  = st.mean([timestamp_first_float, timestamp_last_float])

                    print("Distance_median: ", median_distance) if SCREEN else None
                    print("Timestamp_mean: ", mean_timestamp) if SCREEN else None

                else:
                    log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
                    print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None

            except RuntimeError:
                log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None
                print("Distance_timed_out") if SCREEN else None

    return median_distance, mean_timestamp

def read_setpoint():

    while True:
        raw_value: int = pcf_in_0.value
        scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage

        log_line = str(scaled_value) + "," + str(raw_value) + "," + str("angle")
        log_data(data_file="potmeter", data_line=log_line, remark="") if LOG else None

        cm_rounded: int = int(round(scaled_value * POT_PCM, 0))

        if SCREEN:
            print('Scaled_rounded = ' , round(scaled_value, 4), ' CM_rounded= ', cm_rounded)
            print('Scaled_raw= ' , scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM))

        sleep(POT_INT)
        return cm_rounded

def calculate_acceleration():

    position_1, timestamp_1 = read_distance_sensor()
    position_2, timestamp_2 = read_distance_sensor()
    position_3, timestamp_3 = read_distance_sensor()

    initial_velocity: float = (position_2 - position_1) / (timestamp_2 - timestamp_1)
    final_velocity: float  = ((position_3 - position_2) / (timestamp_3 - timestamp_2))
    acceleration: float  = (final_velocity - initial_velocity) / (timestamp_3 - timestamp_1)

    print(initial_velocity, " ", final_velocity, " ", acceleration)  if SCREEN  else None

    data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
    log_data(data_file="acceleration", data_line=data_line, remark="") if LOG else None

def pid_calculations(setpoint):

    global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
    global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
    offset_value: int = 0
    if TWIN_MODE:
        measurement, measurement_time = digital_twin()
    else:
        measurement, measurement_time = read_distance_sensor()

    error = setpoint - measurement

    if previous_time is None:
        previous_error = 0.0
        previous_time = measurement_time
        i_result = 0.0

    error_sum_array[error_sum_counter] = (error * (measurement_time - previous_time))
    p_result = p_value * error
    i_result =  i_value * sum(error_sum_array)
    d_result = d_value * ((error - previous_error) / (measurement_time - previous_time))
    pid_result = offset_value + p_result + i_result + d_result
    previous_error = error
    previous_time = measurement_time

    log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result)
    log_data(data_file="pid", data_line=log_line, remark="") if LOG else None

    if SCREEN:
        print("P_result: ", p_result)
        print("D_result: ", d_result)
        print("I_result: ", i_result)
        print("PID_result: ", pid_result)

    if error_sum_counter <= error_sum_max:
        error_sum_counter = error_sum_counter + 1
    else:
        error_sum_counter = 0

    return pid_result

def control_server_angle(angle):
    KIT.servo[0].angle = angle # Set angle
    log_line = str(angle)
    log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
    print(angle) if SCREEN else None

def digital_twin(pid_angle):
    global start_loop
    measurement_time = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])

    if start_loop:
        delta_t = measurement_time - (measurement_time - 0.002)
        start_loop = False
    else:
        delta_t = measurement_time - previous_time

    twin_data = pd.read_csv('twin_data_file.csv')
    twin_data.set_index('Arm angle', inplace=True)
    acceleration = twin_data.loc[pid_angle, 'Acceleration']

    # previous acceleration to speed.
    new_speed = previous_speed + (acceleration*delta_t)
    measurement = new_speed * delta_t + previous_measurement

    print(measurement)
    print(new_speed)
    print(previous_speed)
    return measurement, measurement_time