"""
Demonstrates SPI communication.

You can short MOSI to MISO for testing.

MOSI    FIO2
MISO    FIO3
CLK     FIO0
CS      FIO1

If you short MISO to MOSI, then you will read back the same bytes that you
write.  If you short MISO to GND, then you will read back zeros.  If you
short MISO to VS or leave it unconnected, you will read back 255s.

"""

from labjack import ljm
import time

# Open first found LabJack
handle = ljm.open(ljm.constants.dtANY, ljm.constants.ctANY, "ANY")

info = ljm.getHandleInfo(handle)
print("Opened a LabJack with Device type: %i, Connection type: %i,\n" \
    "Serial number: %i, IP address: %s, Port: %i,\nMax bytes per MB: %i" % \
    (info[0], info[1], info[2], ljm.numberToIP(info[3]), info[4], info[5]))

# Setup labjack / assign pins
# sparkfun breakout
# ljm.eWriteName(handle, "SPI_CS_DIONUM", 7)
# ljm.eWriteName(handle, "SPI_CLK_DIONUM", 4)
# ljm.eWriteName(handle, "SPI_MISO_DIONUM", 6)
# ljm.eWriteName(handle, "SPI_MOSI_DIONUM", 5)

# AD breakout
ljm.eWriteName(handle, "SPI_CLK_DIONUM", 0)
ljm.eWriteName(handle, "SPI_MOSI_DIONUM", 1)
ljm.eWriteName(handle, "SPI_MISO_DIONUM", 2)
ljm.eWriteName(handle, "SPI_CS_DIONUM", 3)




# Selecting Mode CPHA=1 (bit 1), CPOL=1 (bit 2)
ljm.eWriteName(handle, "SPI_MODE", 0)

# Speed Throttle: Max. Speed (~ 1 MHz) = 0
ljm.eWriteName(handle, "SPI_SPEED_THROTTLE", 0)
# ljm.eWriteName(handle, "SPI_SPEED_THROTTLE", 61100)
# ljm.eWriteName(handle, "SPI_SPEED_THROTTLE", 23900)

# accelerometer registers
DEVID_AD = 0x00
DEVID_MST = 0x01
PARTID = 0x02
REVID = 0x03
STATUS = 0x0B
SOFT_RESET = 0x1F
FILTER_CTL = 0x2C
POWER_CTL = 0x2D
SELF_TEST = 0x2E
THRESH_ACT_L = 0x20
THRESH_ACT_H = 0x21
TIME_ACT = 0x22
THRESH_INACT_L = 0x23
THRESH_INACT_H = 0x24
TIME_INACT_L = 0x25
TIME_INACT_H = 0x26
ZDATA = 0x0A

# Read back and display the SPI settings
def settings():
    aNames = ["SPI_CS_DIONUM", "SPI_CLK_DIONUM", "SPI_MISO_DIONUM",
              "SPI_MOSI_DIONUM", "SPI_MODE", "SPI_SPEED_THROTTLE",
              "SPI_OPTIONS"]
    aValues = [0]*len(aNames)
    numFrames = len(aNames)
    aValues = ljm.eReadNames(handle, numFrames, aNames)

    print("SPI Configuration:")
    for i in range(numFrames):
        print("  %s = %0.0f" % (aNames[i],  aValues[i]))

def write_reg(address, cmd):
    ljm.eWriteName(handle, "SPI_NUM_BYTES", 3)

    byte_1 = 0x0A
    byte_2 = address
    byte_3 = cmd

    aNames = ["SPI_DATA_TX"]
    aWrites = [ljm.constants.WRITE]
    aNumValues = [3]
    data = [byte_1, byte_2, byte_3]
    err_address = 0
    data = ljm.eNames(handle, 1, aNames, aWrites, aNumValues, data)
    ljm.eWriteName(handle, "SPI_GO", 1)

    return None

