"""
This utility is to decode the data received by DBG_CANRX diagnostics.
For example, when we have following system settings:
1) Data Flow: AX940i  =======> over CAN Bus =====> BD992INS
2) On "AX940i" receiver, output NMEA2000 PGN127257, PGN65499, ... over CAN channel.
3) On "BD992INS", enable the "DBG_CANRX" on a TCP/IP port. 
4) On "BD992INS", enable Datalogging with "Log Diagnostic" checked.
5) Download the T04 file from "BD992INS" and run this script on the T04 file.
   Note: we are decoding the received CAN data, so we need T04 file from BD992INS receiver.

In the T04 file, the DBG_CANRX will have following diagnostic output:
  :
  :
--CAN-0 RX ID 19f1199e T 2859930 DATA:1a4bh 85e4h 005dh 782bh
--CAN-0 RX ID 18ffdb9e T 2859930 DATA:0000h 0000h 0000h 0000h
--CAN-0 RX ID 19f1199e T 2860060 DATA:1a4bh 85e4h 005ch 782ch
--CAN-0 RX ID 18ffdb9e T 2860060 DATA:0000h 0000h 0000h 0000h
--CAN-0 RX ID 19f1199e T 2860130 DATA:1a4bh 85e3h 005ch 782dh
--CAN-0 RX ID 18ffdb9e T 2860130 DATA:0000h 0000h 0000h 0000h
--CAN-0 RX ID 19f1199e T 2860250 DATA:1a4bh 85dbh 005eh 782eh
  :
  :
Where "RX ID 19f1199e" is actually CANID, 
where ((0x19f1199e & 0x01FFFF00) >> 8) == 0x1f119 = PGN#127257

similarly ((0x18ffdb9e & 0x01FFFF00) >> 8) == 0x00ffdb = PGN#65499

Also 
"T 2859930" is the  milliseconds of the receiving time.
"DATA:1a4bh 85e4h 005dh 782bh" is 8-bytes of data for each PGN#127257

The utility is to parse the data for each PGN and plot if it is possible. 

"""

import math
import struct
import numpy as np
import re
import sys
import matplotlib
import matplotlib.pyplot as plt
import subprocess


bShowData = True  #t when True, print the raw data before plot

############### Functions ################################
    
  
def saveDataToList(timeIn, X, Y, Z):
  if bPlotY1: plotY1Data.append(X)
  if bPlotY2: plotY2Data.append(Y)
  if bPlotY3: plotY3Data.append(Z)
  if bPlotY1 or bPlotY2 or bPlotY3: timeInData.append(timeIn)
  
def decodePgn65498(timeIn, dataStr):
  """
   PGN# 65498 - Acceleration
   --Byte0 ~ 1: X-acceleration
        Scale Factor: 1.00E-3, e.g. if we send 1000 = 1m/s/s
        Unit: m/s/s
   --Byte2 ~ 3: Y-acceleration
        Scale Factor: 1.00E-3, e.g. if we send 1000 = 1m/s/s
        Unit: m/s/s
   --Byte4 ~ 5: Z-acceleration
        Scale Factor: 1.00E-3, e.g. if we send 1000 = 1m/s/s
        Unit: m/s/s
   --Byte6: Reserved, set to 0
   --Byte7: Reserver, set to 0
  """
  scaleFactor = 1E-3 
  # Note: the bytes of data is in little-endian, so we use '<'
  X = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[0:4]))[0]
  Y = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[4:8]))[0]
  Z = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[8:12]))[0]
  if bShowData: print(f"{timeIn},{X:.3f},{Y:.3f},{Z:.3f}")
  saveDataToList(timeIn, X, Y, Z) 


def decodePgn65499(timeIn, dataStr):
  """
     PGN# 65499 - Angular Rate

     --Byte0 ~ 1: X-Angular Rate (Roll)
        Scale Factor: 1.00E-3, e.g. if we send 1000 = 1 rad/s
        Unit: rad/s
     --Byte2 ~ 3: Y-Angular Rate (Pitch)
        Scale Factor: 1.00E-3, e.g. if we send 1000 = 1 rad/s
        Unit: rad/s
     --Byte4 ~ 5: Z-Angular Rate (Head)
        Scale Factor: 1.00E-3, e.g. e.g. if we send 1000 = 1 rad/s
        Unit: rad/s
     --Byte6: Reserved, set to 0
     --Byte7: Reserver, set to 0  
  """
  scaleFactor = 1E-3 * 180.0/math.pi # also convert from radian to degree
  # Note: the bytes of data is in little-endian, so we use '<'
  RollRate = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[0:4]))[0]
  PitchRate = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[4:8]))[0]
  YawRate = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[8:12]))[0]
  if bShowData: print(f"{timeIn},{YawRate:.3f},{PitchRate:.3f},{RollRate:.3f}")
  saveDataToList(timeIn, RollRate, PitchRate, YawRate) 

