$regfile = "m328pdef.dat"
$crystal = 16000000
$baud = 9600
$hwstack = 40
$swstack = 40
$framesize = 40

$lib "I2C_TWI.LBX"

Config I2cdelay = 1                                         '10
Config Twi = 100000
Config Scl = Portc.5
Config Sda = Portc.4


'------------------------------------------
Dim M As Word
Dim Rejestr(27) As Byte

Dim Ch0_byte1 , Ch0_byte2 As Byte
Dim Ch0_word As Word
Dim Ch1_byte1 , Ch1_byte2 As Byte
Dim Ch1_word As Word

Dim Proxl , Proxh As Byte
Dim prox_word as Word

Dim Addr_w As Byte
Dim Addr_r As Byte
Dim Apds_base , Apds_adres , Wynik As Byte
Dim Tmp As Byte


Declare Sub Writeregdefault(byval Nr_rejestru As Byte , Byval Zmienna As Byte)
Declare Function Readreg(byval Nr_rejestru As Byte) As Byte


Addr_w = &H72
Addr_r = &H73

Rejestr(1) = &H00
Rejestr(2) = &H01                                           'ATIME
Rejestr(3) = &H02                                           'PTIME
Rejestr(4) = &H03                                           'WTIME
Rejestr(5) = &H04                                           'AILTL
Rejestr(6) = &H05                                           'AILTH
Rejestr(7) = &H06                                           'AIHTL
Rejestr(8) = &H07                                           'AIHTH
Rejestr(9) = &H08                                           'PILTL
Rejestr(10) = &H09                                          'PILTH
Rejestr(11) = &H0A                                          'PIHTL
Rejestr(12) = &H0B                                          'PIHTH
Rejestr(13) = &H0C                                          'PERS
Rejestr(14) = &H0D                                          'CONFIG
Rejestr(15) = &H0E                                          'PPULSE
Rejestr(16) = &H0F                                          'CONTROL

Rejestr(17) = &H10
Rejestr(18) = &H11

Rejestr(19) = &H12                                          'ID
Rejestr(20) = &H13                                          'STATUS
Rejestr(21) = &H14                                          'Ch0DATAL
Rejestr(22) = &H15                                          'Ch0DATAH
Rejestr(23) = &H16                                          'Ch1DATAL
Rejestr(24) = &H17                                          'Ch1DATAH
Rejestr(25) = &H18                                          'PDATAL
Rejestr(26) = &H19                                          'PDATAH
Rejestr(27) = &H1E                                          'POFFSET

Apds_base = &HA0

I2cinit


 '*****************  Write to registers ******************


 Call WriteRegDefault(1 , &H05)                             '05 - proximity, 03-light sensor  -> according to z datasheetem page 19
 Call WriteRegDefault(2 , &HED)
 Call WriteRegDefault(4 , &HFF)
 Call WriteRegDefault(15 , &H08)
 Call WriteRegDefault(27 , &H00)
 Call WriteRegDefault(14 , &H00)
 Call WriteRegDefault(9 , &H00)
 Call WriteRegDefault(10 , &H00)
 Call WriteRegDefault(11 , &H32)
 Call WriteRegDefault(12 , &H00)
 Call WriteRegDefault(13 , &H22)



    'PDRIVE, PDIODE, PGAIN, AGAIN -> CONTROL
    I2cstart
     I2cwbyte Addr_w
     Tmp = Rejestr(16)
     I2cwbyte Apds_base Or Tmp                              'write to CONTROL register
'     wynik = &B00000000 or &B00100000 or  &B00001100 =&H2C
     I2cwbyte &H2C
    I2cstop


 '*****************  Read from registers  ******************

 For M = 1 To 27
  I2cstart
    I2cwbyte Addr_w
    Tmp = Rejestr(m)
    I2cwbyte Apds_base Or Tmp
  I2cstart
    I2cwbyte Addr_r
    I2crbyte Apds_adres , Nack
  I2cstop
  Print Hex(rejestr(m)) ; "  " ; Hex(apds_adres)
 Next M

 Waitms 100

 Do

 '********** PROXIMITY SENSOR '********** Enable=&H05
 ProxH = ReadReg(26)
  Waitms 20
 ProxL = ReadReg(25)
  Waitms 20
 prox_word = Makeint(proxL , proxH)
 Print "proximity: " ; prox_word

   '(
   ''********** LIGHT SENSOR '********** Enable=&H03
  'CH0
  Ch0_byte2 = ReadReg(22)
  Waitms 20
  Ch0_byte1 = ReadReg(21)

  Waitms 20
  'CH1
  Ch1_byte2 = ReadReg(24)
  Waitms 20
  Ch1_byte1 = ReadReg(23)

  Ch0_word = Makeint(ch0_byte1 , Ch0_byte2)
  Ch1_word = Makeint(ch1_byte1 , Ch1_byte2)

  Print "ch0: " ; Ch0_word ; "  ch1: " ; Ch1_word
')

  Waitms 100

 Loop

End


Sub WriteRegDefault(byval Nr_rejestru As Byte , Byval Zmienna As Byte)
    I2cstart
     I2cwbyte Addr_w
     Tmp = Rejestr(nr_rejestru)
     I2cwbyte Apds_base Or Tmp
     I2cwbyte Zmienna
    I2cstop
End Sub


Function ReadReg(byval Nr_rejestru As Byte) As Byte
   I2cstart
   I2cwbyte Addr_w
   Tmp = Rejestr(nr_rejestru)
   I2cwbyte Apds_base Or Tmp
  I2cstart
   I2cwbyte Addr_r
   I2crbyte Readreg , Nack
  I2cstop
 End Function