from machine import I2C, Pin, PWM
from utime import sleep, sleep_ms, ticks_ms
# from pico_i2c_lcd import I2cLcd
import dht
import math
import onewire
import ds18x20


# Global variables for box status and temperature control
box_status = False
ts_pos = 0
target_temperature_in = 0.0
max_temperature_heater = 0.0
target_fan_percent = 10.0
TEMPERATURE_PRECISION = 10

# Temperature and humidity sensor readings
temperature_in = 0.0
temperature_out = 0.0
temperature_heater = 0.0
humidity_in = 0.0
humidity_out = 0.0
fan_duty = 0.0

# PID control parameters
pid_first_millis = 0
pid_last_millis = 0
PID_TEMP_PROXIMITY = 5
PID_KP = 65
PID_KI = 81
PID_KD = 50
PID_SAMPLES = 5
temperature_samples_in = [0.0] * PID_SAMPLES
temperature_samples_heater = [0.0] * PID_SAMPLES

# Hardware and limits configuration
LIMIT_TEMP_IN_MIN = 20
LIMIT_TEMP_IN_MAX = 70
LIMIT_TEMP_HEATER_MIN = 20
LIMIT_TEMP_HEATER_MAX = 75
LIMIT_FAN_SPEED_MIN = 0
LIMIT_FAN_SPEED_MAX = 100
PWM_MAX_VALUE = 1024
PWM_HEATER_INVERT_VALUES = 1
PWM_FREQ_HEATER = 500
PWM_FREQ_FAN = 25000
PWM_RESOLUTION = 10
HEATER_PWM_OFF = PWM_MAX_VALUE
HEATER_DUTY_OFF = False

# LED Pins
PIN_LED_RED = 8     # Replace with the actual pin number
PIN_LED_GREEN = 10   # Replace with the actual pin number
PIN_LED_BLUE = 12
PIN_LED_RED = Pin(PIN_LED_RED, Pin.OUT)
PIN_LED_GREEN = Pin(PIN_LED_GREEN, Pin.OUT)
PIN_LED_BLUE = Pin(PIN_LED_BLUE, Pin.OUT)
LED_STATUS_HEATER = PIN_LED_RED  # Replace with the actual pin number

# DEBUG variables
DEVICE_DISCONNECTED_C = -127
DEF_DEBUG_SENSOR_SAMPLES = 0
DEF_DEBUG_PWM_VALUES = 0
DEF_DEBUG_HETER_SAMPLES = 0
DEF_DEBUG_HEATER_SAMPLES = True

# I2C Configuration
# i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
# i2c_1 = I2C(0, sda=Pin(2), scl=Pin(3), freq=400000)
# lcd = I2cLcd(i2c, 2, 16)

# Environmental sensors initialization
temp_sensor_in = dht.DHT22(Pin(28))
temp_sensor_out = dht.DHT22(Pin(27))

# Heater sensor initialization
ow = onewire.OneWire(Pin(26))
ds = ds18x20.DS18X20(ow)
heater_sensors = ds.scan()

# Heater and fan PIN initialization
PIN_HEATER_CTL = 3
PIN_FAN_PWM = 4
PIN_HEATER_CTL = Pin(PIN_HEATER_CTL, Pin.OUT)
PIN_FAN_PWM = Pin(PIN_FAN_PWM, Pin.OUT)

# Heater PWM -- Make sure heater is initially OFF
PWM_CH_HEATER = 0  # Replace with the actual channel number
ledc_heater = PWM(PIN_HEATER_CTL, freq=PWM_FREQ_HEATER, duty_u16=HEATER_PWM_OFF)

# FAN PW -- Make sure FAN PWM is set to maximum (fan OFF)
PWM_CH_FAN = 1
ledc_fan = PWM(PIN_FAN_PWM, freq=PWM_FREQ_FAN, duty_u16=PWM_MAX_VALUE)

#PRINT na LCD ekran print("Ready to go.") zatim lcd clear kao inicijalni conf

def LED_ON(pin):
    pass

# Function to turn off LED (replace with actual implementation)
def LED_OFF(pin):
    pass