def decodePgn65500(timeIn, dataStr):
  """
   PGN# 65500 - X/Y/Z Velocity
   --Byte0 ~ 1: X-Speed
        Scale Factor: 1.00E-2, e.g. if we send 100 = 1 m/s
        Unit: m/s
   --Byte2 ~ 3: Y-Speed
        Scale Factor: 1.00E-2, e.g. if we send 100 = 1 m/s
        Unit: m/s
   --Byte4 ~ 5: Z-Speed
        Scale Factor: 1.00E-2, e.g. if we send 100 = 1 m/s
        Unit: m/s
   --Byte6: Reserved, set to 0
   --Byte7: Reserver, set to 0
  """
  scaleFactor = 1E-2
  # Note: the bytes of data is in little-endian, so we use '<'
  X = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[0:4]))[0]
  Y = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[4:8]))[0]
  Z = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[8:12]))[0]
  if bShowData: print(f"{timeIn},{X:.2f},{Y:.2f},{Z:.2f}")
  saveDataToList(timeIn, X, Y, Z) 
  

def decodePgn127257(timeIn, dataStr):
  """  
  PGN127257 - Yaw/Pitch/Roll
  The data in the single frame contains:
   - Field 1, byte 0: SID (Sequence ID): 1-byte
        An upward counting number used to tie related information together 
        between different PGNs . For example, the SID would be used to tie 
        together the COG, SOG and RAIM values to a given position. 
        255=no valid position fix to tie it to.
        Range 0 to 250 for valid position fixes. 
   
   - Field 2, byte 1, 2, Yaw
        Oscillation of ship about it's vertical axis. 
        Bow moving to starboard is positive.  
        Range: +/- Pi rad
        Resolution: 1x10E-4 rad ( about 0.0057 deg)

   - Field 3, byte 3, 4, Pitch
        Oscillation of ship about it's latitudinal axis. 
        Bow moving up is positive.  
        Range: +/- Pi rad
        Resolution: 1x10E-4 rad ( about 0.0057 deg)

   - Field 4, byte 5, 6, Roll
        Oscillation of ship about it's logitudinal axis. 
        Roll to the starboard is positive.  
        Range: +/- Pi rad
        Resolution: 1x10E-4 rad ( about 0.0057 deg)
        
   - Field 5, byte 7, Reserved Bits
       To fill the CAN Frame
       all set to logic "1"  
  
  """
  scaleFactor = 1E-4 * 180.0/math.pi # also convert from radian to degree
  # Note: the bytes of data is in little-endian, so we use '<'
  Yaw = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[2:6]))[0]
  Pitch = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[6:10]))[0]
  Roll = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[10:14]))[0]
  # print(f"dataStr={dataStr}, Yaw={Yaw}, Pitch={Pitch}, Roll={Roll}")
  if bShowData: print(f"{timeIn},{Yaw:.3f},{Pitch:.3f},{Roll:.3f}")
  saveDataToList(timeIn, Yaw, Pitch, Roll) 

def decodePgn128259(timeIn, dataStr):
  """  
  PGN128259 - Speed Ground Referenced 
  The data in the single frame contains:
   - Field 1, byte 0, Sequence ID   
   - Field 2, byte 1, 2, Speed Water Referenced (all 0's)
   - Field 3, Field 3 : byte 3, 4, Speed Ground Referenced, 
              units:m/s, 
              resolution:1.0e-2 

   - Field 4, byte 5, Speed Water Referenced Type (255 hardcoded)
  """
  scaleFactor = 1E-2
  # Note: the bytes of data is in little-endian, so we use '<'
  X = scaleFactor * struct.unpack('<h', bytes.fromhex(dataStr[6:10]))[0]
  Y = 0
  Z = 0
  if bShowData: print(f"{timeIn},{X:.2f},{Y:.2f},{Z:.2f}")
  saveDataToList(timeIn, X, Y, Z) 

  
############################################################
################### main ###################################
############################################################


usage = f"""
Usage:
   python {sys.argv[0]} <T04FileName> <pgn> [Y1] [Y2] [Y3] [T1=n1] [T2=n2]
   where:
    - <T04FileName>: required, the T04 file with DBG_CANRX diagnostic enabled
    - <pgn>: required, can be 65498, 65499, 65500, 127257, or 128259.
    - Y1, Y2, Y3: optional, when present, the data will be plotted as Y-values
    - T1=n1: optional, specify the beginning time 
    - T2=n2: optional, specify the ending time
      
   When none of Y1, Y2, Y3 is present in the command, there will be no plot
   When T1 is not given, the beginning time is from the first data entry
   When T2 is not given, the ending time is till the last data entry
   Example: 
     1) python decodeDbgCanRxOfNmea2000Pgns.py <T04File> 65499 Y1 Y2 T1=596000 T2=596999
        This command will plot PGN65499 "YawRate" and "PitchRate" from time 596000 to 596999 in the <T04File>
     2) python decodeDbgCanRxOfNmea2000Pgns.py <T04File> 127257 Y1
        This command will plot PGN#127257 "Yaw" data from <T04File>
"""

