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
import time                                         # Time formatting

import main

# Variables to control sensor
TRIGGER_PIN = board.D4      # GPIO pin xx
ECHO_PIN = board.D17        # GPIO pin xx
TIMEOUT: float = 0.1        # Timout for echo wait
MIN_DISTANCE: int = 4       # Minimum sensor distance to considered valid
MAX_DISTANCE: int = 40      # Maximum sensor distance to considered valid

# Variables to control servo
KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE = 500             # Defines angle 0
MAX_PULSE = 2500            # Defines angle 180
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 = False         # More data to display

# Control the number of samples for single measurement
MAX_SAMPLES = 10

# Control the number of samples for the potentiometer
PCF_VALUE = 65535
POT_MAX = 65280
POT_MIN = 256
POT_INTERVAL = 0.01

# Variables to assist PID calculations
current_time: float = 0
integral: float = 0
time_prev: float  = -1e-6
error_prev: float = 0

# 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

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

def initial():
    ...

# Write data to any of the logfiles
def log_data(file_stamp: str, 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_" + 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(file_stamp):

    # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long distance)
    # Return the mean timestamp and median distance.
    with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=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:
            try:
                distance: float = sonar.distance
                if MIN_DISTANCE < distance < MAX_DISTANCE:

                    log_data(file_stamp,"sensor", str(distance), None) if LOG else None
                    print("Distance: ", 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: list = st.median(sample_array)
                    mean_timestamp: float  = st.mean([timestamp_first_float, timestamp_last_float])
                    print(median_distance) if SCREEN else None
                    print(mean_timestamp) if SCREEN else None

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

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

    return median_distance, mean_timestamp

def read_setpoint(file_stamp):

    i2c = board.I2C()
    pcf = PCF.PCF8591(i2c)
    pcf_in_0 = AnalogIn(pcf, PCF.A0)
    pcf_out = AnalogOut(pcf, PCF.OUT)
    pcf_out.value = PCF_VALUE

    while True:
        raw_value: int = pcf_in_0.value
        scaled_value: float = (raw_value / PCF_VALUE) * pcf_in_0.reference_voltage
        # Calculate angle in reference to raw pot values
        angle =  round(((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN),0)
        log_line = str(scaled_value) + "," + str(raw_value) + "," + str(angle)
        log_data(file_stamp=file_stamp, data_file="potmeter", data_line=log_line, remark=None) if LOG else None

        if SCREEN:
            print('pin 0= ', pcf.read(0))
            print('raw_value= ',raw_value)
            print("pin 0= %0.2fV" % scaled_value)
            print('Scaled= ' , scaled_value)
            print(angle)

        time.sleep(POT_INTERVAL)
        send_servo_angle(set_angle=angle)

def calculate_velocity(file_stamp):

    velocity = "0"
    log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None


def pid_calculations(file_stamp, setpoint):

    global i_result, previous_time, previous_error
    offset_value: int = 320
    measurement, measurement_time = read_distance_sensor(file_stamp=main.file_stamp) # todo Check logging
    error: float = setpoint - measurement
    error_sum: float = 0.0

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

    error_sum: float = error_sum +  (error * (current_time - previous_time))
    p_result = p_value * error
    i_result  =  i_value * error_sum
    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(file_stamp=file_stamp, data_file="pid", data_line=log_line, remark=None) if LOG else None
    return pid_result


def calculate_servo_position():
    ...


def send_servo_angle(set_angle):

    KIT.servo[0].angle = set_angle # Set angle