import sys
import time
import numpy as np
from smbus import SMBus 
import RPi.GPIO as GPIO
import ctypes 

   
bus = SMBus(1)

OPT3101 = 0x58 #OPT3101 i2c adress

reg80Default = 0xe41e #

frameDelayTimeMs = 0

Adapative = 255  #swtich btw low and high range

channelAuto = 255

channelUsed = 0

i = 0   #IPHASE_XTALK
q = 0	#QPHASE_XTALK

distance = 0 #distance in cm

phase = 0 #phase out 



GPIO.setmode(GPIO.BCM)
GPIO.setup(27, GPIO.IN, pull_up_down=GPIO.PUD_UP)  #reset pin

#reset and then waits for the INIT_LOAD bit to be 1
def resetAndwait():
    OPT3101_writeReg(0x00,0x00)
    time.sleep(0.5)
    while (not (OPT3101_readReg(0x03) & 1 << 8)):
	time.sleep(0.001)
	
    

#Write the given value to the an OPT3101 reg	    
def OPT3101_writeReg(adress, data):
	
    bus.write_byte_data(OPT3101, adress, np.int8(data >> 0))

    bus.write_byte_data(OPT3101, adress, np.int8(data >> 8))

    bus.write_byte_data(OPT3101, adress, np.int8(data >> 16))


#Read an OPT3101 reg
def OPT3101_readReg(adress):
    
    regData = bus.read_byte_data(OPT3101, adress)
    regData |= np.uint16(bus.read_byte_data(OPT3101, adress) << 8)
    regData |= np.uint32(bus.read_byte_data(OPT3101, adress) << 16)

    return regData
    

#init and configures the sensor 
def OPT3101_init():
    resetAndwait()
    
    configureDefult()


#Configure the sensor with default setting
def configureDefult():
 
	
    OPT3101_writeReg(0x89, 7000) #TG_OVL_WINDOW_START = 7000
    
    OPT3101_writeReg(0x6e, 0x0a0000) #EN_TEMP_CONV = 1
    

    
    OPT3101_writeReg(0x50, 0x200101) #CLIP_MODE_FC = 1
									 #CLIP_MODE_TEMP = 0
									 #CLIP_MODE_OFFSET = 0

    reg2e = OPT3101_readReg(0x2e) #IG_READ_DATA_SEL

    reg2e = (reg2e & ~((np.uint32)(7 >> 9))) | (2 << 9)

    OPT3101_writeReg(0x2e, reg2e)
    
    set_monoshotMode()

    set_frameTiming(256 )
    
#configure the sensor to use a specified channel "channelauto"
def setChannel(ch):
    
    global channelAuto
    
    
    if ch > 2:	
	    ch = channelAuto

    try:
	    reg2a = OPT3101_readReg(0x2a)
    except IOError, err:
	    print err
	    
    if ch == channelAuto:
	    reg2a |= (1<<0)
	    
    else:
	    reg2a &= ~(np.int32(1 << 0))
	    reg2a = reg2a & ~(np.int32(3 << 1)) | (ch & 3) << 1
    
    OPT3101_writeReg(0x2a, reg2a)

#configure the sensor to use the channel that com numerically after the last	
def nextChannel():
    
    global channelUsed

    nextchannel = channelUsed + 1
    
    if nextchannel > 2:
	    nextchannel = 0
    setChannel(nextChannel)
	
#set brightness 
#0 for nerly
#1 for wide range distance
#Adapative automaticlly uses low or high 	
def setBrightness(br):
	
    reg2a = OPT3101_readReg(0x2a)

    if br == Adapative:
	    reg2a |= np.uint16(1 << 15)
    else:
	    reg2a = reg2a & ~(np.uint32(0x18000)) | (np.uint32(br & 1)) << 16

    OPT3101_writeReg(0x2a, reg2a)
	
	
def set_continueMode():
	
    OPT3101_writeReg(0x27, 0x26ac04)


#monoshot mode 
def set_monoshotMode():
	
    OPT3101_writeReg(0x27, 0x26ac07)

    OPT3101_writeReg(0x76, 0x000121)

    OPT3101_writeReg(0x26, np.uint32(95 << 10 | 0xF))
	
#set frame timing, to average the spacified number  
def set_frameTiming(subFrame):
	
    global timeconst 
    global frameTimsMS
    global frameDelayTimeMs

    if(subFrame < 1 or subFrame > 4096 or subFrame & (subFrame - 1)):
	subFrame = 4096

    timeconst = 0

    while((subFrame << timeconst) < 1024):
	timeconst += 1

    reg2e = OPT3101_readReg(0x2e)


    reg2e = reg2e & ~np.uint32(0xF00000) | np.uint32(timeconst) << 20

    OPT3101_writeReg(0x2e, reg2e)

    #Set NUM_SUB_FRAMES and NUM_AVG_SUB_FRAMES.
    OPT3101_writeReg(0x9f, (subFrame - 1) | np.uint32(subFrame - 1) << 12)

    #Set TG_SEQ_INT_MASK_START and TG_SEQ_INT_MASK_END according to what
    #the OPT3101 datasheet says, but it's probably not needed.

    OPT3101_writeReg(0x97, (subFrame - 1) | (np.uint32)(subFrame - 1) << 12)

    #Assuming that SUB_VD_CLK_CNT has not been changed, each sub-frame is
    #0.25 ms.  The +3 is to make sure we round up.

    frameTimeMs = (subFrame + 3) / 4

    #Add a ~6% margin in case the OPT3101 clock is running faster.
    frameDelayTimeMs = frameTimeMs + (frameTimeMs + 15) / 16



#set TG_EN bit to 1
def enable_timing():
    global 	timingGenerator 
    
    OPT3101_writeReg(0x80, reg80Default | 1)



#Read the sensor's output registers and update the data 
def readOutput():
	
    global i
    global q
    global distance 
    global phase
    global amplitud
    global channelUsed
    
    try:
	reg08 = OPT3101_readReg(0x08)
	reg09 = OPT3101_readReg(0x09)
	reg0a = OPT3101_readReg(0x0a)
    except IOError, err:
		print err
    #channelUsed = reg08 >> 18 & 3
    
    
    #if channelUsed > 2:
		
     #   channelUsed = 2
        
        
    i = OPT3101_readReg(0x3b)
    
    if i > 0x7fffff:
	i -= 0x1000000
    q = OPT3101_readReg(0x3c)
    
    if q > 0x7fffff:
	q -= 0x1000000
	
    phase = np.int32(reg08 & 0xFFFF)

    amplitud = reg09 & 0xFFFF
    print("amp", amplitud)
    temp = reg0a >> 12 & 0xFFF
    
    print("temp", temp)
    distance = np.uint32(phase) * 14990  >> 16 #calc distance 

    if distance < 0:
      return -1
    print ('Dist in cm :  ', distance / 10)
    velocity = set_velocity(distance)
    
    return velocity



def set_velocity(distance):
    if 5 < distance < 10:
	return 30
    elif 10 <= distance < 30:
	return 100
    else: 
	return 0

	
#To read the results from sample
def sample():
    
    enable_timing()

    readOutput()


#some initializes before using 
def setUP():

    global Adapative
    global channelAuto
    
    OPT3101_init()
    

    setChannel(1)	
  
    setBrightness(Adapative)


def main():
    
    global channelUsed
    
    setUP()
    
    while True:
        sample()
        time.sleep(0.5)


if __name__ == '__main__':
	
	main()