from adafruit_hcsr04 import HCSR04 as hcsr04        # PWM driver board for servo
import board                                        # PWM driver board for servo
from adafruit_servokit import ServoKit              # Servo libraries
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 adafruit_pcf8591 as pcf8591                  # AD/DA converter board for potentiometer
import numpy as np                                  # Number handling
import pandas as pd                                 # Data handling
import matplotlib.pyplot as plt                     # Plotter handling
from scipy.integrate import odeint                  # Integral calculations
import statistics as st                             # Mean and median calculations
import csv                                          # CSV handling
from datetime import datetime                       # Date and time formatting
import time                                         # Time formatting


######################################## Variables (start) ##################################
# 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

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

# Variables to control PID values (PID formula tweaks)
p_value  = 2
i_value  = 0
d_value  = 0

# Initial variables, used in pid_calculations()
i_result = 0
previous_time = 0
previous_error = 0

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

######################################## Variables (end) ##################################

def initial():
    ...

# Create timestamp strings for logs and screen
def time_stamper():
    log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
    file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
    return log_timestamp, file_timestamp

# Write data to any of the logfiles
def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
    log_stamp, _ = time_stamper()

    with open("pid-balancer_" + data_file + "_data_" + fixed_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(fixed_file_stamp):

    with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
        samples: int = 0
        max_samples: int = 10
        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(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, _ = time_stamper()
                    if samples == max_samples - 1: timestamp_last, _ = time_stamper()

                    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(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
                    print("Distance: ", distance) if SCREEN else None

            except RuntimeError:
                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():


    ############# AnalogOut & AnalogIn Example ##########################
    #
    # This example shows how to use the included AnalogIn and AnalogOut
    # classes to set the internal DAC to output a voltage and then measure
    # it with the first ADC channel.
    #
    # Wiring:
    # Connect the DAC output to the first ADC channel, in addition to the
    # normal power and I2C connections
    #
    #####################################################################
    i2c = board.I2C()
    pcf = PCF.PCF8591(i2c)

    pcf_in_0 = AnalogIn(pcf, PCF.A0)
    pcf_out = AnalogOut(pcf, PCF.OUT)

    while True:
        print("Setting out to ", 65535)
        pcf_out.value = 65535
        raw_value = pcf_in_0.value
        scaled_value = (raw_value / 65535) * pcf_in_0.reference_voltage

        print("Pin 0: %0.2fV" % (scaled_value))
        print("")
        time.sleep(1)


def calculate_velocity():
    ...

def pid_calculations(setpoint):

    global i_result, previous_time, previous_error
    offset_value: int = 320
    measurement, current_time = read_distance_sensor
    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  = 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) / (current_time - previous_time))
    pid_result  = offset_value + p_result + i_result + d_result
    previous_error  = error
    previous_time  = current_time

    return pid_result


def calculate_new_servo_pos():
    ...


def send_data_to_servo():

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