def sample_sens_in_and_out():
    tmp_temp = 0.0
    tmp_humid = 0.0
    i = 5

    # Read temperature and humidity from the out sensor
    while i > 0:
        temp_sensor_out.measure()
        tmp_temp = temp_sensor_out.temperature()
        # delay(5)
        tmp_humid = temp_sensor_out.humidity()
        if not math.isnan(tmp_temp) and not math.isnan(tmp_humid):
            break
        sleep(50)
        i -= 1
    # Make sure final values are valid
    if not math.isnan(tmp_temp) and not math.isnan(tmp_humid):
        global temperature_out, humidity_out
        temperature_out = tmp_temp
        humidity_out = tmp_humid

        # Debug print if enabled
        if DEF_DEBUG_SENSOR_SAMPLES:
            print(f"TempOut: {temperature_out} - HumidOut: {humidity_out}")

    # Otherwise we have a problem
    else:
        print("Out sensor: I2C error!")
    # Read SHT temperature and humidity
    i = 5
    while i > 0:
        temp_sensor_in.measure()
        tmp_temp = temp_sensor_in.temperature()
        tmp_humid = temp_sensor_in.humidity()
        if not math.isnan(tmp_temp) and not math.isnan(tmp_humid):
            break
        sleep(50)
        i -= 1
    # Make sure final values are valid
    if not math.isnan(tmp_temp) and not math.isnan(tmp_humid):
        global temperature_in, humidity_in
        temperature_in = tmp_temp
        humidity_in = tmp_humid

        # Debug print if enabled
        if DEF_DEBUG_SENSOR_SAMPLES:
            print(f"TempIn: {temperature_in} - HumidIn: {humidity_in}")

    # Otherwise we have a problem
    else:
        print("In sensor: I2C error!")

def readHeaterTemperature():
    temp_c = 0.0
    i = 5
    ds.convert_temp()
    while i > 0:
        sleep_ms(int(750 / (2 ** (12 - TEMPERATURE_PRECISION))))
        for heater_sensor in heater_sensors:
            temp_c = ds.read_temp(heater_sensor)
            if temp_c != DEVICE_DISCONNECTED_C:
                return temp_c
                # if DEF_DEBUG_SENSOR_SAMPLES:
                #     print("Temp C: " + float(temp_c))
                # #endif
            else:
                # if DEF_DEBUG_SENSOR_SAMPLES:
                print("Heater sensor fault: " + str(temp_c))
            sleep_ms(int(750 / (2 ** (12 - TEMPERATURE_PRECISION))))
            i -= 1
    return None

def set_fan_duty(duty):
    if duty > 100:
        duty = 100
    global fan_duty
    fan_duty = duty
    pwm_raw_fan = int((PWM_MAX_VALUE * (100 - duty)) / 100.0)
    if DEF_DEBUG_PWM_VALUES:
        print(f"Setting fan to {pwm_raw_fan}")
    ledc_fan.duty_u16(pwm_raw_fan)

def set_heater_duty(duty):
    if duty > 100:
        duty = 100
    pwm_raw_heater = int((PWM_MAX_VALUE * duty) / 100.0)
    if DEF_DEBUG_PWM_VALUES:
        print(f"Setting heater duty {duty}%")
    set_heater_pwm(pwm_raw_heater)
def set_heater_pwm(pwm):
    if pwm > PWM_MAX_VALUE:
        pwm = PWM_MAX_VALUE
    # Apply inversion if specified
    if PWM_HEATER_INVERT_VALUES:
        pwm = PWM_MAX_VALUE - pwm
    # Debug print if enabled
    if DEF_DEBUG_PWM_VALUES:
        print(f"set_heater_duty: {pwm}/{PWM_MAX_VALUE}")
    ledc_heater.duty_u16(pwm)

def sample_temperatures(temp_in, heater):
    global ts_pos, pid_first_millis, pid_last_millis
    temperature_samples_in[ts_pos] = temp_in
    temperature_samples_heater[ts_pos] = heater
    ts_pos += 1
    if ts_pos == 0:
        # Record milliseconds of our first sample
        pid_first_millis = ticks_ms()
    if ts_pos >= PID_SAMPLES:
        pid_last_millis = ticks_ms()
        ts_pos = 0
        return True
    else:
        return False

