#!/usr/bin/env python
import time
import struct
from panda import Panda
from hexdump import hexdump
from panda.isotp import isotp_send, isotp_recv

# 0x7e0 = Toyota
# 0x18DB33F1 for Honda?

def get_current_data_for_pid(pid):
  # 01 xx = Show current data
  isotp_send(panda, "\x01"+chr(pid), 0x7e0)
  return isotp_recv(panda, 0x7e8)

def get_supported_pids():
  ret = []
  pid = 0
  while 1:
    supported = struct.unpack(">I", get_current_data_for_pid(pid)[2:])[0]
    for i in range(1+pid, 0x21+pid):
      if supported & 0x80000000:
        ret.append(i)
      supported <<= 1
    pid += 0x20
    if pid not in ret:
      break
  return ret

if __name__ == "__main__":
  panda = Panda()
  panda.set_safety_mode(Panda.SAFETY_ELM327)
  panda.can_clear(0)

  # 09 02 = Get VIN
  isotp_send(panda, "\x09\x02", 0x7e0)
  ret = isotp_recv(panda, 0x7e8)
  hexdump(ret)
  print "VIN: %s" % ret[2:]

  # 03 = get DTCS
  isotp_send(panda, "\x03", 0x7e0)
  dtcs = isotp_recv(panda, 0x7e8)
  print "DTCs:", dtcs[2:].encode("hex")

  supported_pids = get_supported_pids()
  print "Supported PIDs:",supported_pids

  while 1:
    speed = struct.unpack(">B", get_current_data_for_pid(13)[2:])[0]                  # kph
    rpm = struct.unpack(">H", get_current_data_for_pid(12)[2:])[0]/4.0                # revs
    throttle = struct.unpack(">B", get_current_data_for_pid(17)[2:])[0]/255.0 * 100   # percent
    temp = struct.unpack(">B", get_current_data_for_pid(5)[2:])[0] - 40               # degrees C
    load = struct.unpack(">B", get_current_data_for_pid(4)[2:])[0]/255.0 * 100        # percent
    print "%d KPH, %d RPM, %.1f%% Throttle, %d deg C, %.1f%% load" % (speed, rpm, throttle, temp, load)
    time.sleep(0.2)