if (len(sys.argv) < 3):
   print(usage)
   exit()


fileName = sys.argv[1]
wantedPgn = int(sys.argv[2])

T1 = 0
T2 = 0

for arg in sys.argv:
   matched = re.match(re.compile(r"T1=(\d+)"), arg)
   if (matched):
      T1 = int(matched.group(1))
   else:
      matched = re.match(re.compile("T2=(\d+)"), arg)
      if (matched):
         T2 = int(matched.group(1))
         
  
plotY1Data = []
plotY2Data = []
plotY3Data = []
timeInData = []

bPlotY1 = ("Y1" in sys.argv)
bPlotY2 = ("Y2" in sys.argv)
bPlotY3 = ("Y3" in sys.argv)

bShowData = not (bPlotY1 or bPlotY2 or bPlotY3)

pgnHandlingDict = {
   65498: 
          { "title"   : "Acceleration",
            "y1Legend": "X-acceleration",
            "y2Legend": "Y-acceleration",
            "y3Legend": "Z-acceleration",
            "yLabel"  : "m/s/s",
            "call"    : decodePgn65498
          },
   65499: 
          { "title"   : "Angular Rate",
            "y1Legend": "Roll Rate",
            "y2Legend": "Pitch Rate",
            "y3Legend": "Yaw Rate",
            "yLabel"  : "deg/s",
            "call"    : decodePgn65499
          },
   65500: 
          { "title"   : "X/Y/Z Velocity",
            "y1Legend": "X-Speed",
            "y2Legend": "Y-Speed",
            "y3Legend": "Z-Speed",
            "yLabel"  : "m/s",
            "call"    : decodePgn65500
          },
   127257: 
          { "title"   : "Yaw/Pitch/Roll",
            "y1Legend": "Yaw",
            "y2Legend": "Pitch",
            "y3Legend": "Roll",
            "yLabel"  : "deg",
            "call"    : decodePgn127257
          },
   128259: 
          { "title"   : "Speed Ground Referenced",
            "y1Legend": "Ground Speed",
            "y2Legend": "N/A",
            "y3Legend": "N/A",
            "yLabel"  : "m/s",
            "call"    : decodePgn128259
          },
}

pgnToHandle = pgnHandlingDict.get(wantedPgn)

startTime = 0
duration = 0

##################################
def getDiagDataFromT04():
    global T1, T2, startTime, duration
    dataPattern = re.compile("^.* RX ID (.*) T (\d+) DATA:(.*)h$");
    cmd = f"viewdat.exe -d99 {fileName}"
    p = subprocess.run(cmd, shell=True, capture_output=True, text=True)
    if (p.returncode == 0):
        lines = p.stdout.splitlines()
        for line in lines:
            matched = re.match(dataPattern, line)
            if matched:
                pgnNum = (int(matched.group(1), 16) & 0x01FFFF00) >> 8;
                if (pgnNum == wantedPgn):
                  timeIn = int(matched.group(2))/1000  # in seconds
                  if (T1 != 0 and timeIn < T1):
                     continue
                  if (T2 != 0 and timeIn > T2):
                     continue
                  if startTime == 0: 
                     startTime = timeIn
                  duration = timeIn - startTime
                  hexDataStr = "".join(matched.group(3).split("h "))
                  pgnToHandle["call"](timeIn, hexDataStr)
    else:
        print(f"failed command code :{p.returncode}")    

##################################

def main():
    if pgnToHandle == None:
       print(f"\r\n!!!!Not supported PGN#{wantedPgn} !!!!\r\n")
       return
       
    getDiagDataFromT04()

    if bPlotY1 or bPlotY2 or bPlotY3:
      xData = np.array(timeInData)
      legendList = []      
      if bPlotY1: 
         y1Data = np.array(plotY1Data)
         plt.plot(xData, y1Data, 'b')  # blue color
         legendList.append(pgnToHandle["y1Legend"])
         
      if bPlotY2: 
         y2Data = np.array(plotY2Data)
         plt.plot(xData, y2Data, 'r')  # red color
         legendList.append(pgnToHandle["y2Legend"])
        
      if bPlotY3: 
         y3Data = np.array(plotY3Data)
         plt.plot(xData, y3Data, 'g')  # green color
         legendList.append(pgnToHandle["y3Legend"])

      plt.xlabel(f"Seconds (Total {duration:.3f}) {fileName}") # the receiving side system uptime
      plt.title(pgnToHandle["title"] + "(NMEA2000 PGN" + str(wantedPgn) + ")")
      plt.legend(legendList)
      plt.ylabel(pgnToHandle["yLabel"])
    
      plt.show()

if __name__ == "__main__":    
    main()