# import numpy as np
# import matplotlib.pyplot as plt
import datetime
# from subprocess import Popen, PIPE, call
import os
import sys
import time

import crc16
import serial

port = "/dev/ttyUSB0"

# parameters of serial connection
kws = dict(
    baudrate=115200,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    timeout=0.02,
)

# constants
EOT = 0x0A
SOT = 0x0D
SOE = 0x5E
ECC = 0x40

type_nack = 0
type_crcerr = 1
type_busy = 2
type_ack = 3
type_read = 4
type_write = 5
type_writeCLR1 = 7
type_data = 8
type_writeTGL1 = 9

missing = b""

# module type
module_dict = {
    0x20: "Koheras AdjustiK/BoostiK (K81-1 to K83-1)",
    0x21: "Koheras BasiK Module (K80-1)",
    0x33: "Koheras BASIK Module (K1x2)",
    0x34: "Koheras ADJUSTIK/ACOUSTIK (K822 / K852)",
    0x36: "Koheras BASIK MIKRO Module (K0x2)",
    0x60: "SuperK Extreme (S4x2), Fianium",
    0x61: "SuperK Extreme Front panel",
    0x66: "RF Driver (A901) & SuperK Select (A203)",
    0x67: "SuperK SELECT (A203)",
    0x68: "SuperK VARIA (A301)",
    0x6A: "NKTP Booster 2013",
    0x6B: "Extend UV (A351)",
    0x70: "BoostiK OEM Amplifier (N83)",
    0x74: "SuperK COMPACT (S024)",
    0x7D: "SuperK EVO (S2x1)",
}

# main class


