import Messages
import time
from ProcessList import process_list
from PCANBasic import *
from PCANBasic import TPCANTimestamp, TPCANMsg
import matplotlib.pyplot as plt
import numpy as np

# Initialize PCAN device and set filter for incoming messages with ID 1979
PCAN = PCANBasic()
res = PCAN.Initialize(PCAN_USBBUS1, PCAN_BAUD_500K)
PCAN.FilterMessages(PCAN_USBBUS1, 1979, 1979, PCAN_MESSAGE_STANDARD)


# Infinite loop runs until program is closed
while True:
    print("new")
    # Create empty list for cell voltage message strings
    # Each entry in the list is a message containing the voltage information
    v = []
    # wait 10 seconds to read the cell voltages again
    time.sleep(10)
    # Send message to request cell voltage data
    PCAN.Write(PCAN_USBBUS1, Messages.rCellVInit)
    # pause
    time.sleep(.018)
    # Read the first message
    MSG = PCAN.Read(PCAN_USBBUS1)
    # MSG[1] is the data we are requesting
    MSG = MSG[1]
    # Bytes 4, 5, 6, and 7 store the first two cell voltages
    # For each byte we need to create a binary string and make sure they are 8 bits long
    s4 = "{0:b}".format(MSG.DATA[4])
    while len(s4) != 8:
        s4 = "0" + s4
    s5 = "{0:b}".format(MSG.DATA[5])
    while len(s5) != 8:
        s5 = "0" + s5
    s6 = "{0:b}".format(MSG.DATA[6])
    while len(s6) != 8:
        s6 = "0" + s6
    s7 = "{0:b}".format(MSG.DATA[7])
    while len(s7) != 8:
        s7 = "0" + s7
    # Concatenate all the strings
    s = s4 + s5 + s6 + s7
    # Add them to the list of voltage messages
    v.append(s)

    # For cell voltages we need to request and read 28 additional messages. Each message contains 3 1/2 cell voltages.
    for i in range(1, 29):
        # Request a new messages
        PCAN.Write(PCAN_USBBUS1, Messages.rCellV)
        # Wait 18 ms
        time.sleep(.18)
        # Read the next message
        MSG = PCAN.Read(PCAN_USBBUS1)
        MSG = MSG[1]
        # For the remaining voltages, the cell voltage data is contained in bytes 1 - 7
        s1 = "{0:b}".format(int(MSG.DATA[1]))
        while len(s1) != 8:
            s1 = "0" + s1
        s2 = "{0:b}".format(int(MSG.DATA[2]))
        while len(s2) != 8:
            s2 = "0" + s2
        s3 = "{0:b}".format(MSG.DATA[3])
        while len(s3) != 8:
            s3 = "0" + s3
        s4 = "{0:b}".format(MSG.DATA[4])
        while len(s4) != 8:
            s4 = "0" + s4
        s5 = "{0:b}".format(MSG.DATA[5])
        while len(s5) != 8:
            s5 = "0" + s5
        s6 = "{0:b}".format(MSG.DATA[6])
        while len(s6) != 8:
            s6 = "0" + s6
        s7 = "{0:b}".format(MSG.DATA[7])
        while len(s7) != 8:
            s7 = "0" + s7
        # Concatenate byte strings
        s = s1 + s2 + s3 + s4 + s5 + s6 + s7
        # Append them to the list
        v.append(s)

    vv = process_list(v)