import logging
import time
import csamplePump
import exceptions
from machine import UART

fidolog = logging.getLogger("fido")
testing_prefix = __name__ + ": "

NUMBER_OF_READINGS = 16


def init():
    # initialize serial port for sample pump
    global samplePumpUART
    csamplePump.init()
    samplePumpUART = UART(3,115200)
    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)


def getline():
    return samplePumpUART.readline()


# getData() returns: [target_deciPSI,actual_deciPSI,battery_deciAmp,dutypercent,error_code]
# Error_codes:
#    0          no error
#   -1          nothing returned from read, possible timeout
#   -2          empty line
#   -3          returnd line is to short <=1 character
#   -4          valid line conditions not met
#   -5          not the correct number of data components to be valid (4)
def getData():
    rawbytes = samplePumpUART.readline()
    if (rawbytes is not None):
        if rawbytes[0] == 13:
            rawstring = rawbytes.decode('UTF-8')
            rawstrlen = len(rawstring)
            if rawstrlen > 1:
                if (rawstring[0] == '\r' and rawstring[rawstrlen-1] == '\n' and rawstring.count('\r') == 1 and rawstring.count('\n') == 1 and rawstring.count(',') == 4):
                    stripsplit = rawstring.strip().split(',')
                    if len(stripsplit) == 5:
                        target_deciPSI = int(stripsplit[0])
                        actual_deciPSI = int(stripsplit[1])
                        battery_deciAmp = int(stripsplit[2])
                        dutypercent = int(stripsplit[3])
                        turbidity = int(stripsplit[4])
                        return [target_deciPSI,actual_deciPSI,battery_deciAmp,dutypercent,turbidity,0]
                    return [0,0,0,0,0,-5]
                return [0,0,0,0,0,-4]
            return [0,0,0,0,0,-3]
        return [0,0,0,0,0,-2]
    return [0,0,0,0,0,-1]


def on():
    csamplePump.on()


def off():
    csamplePump.off()


def start():
    csamplePump.off()
    time.sleep(1)
    csamplePump.on()
    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=11000)
    for count in range(2):
        rawbytes = samplePumpUART.readline()
        if rawbytes is not None:
            line = rawbytes.decode('UTF-8')
            line = line.replace("\r", "")
            line = line.strip()
            index = line.find("Start")
            line = line[index:]
            if count == 0:
                fidolog.debug(line)
            if count == 1:
                fidolog.debug(line)
                tagphrase = line[0:8:1]
                if tagphrase == "Starting":
                    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)
                    return 1
        else:
            samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)
            csamplePump.off()
            raise exceptions.SamplePumpError("No response from Pump Controller")
    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)
    csamplePump.off()
    raise exceptions.SamplePumpError("start bad data lines from sample pump")


def setPressure(pressurePSI):
    # For reason not explained here,
    # the sample pump is reading half scale in 0.1 psi
    # therefore requesting 20psi is 20*10/2 = 100
    if pressurePSI > 26 or pressurePSI < 2:
        raise exceptions.SamplePumpError("setPressure Pressure outside or range")
    pressure = int(pressurePSI*10/2)
    pressurestr = "{:d}".format(pressure)
    samplePumpUART.write('!VAR 1 '+pressurestr+'\n\r')
    return 1


def stablePressure(pressurePSI, errorPSI):
    readingIndex = 0
    removeIndex = 0
    readingSum = 0
    readings = [0]*NUMBER_OF_READINGS
    count = 0
    missed = 0
    setPoint_deciPSI = int(pressurePSI*10/2)
    error_deciPSI = int(errorPSI*10/2)
    while count < NUMBER_OF_READINGS and missed < 8:
        [target_deciPSI,actual_deciPSI,battery_deciAmp,dutypercent,turbidity,error] = getData()
        if error >= 0:
            fidolog.debug("Current {} of {} in (deciPSI)  = {}".format(count,NUMBER_OF_READINGS,actual_deciPSI))
            count = count+1
            missed = 0
            readings[readingIndex] = actual_deciPSI
            readingIndex = readingIndex+1
            readingSum = readingSum+actual_deciPSI
        else:
            missed = missed + 1
            if missed >= 8:
                raise exceptions.SamplePumpError("stablePressure To many missed reading")
    readingIndex = 0
    readingAvg = readingSum/NUMBER_OF_READINGS
    while count < 600 and missed < 8:
        fidolog.debug("Current avg (deciPSI) = {}".format(readingAvg))
        if ((readingAvg < (setPoint_deciPSI + error_deciPSI)) and (readingAvg > (setPoint_deciPSI - error_deciPSI))):
            return readingAvg
        [target_deciPSI,actual_deciPSI,battery_deciAmp,dutypercent,turbidity,error] = getData()
        if error >= 0:
            missed = 0
            count = count + 1
            readingSum = readingSum - readings[removeIndex]
            removeIndex = removeIndex + 1
            if removeIndex >= NUMBER_OF_READINGS:
                removeIndex = 0
            readings[readingIndex] = actual_deciPSI
            readingIndex = readingIndex + 1
            readingSum = readingSum + actual_deciPSI
            if readingIndex >= NUMBER_OF_READINGS:
                readingIndex = 0
            readingAvg = readingSum/NUMBER_OF_READINGS
        else:
            missed = missed+1
            if missed >= 8:
                raise exceptions.SamplePumpError("stablePressure To many missed reading")
    raise exceptions.SamplePumpError("stablePressure Sample pump pressure failed to stabalize")


