#!/usr/bin/python

#uncomment for debbug purposes
#import logging
#logging.basicConfig(level=logging.DEBUG) 

import sys
import time
from pymlab import config

class axis:
    def __init__(self, SPI_CS, Direction, StepsPerUnit):
        ' One axis of robot '
        self.CS = SPI_CS
        self.Dir = Direction
        self.SPU = StepsPerUnit
        self.Reset()

    def Reset(self):
        ' Reset Axis and set default parameters for H-bridge '
        spi.SPI_write_byte(self.CS, 0xC0)      # reset
        spi.SPI_write_byte(self.CS, 0x14)      # Stall Treshold setup
        spi.SPI_write_byte(self.CS, 0x7F)  
        spi.SPI_write_byte(self.CS, 0x14)      # Over Current Treshold setup 
        spi.SPI_write_byte(self.CS, 0x0F)  
        #spi.SPI_write_byte(self.CS, 0x15)      # Full Step speed 
        #spi.SPI_write_byte(self.CS, 0x00)
        #spi.SPI_write_byte(self.CS, 0x30) 
        #spi.SPI_write_byte(self.CS, 0x0A)      # KVAL_RUN
        #spi.SPI_write_byte(self.CS, 0x50)
      
    def MaxSpeed(self, speed):
        ' Setup of maximum speed '
        spi.SPI_write_byte(self.CS, 0x07)       # Max Speed setup 
        spi.SPI_write_byte(self.CS, 0x00)
        spi.SPI_write_byte(self.CS, speed)  

    def ReleaseSW(self):
        ' Go away from Limit Switch '
        while self.ReadStatusBit(2) == 1:           # is Limit Switch ON ?
            spi.SPI_write_byte(self.CS, 0x92 | (~self.Dir & 1))     # release SW 
            while self.IsBusy():
                pass
            self.MoveWait(10)           # move 10 units away
 
    def GoZero(self, speed):
        ' Go to Zero position '
        self.ReleaseSW()

        spi.SPI_write_byte(self.CS, 0x82 | (self.Dir & 1))       # Go to Zero
        spi.SPI_write_byte(self.CS, 0x00)
        spi.SPI_write_byte(self.CS, speed)  
        while self.IsBusy():
            pass
        time.sleep(0.3)
        self.ReleaseSW()

    def Move(self, units):
        ' Move some distance units from current position '
        steps = units * self.SPU  # translate units to steps 
        if steps > 0:                                          # look for direction
            spi.SPI_write_byte(self.CS, 0x40 | (~self.Dir & 1))       
        else:
            spi.SPI_write_byte(self.CS, 0x40 | (self.Dir & 1)) 
        steps = int(abs(steps))     
        spi.SPI_write_byte(self.CS, (steps >> 16) & 0xFF)
        spi.SPI_write_byte(self.CS, (steps >> 8) & 0xFF)
        spi.SPI_write_byte(self.CS, steps & 0xFF)

    def MoveWait(self, units):
        ' Move some distance units from current position and wait for execution '
        self.Move(units)
        while self.IsBusy():
            pass

    def Float(self):
        ' switch H-bridge to High impedance state '
        spi.SPI_write_byte(self.CS, 0xA0)

    def ReadStatusBit(self, bit):
        ' Report given status bit '
        spi.SPI_write_byte(self.CS, 0x39)   # Read from address 0x19 (STATUS)
        spi.SPI_write_byte(self.CS, 0x00)
        data0 = spi.SPI_read_byte()           # 1st byte
        spi.SPI_write_byte(self.CS, 0x00)
        data1 = spi.SPI_read_byte()           # 2nd byte
        #print hex(data0), hex(data1)
        if bit > 7:                                   # extract requested bit
            OutputBit = (data0 >> (bit - 8)) & 1
        else:
            OutputBit = (data1 >> bit) & 1        
        return OutputBit

    
    def IsBusy(self):
        """ Return True if tehre are motion """
        if self.ReadStatusBit(1) == 1:
            return False
        else:
            return True

# End Class axis --------------------------------------------------





cfg = config.Config(
    i2c = {
        "port": 1,
    },

    bus = 
    [
                {
                        "type": "i2chub",
                        "address": 0x70,
                        "children": 
            [
                { "name":"spi", "type":"i2cspi", "channel":7}
            ],
                },
    ],
)


cfg.initialize()

print "Irradiation unit. \r\n"

spi = cfg.get_device("spi")

spi.route()


try:

    while True:
        print "SPI configuration.."
        spi.SPI_config(spi.I2CSPI_MSB_FIRST| spi.I2CSPI_MODE_CLK_IDLE_HIGH_DATA_EDGE_TRAILING| spi.I2CSPI_CLK_461kHz)

        print "Robot inicialization"
        X = axis(spi.I2CSPI_SS1, 0, 641)
        Y = axis(spi.I2CSPI_SS0, 1, 642)
        Z = axis(spi.I2CSPI_SS2, 1, 32256)

        X.MaxSpeed(35) # max. 50
        Y.MaxSpeed(35) # max. 50
        Z.MaxSpeed(20) # max. 30
        
        Z.GoZero(100)
        Y.GoZero(20)
        X.GoZero(20)

        time.sleep(1)

        X.Move(16)
        Y.Move(150)
        Z.MoveWait(39)
        time.sleep(1)
        Z.MoveWait(-5)

                    
        print "Robot is running"

        xcorner = 72
        xsteps = 9
        ysteps = 6 # *2 + 1 line
        yy = 0
        space = 4
        grid = 8
        delay = 1 #50

        X.MoveWait(xcorner)

        for y in range(ysteps):
            for x in range(xsteps):
                print x, yy
                Z.MoveWait(space)
                time.sleep(delay)
                Z.MoveWait(-space)
                if x < (xsteps - 1):
                    X.MoveWait(grid)
            Y.MoveWait(-grid)
            yy += 1
            for x in range(xsteps):
                print x, yy
                Z.MoveWait(space)
                time.sleep(delay)
                Z.MoveWait(-space)
                if x < (xsteps - 1):
                    X.MoveWait(-grid)
            Y.MoveWait(-grid)
            yy += 1

        for x in range(xsteps):
            print x, yy
            Z.MoveWait(space)
            time.sleep(delay)
            Z.MoveWait(-space)
            if x < (xsteps - 1):
                X.MoveWait(grid)
        Y.MoveWait(-20)

        X.MoveWait(-(xcorner+(xsteps-1)*grid))
        #Z.MoveWait(-10)
        Y.MoveWait(ysteps*grid*2+20)
        X.Float()
        Y.Float()
        Z.Float()
                   

        while True:
            print X.ReadStatusBit(2), "end"
            time.sleep(1)

finally:
    print "stop"