class Koheras:
    """A class for Koheras Laser Module"""

    def __init__(self, port=port, master=162, slave=1):

        # serial port setting
        self.port = port
        self.kws = kws

        self.master = master
        self.slave = slave

        self.ser = serial.Serial(self.port, **self.kws)

        # read module type
        self.module = self.module_type()
        self.type = module_dict[self.module]
        print("module type:", module_dict[self.module])

        # read serial number
        self.SN = self.module_serial()

        # read setup
        self.setup = self.setup_read()

        # read status
        self.status = self.status_read()

        # read setpoint of output power
        self.pset = self.power_sp_read()

        # read setpoint of wavelength offset
        self.wlset = self.wl_offset_sp_read()

        # read wavelength reference
        self.wlref = self.wl_ref_read()

    def close(self):
        self.ser.close()

    # Common Methods

    def calc_crc(self, msg):
        """calculate CRC value, see https://pypi.org/project/crc16/"""
        try:
            msg_bytes = bytes(msg)
            return crc16.crc16xmodem(msg_bytes)
        except ValueError:
            print("Invalid crc bytes:", msg)
            return None

    def I16toD(self, val):
        """Convert signed 16-bit integer to decimal integer, see http://stackoverflow.com/questions/1604464/twos-complement-in-python"""

        # return -(val & 0b1000_0000_0000_0000) | (val & 0b0111_1111_1111_1111)
        return -(val & 0x8_0_0_0) | (val & 0x7_F_F_F)

    def DtoI16(self, val):
        """Convert decimal integer to signed 16-bit integer, see https://note.nkmk.me/python-bin-oct-hex-int-format/#2"""

        return val & 0xFFFF

    def I32toD(self, val):
        """Convert signed 32-bit integer to decimal integer, see http://stackoverflow.com/questions/1604464/twos-complement-in-python"""

        # return -(val & 0b1000_0000_0000_0000) | (val & 0b0111_1111_1111_1111)
        return -(val & 0x8_0_0_0_0_0_0_0) | (val & 0x7_F_F_F_F_F_F_F)

    def DtoI32(self, val):
        """Convert decimal integer to signed 32-bit integer, see https://note.nkmk.me/python-bin-oct-hex-int-format/#2"""

        return val & 0xFFFF_FFFF

    def test_received(self, msg):
        """Sanity check for the received message"""

        if msg == missing:
            print("Timeout")
            return None

        else:
            msg = list(msg)

        # remove EOT, SOT, SOE & ECC
        # remove SOE
        for idx, char in enumerate(msg):
            if char == SOE:
                if msg[idx - 1] == SOE:
                    pass
                else:
                    msg[idx + 1] = msg[idx + 1] - ECC

        for idx, char in enumerate(msg):
            if char == SOE:
                if msg[idx + 1] == SOE:
                    del msg[idx]
                elif msg[idx + 1] == SOT:
                    del msg[idx]
                elif msg[idx + 1] == EOT:
                    del msg[idx]

        # remove SOT & EOT
        msg = msg[1:-1]

        # CRC test
        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        CRC_msg = msg[-2] * 256 + msg[-1]
        CRC_calc = self.calc_crc(msg[:-2])

        if CRC_calc != CRC_msg:
            print("Invalid message! The received message seems incomplete.")
            return None

        # Status test
        msg_type = msg[2]

        if msg_type in [type_ack, type_data]:
            return msg[4:-2]

        if msg_type == type_nack:
            print("Invalid message! The message sent was not acknowledged.")
            return None

        if msg_type == type_crcerr:
            print("Invalid message! The CRC value of the message sent was not matched.")
            return None

        if msg_type == type_busy:
            print("The module is busy! The message sent was not received.")
            return None

    def cmd_send(self, reg_id, cmd_type=type_read, val=0, val_len=1):
        """generate a binary array sending to the module"""

        if cmd_type == type_read:  # for Read type commands

            # raw message
            msg = [
                self.slave,
                self.master,
                cmd_type,
                reg_id,
            ]

        elif cmd_type == type_write:  # for Write type commands

            if val_len == 2:
                # 2byte value (little endian)
                val_lsb = val % 256
                val_msb = val // 256

                # raw message
                msg = [self.slave, self.master, cmd_type, reg_id, val_lsb, val_msb]

            elif val_len == 4:
                # 4byte value (little endian)
                val_1b = val % 16**2
                val_2b = (val % 16**4) // 16**2
                val_3b = (val % 16**6) // 16**4
                val_4b = val // 16**8

                # raw message
                msg = [
                    self.slave,
                    self.master,
                    cmd_type,
                    reg_id,
                    val_1b,
                    val_2b,
                    val_3b,
                    val_4b,
                ]

            else:
                # 1byte value

                # raw message
                msg = [self.slave, self.master, cmd_type, reg_id, val]

        # calculate CRC value
        CRC = self.calc_crc(msg)
        CRC_lsb = CRC % 256
        CRC_msb = CRC // 256
        msg.extend([CRC_msb, CRC_lsb])

        # convert special characters
        for idx, char in enumerate(msg):
            if (char == EOT) | (char == SOT) | (char == SOE):
                msg[idx] = char + ECC
                msg.insert(idx, SOE)

        # frame the message
        msg.insert(0, SOT)
        msg.append(EOT)

        return msg

    def sendrecv(self, msg, ret=False):
        """Send and receive a binary array to the module"""

        retval = self.ser.write(bytes(msg))
        recv = self.ser.readline()

        if ret:
            return retval, recv
        else:
            return recv

    # Commands

    # Status, setup, module information

    def module_type(self):
        """Read module type"""

        cmd_id = 0x61
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)[0]

        return msg

    def module_serial(self):
        """Read module serial number"""
        cmd_id = 0x65
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        SN = bytes(msg).decode("utf-8")
        print("serial number:", SN)

        return SN

    def status_read(self):
        """Read status of the module"""

        cmd_id = 0x66
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        """
            Koheras AdjustiK/BoostiK (K81-1 to K83-1)
            Bit 0: Emission on
            Bit 1: Interlock relays off
            Bit 2: Interlock supply voltage low (possible short circuit) Bit 3: Interlock loop open
            Bit 4: Module address problem
            Bit 5: SD card problem
            Bit 6: Module communication problem
            Bit 7: No backplane detected
            Bit 8: Illegal MAC address
            Bit 9: Power supply low
            Bit 10: Temperature out of range
            Bit 15: System error code present
        """

        if not msg:
            print("tried to read status, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None
        else:
            return msg[0] + msg[1] * 256

    def setup_read(self):
        """Read setup of the module"""

        cmd_id = 0x31
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read setup, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None
        else:
            return msg[0] + msg[1] * 256

    # Emission

    def emission(self, on):
        """Emission, on=1, off=0"""

        if not on in [0, 1]:
            print("Wrong input value!")
            return None

        cmd_id = 0x30
        typ = type_write
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=on)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            if on == 1:
                print("Emission on")
            else:
                print("Emission off")
        else:
            print("tried switching emission, but command failed")

        return on

    # Output power

    def power_sp_read(self):
        """Read output power setpoint[mW]"""

        cmd_id = 0x22
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the output power setpoint, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        Pset = (msg[0] + msg[1] * 256) / 100  # [mW]
        # print("output power setpoint:", Pset, "mW")

        return Pset

    def power_sp_write(self, Pset):
        """write output power setpoint as Pset[mW]"""

        cmd_id = 0x22
        typ = type_write
        val = int(Pset * 100)
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=val, val_len=2)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            print("output power set to:", Pset, "mW")
        else:
            print("tried to write the output power setpoint, but command failed")

        return Pset

    def power_read(self):
        """Read actual output power[mW]"""

        cmd_id = 0x17
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the actual output power, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        Pout = (msg[0] + msg[1] * 256) / 100  # [mW]
        # print("output power:", Pout, "mW")

        return Pout

    # Wavelength tuning

    def wl_offset_sp_read(self):
        """Read wavelength offset setpoint[pm]"""

        cmd_id = 0x2A
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the wavelength offset setpoint, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        wlset = self.I16toD(msg[0] + msg[1] * 256) / 10  # [pm]

        # print("wavelength offset setpoint", wlset, "pm")

        return wlset

    def wl_offset_sp_write(self, wlset):
        """Write wavelength offset setpoint as wlset[pm]"""

        cmd_id = 0x2A
        typ = type_write
        val = self.DtoI16(int(wlset * 10))
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=val, val_len=2)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            print("wavelength offset set to:", wlset, "pm")
            return wlset

        else:
            print("tried writing the wavelength offset setpoint, but command failed")
            return None

    def wl_ref_read(self):
        """Read reference wavelength[pm]"""

        cmd_id = 0x32
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the reference wavelength, but failed")
            return None

        if len(msg) < 4:
            print("Message too short for unknown reason")
            return None

        wlref = (
            msg[0] + msg[1] * 16**2 + msg[2] * 16**4 + msg[3] * 16**6
        ) / 10  # [pm]

        # print("reference wavelength: {:,} pm".format(wlref))

        return wlref

    def wl_read(self):
        """Read actual wavelength offset[pm]"""

        cmd_id = 0x72
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the actual wavelength offset, but command failed")
            return None

        if len(msg) < 4:
            print("Message too short for unknown reason")
            return None

        wl = (
            self.I32toD(msg[0] + msg[1] * 16**2 + msg[2] * 16**4 + msg[3] * 16**6)
            / 10
        )  # [pm]

        # print("wavelength offset:", wl, "pm")

        return wl

    # Temperature

    def module_temp_read(self):
        """Read temperature of the module[ºC]"""

        cmd_id = 0x1C
        typ = type_read
        cmd = self.cmd_send(cmd_id, cmd_type=typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read the module temperature, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        temp = self.I16toD(msg[0] + msg[1] * 16**2) / 10  # [ºC]

        # print("module temperature:", temp, "ºC")

        return temp

    # Modulation

    def wl_modulation(self, modon):
        """Enable/disable wavelength modulation, on=1, off=0"""

        if not modon in [0, 1]:
            print("Wrong input value!")
            return None

        cmd_id = 0xB5
        typ = type_write
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=modon)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            if modon == 1:
                print("Wevwlength modulation1 on")
            else:
                print("Wavelength modulation off")
        else:
            print("tried switching modulation on/off, but command failed")
            return None

        return modon

    def wl_modtype(self, modtype):
        """Set modulation range, narrow=1, wide=0"""

        if not modtype in [0, 1]:
            print("Wrong input value!")
            return None

        self.setup = self.setup_read()

        cmd_id = 0x31
        typ = type_write
        val = (self.setup & 0xFFFD) | (modtype << 1)
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=val, val_len=2)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            if modtype == 0:
                print("wide range")
            else:
                print("narrow range")
        else:
            print("tried switching modulation type, but command failed")
            return None

        return modtype

    def wl_coupling(self, modcoup):
        """Set modulation coupling, DC=1, AC=0"""

        if not modcoup in [0, 1]:
            print("Wrong input value!")
            return None

        self.setup = self.setup_read()

        cmd_id = 0x31
        typ = type_write
        val = (self.setup & 0xFFFB) | (modcoup << 2)
        cmd = self.cmd_send(cmd_id, cmd_type=typ, val=val, val_len=2)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if msg == []:
            if modcoup == 0:
                print("AC coupled")
            else:
                print("AC coupled")
        else:
            print("tried switching modulation coupling, but command faield")

        return modcoup

    # Supply voltage

    def voltage_read(self):
        """Read supplied voltage [V]"""

        cmd_id = 0x1E
        typ = type_read
        cmd = self.cmd_send(cmd_id, typ)
        msg = self.sendrecv(cmd)
        msg = self.test_received(msg)

        if not msg:
            print("tried to read supply voltage, but command failed")
            return None

        if len(msg) < 2:
            print("Message too short for unknown reason")
            return None

        Vdev = self.I16toD(msg[0] + msg[1] * 16**2) / 1000  # [V]
        # print("Supplied voltage:", Vdev, "[V]")

        return Vdev