# Roboteq SDC2160 MicroBasic hex updater.
# The sample pump controller is the Roboteq SDC2160, using samplePumpUART.
# This first sends !EX to emergency-stop the running controller/script behavior,
# then enters script-load mode and transfers a RoboRun+ compiled .hex file.

def _roboteq_uart_nonblocking():
    # Reconfigure the already-created UART to avoid UART.read() blocking.
    # This matches the UART setup in init(), but with timeout=0.
    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=0)
    return samplePumpUART


def _roboteq_uart_normal_timeout():
    # Restore the timeout expected by the existing sample pump code.
    samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)


def _roboteq_flush_uart(uart, quiet_ms=100, max_ms=1000):
    # Drain received bytes, but do not wait forever.  This avoids the old
    # while uart.any(): uart.read() hang if the controller is still talking.
    start = time.ticks_ms()
    last_data = start

    while time.ticks_diff(time.ticks_ms(), start) < max_ms:
        n = uart.any()
        if n:
            uart.read(n)
            last_data = time.ticks_ms()
        else:
            if time.ticks_diff(time.ticks_ms(), last_data) >= quiet_ms:
                return 1
            time.sleep_ms(5)
    return 0


def _roboteq_read_until(uart, token, timeout_ms=3000):
    start = time.ticks_ms()
    buf = b""

    while time.ticks_diff(time.ticks_ms(), start) < timeout_ms:
        n = uart.any()
        if n:
            data = uart.read(n)
            if data:
                buf += data
                if token in buf:
                    return buf
        else:
            time.sleep_ms(5)

    return buf


def roboteqEmergencyStop():
    # Roboteq emergency stop.  This should stop the running pump script/drive
    # behavior before entering the script loader.
    uart = _roboteq_uart_nonblocking()
    uart.write(b"!EX\r")
    time.sleep_ms(100)
    _roboteq_flush_uart(uart, quiet_ms=100, max_ms=1000)
    _roboteq_uart_normal_timeout()
    return 1


def roboteqClearEmergencyStop():
    # Manual recovery helper.  Do not call automatically after uploading unless
    # you intentionally want to release the Roboteq from emergency stop.
    uart = _roboteq_uart_nonblocking()
    uart.write(b"!MG\r")
    time.sleep_ms(100)
    _roboteq_flush_uart(uart, quiet_ms=100, max_ms=1000)
    _roboteq_uart_normal_timeout()
    return 1


def uploadRoboteqHex(filename="/flash/roboteq.hex", emergency_stop=1):
    # samplePumpUART must already exist from init().
    if 'samplePumpUART' not in globals():
        raise exceptions.SamplePumpError("uploadRoboteqHex called before samplePump.init()")

    uart = _roboteq_uart_nonblocking()

    csamplePump.off()  #shut it off
    time.sleep(1)   #make sure it is reset
    csamplePump.on()  #back on
    time.sleep(10)  #wait for it to start up

    try:
        if emergency_stop:
            # Stop the active SDC2160 script/control behavior first.  This should
            # greatly reduce or eliminate serial chatter before the loader starts.
            uart.write(b"!EX\r")
            time.sleep_ms(200)

        _roboteq_flush_uart(uart, quiet_ms=150, max_ms=1500)

        # Enter Roboteq script-load mode.
        uart.write(b"%SLD 321654987\r")
        resp = _roboteq_read_until(uart, b"HLD", 3000)
        if b"HLD" not in resp:
            raise exceptions.SamplePumpError("Roboteq did not enter HLD mode: {}".format(resp))

        try:
            f = open(filename, "rb")
        except OSError as e:
            samplePumpUART.init(115200,bits=8,parity=None,stop=1,timeout=1500)
            raise exceptions.SamplePumpError("uploadRoboteqHex unable to open file '{}': {}".format(filename, e))

        with f:
            line_num = 0
            for raw in f:
                line_num += 1
                line = raw.strip()
                if not line:
                    continue

                uart.write(line + b"\r")

                ack = _roboteq_read_until(uart, b"+", 1000)
                if b"+" not in ack:
                    raise exceptions.SamplePumpError("No + ack at line {}: {}".format(line_num, ack))

        fidolog.info("Roboteq hex upload complete")
        return 1

    finally:
        _roboteq_uart_normal_timeout()