#! /usr/bin/python
#

import serial
import hal
import time
import sys

B_Z0=5
B_Y0=11
B_X0=16

# 7-Segment-Decoder
s7dec = bytearray(13)
s7dec[0] = 0xEE
s7dec[1] = 0x82
s7dec[2] = 0xDC
s7dec[3] = 0xD6
s7dec[4] = 0xB2
s7dec[5] = 0x76
s7dec[6] = 0x7E
s7dec[7] = 0xC2
s7dec[8] = 0xFE
s7dec[9] = 0xF6
s7dec[10] = 0x10
s7dec[11] = 0x7C
s7dec[12] = 0xFF

# Variable zum Version auslesen
bout_config = bytearray(4)
bout_config[0] = 0xAA
bout_config[1] = 0x01
bout_config[2] = 0x00
bout_config[3] = 0x00
bout_config[2] = bout_config[0] ^ bout_config[1] ^ bout_config[3];

# Variable zum Daten Senden
bout_data    = bytearray(22)
bout_data[0] = 0xAA;
bout_data[1] = 0x10;
bout_data[2] = 0x00;
bout_data[3] = 0x12;


# Funktion zum Antwort lesen
def readback(sp):
  if sp.isOpen():

    j = 0;
    ret = sp.read(4)

    r = bytearray(ord(ret[3]) + 4)
    for i in ret:
      r[j] = ord(i)
      j+=1

    if ord(ret[3]) > 0:
      ret = sp.read(ord(ret[3]))

      for i in ret:
        r[j] = ord(i)
        j+=1

    return r
  else: return 0 

#
# Beginn Hauptprogram
#

# Halpins anlegen...
h = hal.component("hal_mydro")
h.newpin("x_val", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("y_val", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("z_val", hal.HAL_FLOAT, hal.HAL_IN)

# Port oeffnen..
if len(sys.argv) > 0:
  ser = serial.Serial(sys.argv[1], timeout=1)


if ser.isOpen():

  # read version
  b = ser.write(bout_config)
  ser.flush()
  r = readback(ser)
  if r[1] == 0x20:
    print 'hal_mydro: Version = ' + hex(r[4])
  else:
    print 'hal_mydro: Get Version failed!'
    h.exit()


  # Geraet gefunden und funzt...
  h.ready()

  try:
    while 1:

      # Z-Daten
      l=B_Z0
      k=abs(h.z_val)+0.001
      for i in range(2,-3,-1):
        t = k
        j = int(k/pow(10,i))
        k = k - j * pow(10,i)
        #print 'Stelle=' + str(i) + " Wert=" + str(j)
        if j > 9:
          j=11
        bout_data[l] = s7dec[j]
        l += 1
      if h.z_val < 0:
        if bout_data[B_Z0] == s7dec[0]:
          bout_data[B_Z0] = s7dec[10]
        else:
          bout_data[B_Z0] = s7dec[11]

      bout_data[7] += 1   # Dezimalpunkt
      bout_data[4] = 0xff # LEDs


  
      # Y-Daten
      l=B_Y0
      k=abs(h.y_val)+0.001
      for i in range(2,-3,-1):
        t = k
        j = int(k/pow(10,i))
        k = k - j * pow(10,i)
        if j > 9:
          j=11
        bout_data[l] = s7dec[j]
        l += 1
        #print 'bout_pos=' + str(l) + 'Stelle=' + str(i) + " Wert=" + str(j)
      if h.y_val < 0:
        if bout_data[B_Y0] == s7dec[0]:
          bout_data[B_Y0] = s7dec[10]
        else:
          bout_data[B_Y0] = s7dec[11]

      bout_data[13] += 1   # Dezimalpunkt
      bout_data[10] = 0    # Unbenutzt
  

      # X-Daten
      l=B_X0
      k=abs(h.x_val)+0.001
      for i in range(3,-3,-1):
        t = k
        j = int(k/pow(10,i))
        k = k - j * pow(10,i)
        #print 'Stelle=' + str(i) + " Wert=" + str(j)
        if j > 9:
          j=11
        bout_data[l] = s7dec[j]
        l += 1
      if h.x_val < 0:
        if bout_data[B_X0] == s7dec[0]:
          bout_data[B_X0] = s7dec[10]
        else:
          bout_data[B_X0] = s7dec[11]

      bout_data[19] += 1   # Dezimalpunkt

      # Checksumme Berechnen...
      bout_data[2] = bout_data[0] ^ bout_data[1] 
      for i in range(3,22):
        bout_data[2] ^= bout_data[i]

      # Schreiben
      ser.write(bout_data)
      r = readback(ser)
      if r[1] != 0x01:
        print 'hal_mydr: Write Data FAILED!'
        h.exit()

      # ende ein Schleifendurchlauf
      time.sleep(0.1)
  except KeyboardInterrupt: 
    pass

  # Ende. Minus schreiben...
  for i in range(4,22):
    bout_data[i] = s7dec[10]
  bout_data[4] = 0x00 # LEDs aus..

  # Checksumme Berechnen...
  bout_data[2] = bout_data[0] ^ bout_data[1] 
  for i in range(3,22):
    bout_data[2] ^= bout_data[i]

  # schreiben..
  ser.write(bout_data)
  r = readback(ser)
  if r[1] != 0x01:
    print 'hal_mydr: Write Data FAILED!'
    h.exit()

  print 'hal_mydro: regular exit'
  ser.close()
else: print 'hal_mydro: failed to open'