def heater_recalc_pwm():
    global average, pid_del_p, pid_del_i, pid_del_d, pid_val_p, pid_val_i, pid_val_d, pwm_val
    global pid_target_temperature, pid_temperature, pid_temperature_previous, pid_time_diff

    # We are using a simple PID-like control loop to calculate PWM value for a
    # heater Heater is heating up our box to a target temperature and keeping
    # it steady after/if that temperature is reached. At the same time we have
    # to ensure heater does not go beyond our "safe" temperature and start
    # damaging itself/fillament/cables/enclosure etc.
    #
    # As a simple solution, we will use the following flow
    # 1. Check if heater temperature is higher or equal to the maximum set
    # heater temperature
    # 		- if True, use heater max temperature as the target for our PID
    # controller
    # 		- if False, use enclosure maximum temperature as the target for
    # our PID controller
    average = 0
    pid_del_p = 0
    pid_del_i = 0
    pid_del_d = 0
    pid_val_p = 0
    pid_val_i = 0
    pid_val_d = 0
    pwm_val = 0
    pid_target_temperature = 0.0
    pid_temperature = 0.0
    pid_temperature_previous = 0.0
    pid_time_diff = 0
    # Make sure values are valid
    if math.isnan(temperature_heater):
        print("Invalid temperature values")
        print(f"temp_heater: {temperature_heater}")
        print(f"target_temperature_in: {target_temperature_in}")
        set_heater_duty(HEATER_DUTY_OFF)
        return
    # Inside temperature has NOT reached the target temperature
    # However, heater temperature is approaching it's maximum allowed
    # temperature. Then, regulate heater max temperature
    if temperature_heater >= (max_temperature_heater - PID_TEMP_PROXIMITY) and temperature_in < target_temperature_in:
        print("Using HEATER temperature as target.")
        for i in range(PID_SAMPLES):
            average += temperature_samples_heater[i]
        average /= PID_SAMPLES
        pid_target_temperature = max_temperature_heater
        pid_temperature = temperature_heater
        pid_temperature_previous = temperature_samples_heater[PID_SAMPLES - 1]
    # Heater is not close to it's maximum allowed temperature
    # Or, inside temperature has already reached the target value
    else:
        print("Using IN temperature as target.")
        for i in range(PID_SAMPLES):
            average += temperature_samples_in[i]
        average /= PID_SAMPLES
        pid_target_temperature = target_temperature_in
        pid_temperature = temperature_in
        pid_temperature_previous = temperature_samples_in[PID_SAMPLES - 1]
    # P
    pid_del_p = pid_target_temperature - pid_temperature
    pid_val_p = int(pid_del_p) * PID_KP
    # I
    pid_del_i = pid_target_temperature - average
    pid_val_i = int(pid_del_i) * PID_KI
    # D
    pid_del_d = pid_target_temperature - pid_temperature_previous
    pid_time_diff = pid_last_millis - pid_first_millis
    if pid_time_diff > 0:
        pid_val_d = int(pid_del_d) / pid_time_diff
        pid_val_d = int(pid_val_d) * PID_KD
    else:
        pid_val_d = 0
    # P + I + D
    pwm_val = pid_val_p + pid_val_i + pid_val_d
    # Debug print statements
    print(f"Pd: {pid_del_p}")
    print(f"Pv: {pid_val_p}")
    print(f"Id: {pid_del_i}")
    print(f"Iv: {pid_val_i}")
    print(f"Dd: {pid_del_d}")
    print(f"Dv: {pid_val_d}")
    print(f"Calc PWM val: {pwm_val}")

    # If DEF_DEBUG_PID
    if pwm_val > PWM_MAX_VALUE:
        pwm_val = PWM_MAX_VALUE
    elif pwm_val < 0:
        pwm_val = 0
    # If DEF_DEBUG_PID
    print(f"New PWM val: {pwm_val}")
    set_heater_pwm(pwm_val)

def loop():
    global temperature_heater, temperature_in, target_temperature_in, box_status
    # Sample temperature and humidity from all available sensors
    if not readHeaterTemperature():
        # Handle heater sensor error
        print("Failed to read heater temperature!")

    sample_sens_in_and_out()

    # Debug
    if DEF_DEBUG_HEATER_SAMPLES:
        print(f"{temperature_in}\t{temperature_heater}\t{target_temperature_in}")

    # Check if we have WiFi connection, if not try to reconnect
    # check_wifi_connection()

    # Print current states
    print("Current States:")
    print(f"Box Status: {'On' if box_status else 'Off'}")
    print(f"Target Temperature: {target_temperature_in} °C")
    print(f"Fan Speed: {fan_duty}%")
    print(f"Max Heater Temperature: {max_temperature_heater} °C")
    print(f"Current Heater Temperature: {temperature_heater} °C")
    print(f"Current Inside Temperature: {temperature_in} °C")


    # Dry box is active
    if box_status:
        LED_ON(LED_STATUS_HEATER)
        # Sample temperature values
        if sample_temperatures(temperature_in, temperature_heater):
            # Recalculate PWM value for the heater
            heater_recalc_pwm()
    # Dry box is off. Turn/Keep off the heater
    else:
        LED_OFF(LED_STATUS_HEATER)
        set_heater_duty(HEATER_DUTY_OFF)

# Function to set target temperature, fan speed, and max heater temperature
# def set_control_parameters(target_temp, fan_speed, max_heater_temp):
#     global target_temperature_in, target_fan_percent, max_temperature_heater
#     target_temperature_in = target_temp
#     target_fan_percent = fan_speed
#     max_temperature_heater = max_heater_temp

# Function to turn on the dry box
def turn_on_dry_box():
    global box_status
    box_status = True

# Function to turn off the dry box
def turn_off_dry_box():
    global box_status
    box_status = False
    set_fan_duty(0)
    set_heater_duty(HEATER_DUTY_OFF)

def main():
    """Main function to run the I2C LCD display example."""
    while True:
        # set_control_parameters(60.0, 50.0, 70.0)  # Example values, replace with your desired values
        turn_on_dry_box()
        loop()

if __name__ == '__main__':
    main()