def read_reg(address):
    ljm.eWriteName(handle, "SPI_NUM_BYTES", 3)

    byte_1 = 0x0B
    byte_2 = address
    byte_3 = 0x0

    # concurrent write/read (doesnt work?)
    # aNames = ["SPI_DATA_TX", "SPI_DATA_RX"]
    # aWrites = [ljm.constants.WRITE, ljm.constants.READ]
    # aNumValues = [3,3]
    # data = [byte_1, byte_2, byte_3, byte_3, byte_3, byte_3]
    # data = ljm.eNames(handle, 2, aNames, aWrites, aNumValues, data)
    # ljm.eWriteName(handle, "SPI_GO", 1)

    ljm.eWriteNameArray(handle, "SPI_DATA_TX", 3, [byte_1, byte_2, byte_3])
    ljm.eWriteName(handle, "SPI_GO", 1)
    data = ljm.eReadNameArray(handle, "SPI_DATA_RX", 3)
    ljm.eWriteName(handle, "SPI_GO", 1)

    return bin(int(data[-1]))[2:].zfill(8)

def read_reg_LH(address_L, address_H):
    ljm.eWriteName(handle, "SPI_NUM_BYTES", 3)
    # ljm.eWriteName(handle, "SPI_OPTIONS", int('00'.zfill(8), 2))

    byte_1 = 0x0B
    byte_2 = address_L
    byte_3 = 0x0

    ljm.eWriteNameArray(handle, "SPI_DATA_TX", 3, [byte_1, byte_2, byte_3])
    ljm.eWriteName(handle, "SPI_GO", 1)
    data_L = ljm.eReadNameArray(handle, "SPI_DATA_RX", 3)
    ljm.eWriteName(handle, "SPI_GO", 1)

    data_L = bin(int(data_L[-1]))[2:].zfill(8)

    byte_1 = 0x0B
    byte_2 = address_H
    byte_3 = 0x0

    ljm.eWriteNameArray(handle, "SPI_DATA_TX", 3, [byte_1, byte_2, byte_3])
    ljm.eWriteName(handle, "SPI_GO", 1)
    data_H = ljm.eReadNameArray(handle, "SPI_DATA_RX", 3)
    ljm.eWriteName(handle, "SPI_GO", 1)

    data_H = bin(int(data_H[-1]))[2:].zfill(8)

    data = data_H + data_L

    data = twos_complement(int(data,2),16)

    return data

def twos_comp(val, bits):
    ''' Returns two's complement of value given a number of bits
    '''
    if ((val & (1 << (bits - 1))) != 0):
        val = val - (1 << bits)
    return val

def twos_complement(input_value, num_bits):
	'''Calculates a two's complement integer from the given input value's bits'''
	mask = 2**(num_bits - 1)
	return -(input_value & mask) + (input_value & ~mask)


# soft reset
write_reg(SOFT_RESET, 0x52)
print('soft reset')
time.sleep(0.01)

# status
stat = read_reg(0x0B)
print('status: {}'.format(stat))

# general device settings
odr = '011'
ext_samp = '0'
half_bw = '0'
res = '0'
rng = '00'
write_reg(FILTER_CTL, int(rng + res + half_bw + ext_samp + odr,2))

# status
stat = read_reg(0x0B)
print('status: {}'.format(stat))

# part ID
pid = read_reg(0x02)
print('Part ID: {}'.format(pid))

# device ID
devid = read_reg(DEVID_AD)
print('devID: {}'.format(devid))

# rev ID
rid = read_reg(0x03)
print('Rev ID: {}'.format(rid))

# begin measure
cmd = int('10'.zfill(8),2)
write_reg(POWER_CTL, cmd)
time.sleep(0.02)

# read status
stat = read_reg(STATUS)
print('status: {}'.format(stat))

# read filter settings
filt = read_reg(FILTER_CTL)
print('filter settings: {}'.format(filt))

# read power settings
pwr = read_reg(POWER_CTL)
print('power control settings: {}'.format(pwr))

for i in range(20):
    #read 12 bit Z
    x = read_reg_LH(0x0E, 0x0F)
    y = read_reg_LH(0x10, 0x11)
    z = read_reg_LH(0x12, 0x13)
    print('x: {0:0d}, y: {1:0d}, z: {2:0d}'.format(x, y, z))

#Close handle
ljm.close(handle)