from aecg100 import *
import sys
import time
import math

# Define valid range of R value
MIN_R_Value = 0
MAX_R_Value = 1.0

def calculate_R_Value(target_spo2, degree, A, B, C = None):
    """
    Calculate R value from SpO2 using linear or quadratic equation.
    Returns the valid R value in [0, 1], or None if not found.
    """
    if degree == 1:
        # SpO2 = A + B * R_Value
        R_Value = (A - target_spo2) / B
        R_Values = [R_Value]

    elif degree == 2:
        # SpO2 = A + B * R_Value + C * (R_Value ^ 2)
        if C is None:
            print("Not enough argument to calculate R_Value")
            return None

        # Transforming into general quadratic equation form: a*R^2 + b*R + c = 0
        a = C
        b = B
        c = A - target_spo2

        # Computes the discriminant(D) of the quadratic equation to determine possible R-values
        # D = b^2 - 4*a*c
        D = b ** 2 - 4 * a * c

        if D < 0:
            # No real solution exists if the discriminant(D) is negative
            print("Error: Invalid argument and target SpO2 combination (no real root for R-value)")
            return None

        R_Value_1 = (-b + math.sqrt(D)) / (2 * a)
        R_Value_2 = (-b - math.sqrt(D)) / (2 * a)
        R_Values = [R_Value_1, R_Value_2]

    else:
        print("Invalid degree")
        return None

    valid_R_Value = select_valid_R(R_Values, degree)
    return valid_R_Value


def select_valid_R(R_Values, degree):
    valid_R_Values = [R for R in R_Values if MIN_R_Value <= R <= MAX_R_Value]

    if len(valid_R_Values) == 1:
        R_Value = valid_R_Values[0]

    elif len(valid_R_Values) == 2:
        # the smaller R value is selected, as it typically corresponds to a higher SpO₂
        R_Value = min(valid_R_Values)
        print(
            f"Warning: Two valid R value found ({valid_R_Values[0]:.4f} & {valid_R_Values[1]:.4f})"
            f"Warning: The smaller value {R_Value:.4f} has been selected."
        )

    else:
        if degree == 1:
            print(
                f"Calculated solutions ({R_Values[0]:.4f}) falls outside the reference range ({MIN_R_Value} ~ {MAX_R_Value})."
            )
        elif degree == 2:
            print(
                f"Both calculated solutions ({R_Values[0]:.4f} and {R_Values[1]:.4f}) fall outside the reference range ({MIN_R_Value} ~ {MAX_R_Value})."
            )
        return None

    return R_Value



# Initialize and connect to AECG100
device = AECG100(get_lib_path())
if device.connect(-1, 5000) == False:
    print('Error: device is not connected')
    device.free()
    sys.exit()


# Check the connections by printing out each serial number
print('device is connected... ({} / {})'.format(
    device.get_serial_number(),
    device.get_ppg_serial_number()))
sys.stdout.flush()


# Calculate the R Value from a Target SpO₂
target_spo2 = 85

# R_Value = calculate_R_Value(target_spo2, 1, A=110, B=25)
R_Value = calculate_R_Value(target_spo2, 2, A=111.8, B=-18, C=-11.7)

if R_Value is None:
    device.free()
    sys.exit()


# Derive DC, AC components from the calculated R value
R_DC = 400
R_AC = None
IR_DC = 400
IR_AC = 20

if R_DC is None:
    R_DC = (R_AC * IR_DC) / (R_Value * IR_AC)
elif IR_AC is None:
    R_DC = (R_AC * IR_DC) / (R_Value * R_DC)
elif R_AC is None:
    R_AC = (R_Value * R_DC * IR_AC) / IR_DC
elif IR_DC is None:
    IR_DC = (R_Value * R_DC * IR_AC) / R_AC


# Assign R_DC, R_AC, IR_DC, IR_AC to ppg waveforms
# Get default waveform parameters
ppg_waveform_1 = device.get_default_ppg_ch1_waveform()          # R
ppg_waveform_2 = device.get_default_ppg_ch2_waveform()          # IR
ppg_waveform_1.VolDC = R_DC
ppg_waveform_1.VolSP = R_AC
ppg_waveform_2.VolDC = IR_DC
ppg_waveform_2.VolSP = IR_AC


# Set Heart Rate
bpm = 60
frequency = bpm / 60
# Rescale PPG waveforms
ppg_waveform_1.Frequency = ppg_waveform_2.Frequency = frequency
ppg_waveform_1.TimePeriod = ppg_waveform_2.TimePeriod = int(1000 / frequency)
ppg_waveform_1 = device.rescale_ppg_waveform(ppg_waveform_1)
ppg_waveform_2 = device.rescale_ppg_waveform(ppg_waveform_2)


print(f"  SpO2: {target_spo2}%")
print(f"  HR: {bpm} BPM")
print(f"  R-PI: {(R_AC / R_DC) * 100:.2f}%  R-AC:{R_AC}, R-DC:{R_DC}", flush=True)
print(f"  IR-PI: {(IR_AC / IR_DC) * 100:.2f}%  IR-AC:{IR_AC}, IR-DC:{IR_DC} ", flush=True)
print(f"  R_Value:{(R_AC / R_DC) / (IR_AC / IR_DC):.2f}")

device.play_ppg_ex(pointer(ppg_waveform_1), pointer(ppg_waveform_2), OutputSignalCallback(0), OutputSignalCallback(0))
# device.play_ppg(1, pointer(ppg_waveform_1), OutputSignalCallback(0))        #R
# device.play_ppg(2, pointer(ppg_waveform_2), OutputSignalCallback(0))        #IR
print('Playing...')

time.sleep(60)

device.stop_output()

device.free()
