C--------------------------------------------------------------------
      SUBROUTINE TOFUNPACK(vecta,lung,me)

C                                  modified  D.Campana,  May. 06
C                                            D.Campana,  Dec. 04
C--------------------------------------------------------------------

      IMPLICIT NONE

C
C Normal variables definition
C
      integer lung
      integer*1 vecta(lung)
      integer*1 vectof(lung,16)
      integer*2 ibuf
      integer me
      integer check, crctof 
      integer ic0,sup,inf 
      integer i, ic, bit, bi,j ,iadd, iword,iw,idw,iup,ind
      integer start,ntdc,tdcfirst,tdccodeq,tdccodet
      integer tdcnum(12), boardnum(12)
      integer tdcadd(8),coldadd(8)
      integer tdcid(12),evcount(12)
      integer tdcmask(12),adc(4,12),tdc(4,12)
      integer rawadc(4,12),rawtdc(4,12),grayadc(4,12),graytdc(4,12)
      integer temp1(12),temp2(12)
      logical flag2
      integer ii,ik,adc_ch2,tdc_ch1,icorr
      integer dsphot,dspcold,code
C 
c      data start,ntdc /150,12/ ! to read data before Christmas 2004
      data start,ntdc /153,12/ ! to read data after Christmas 2004
      data tdcadd /1,0,3,2,5,4,7,6/
      data coldadd /6,7,4,5,2,3,0,1/

      COMMON / tofvar /tdcid,evcount,tdcmask,adc,tdc,temp1,temp2
      save / tofvar /

C         
c----+---1---------2---------3---------4---------5---------6---------7---------8
C     Begin ! 
C    'start' is  a pointer to the ToF data 
c----+---1---------2---------3---------4---------5---------6---------7---------8

C
      ic = start
C
c      print *,'++++++++++ Tof Unpack entro ++++++++++++++++'

      dspcold = 0 
      dsphot = 0 

      do j = 1,ntdc
      flag2=.true.
      ic0 = ic      ! first index for the CRC computation
      tdcid(j) = 0
      evcount(j) = 0
      tdcnum(j) = 0        !  the 4 MSBs  in TDCid
      boardnum(j) = 0      !  the 4 LSBs  in TDCid      
      do bit = 0, 7
         bi = ibits(vecta(ic),bit,1)
         if (bi.eq.1)then 
            tdcid(j) = ibset(tdcid(j),bit)
           if (bit.le.3)then
              boardnum(j) = ibset(boardnum(j),bit)
           else
              tdcnum(j) = ibset(tdcnum(j),bit-4)
            endif
         endif
         bi = ibits(vecta(ic+1),bit,1)
         if (bi.eq.1) evcount(j) = ibset(evcount(j),bit)
      enddo
c
      ic=ic+2

      if ((ic+1).gt.lung) then
c        print *,' 1 ME' 
	me=1
         return
      endif

c      print *,'++++++++++ Tof Unpack 2 ++++++++++++++++'
      tdcmask(j) = 0
      do bit = 0, 7
         bi = ibits(vecta(ic),bit,1)
         if (bi.eq.1) tdcmask(j) = ibset(tdcmask(j),bit+8)
         bi = ibits(vecta(ic+1),bit,1)
         if (bi.eq.1) tdcmask(j) = ibset(tdcmask(j),bit)
      enddo
c
      ic=ic+2
      if (ic.gt.lung) then
c        print *,' 2 ME' 
         me=1
         return
      endif
c
c      print *,'++++++++++ Tof Unpack 3 ++++++++++++++++'
c
c   if the first word RAWADC are equal to 0
c   the data storage is shifted by a word --> ic = ic+1
c   and TEMP2 is overwritten by the CRC --> flag2=.false.
c
       tdcfirst = 0  
       do bit = 0,7
         bi = ibits(vecta(ic),bit,1)
         if (bi.eq.1) tdcfirst = ibset(tdcfirst,bit)
       enddo       
       if (tdcfirst.eq.0) then
         ic=ic+1
         flag2=.false.
       endif
      if (ic.gt.lung) then
c        print *,' 3 ME' 
         me=1
         return
      endif
c
c  ----- se stiamo acquisendo con il DSP cold, bisogna tener conto
c  ----- che c'e' un' inversione nel cavo che va alla scheda 5
c  ----- quindi per TDCnum =1,2 e Boardnum=4 (va da 0 a 5)
c  ----- le parole che seguono sono invertite bit a bit 
c  -----        cioe' si fa il complemento a 2**8-1 =255
c
       if (dspcold.eq.0 .and. dsphot.eq.0) then
       if (boardnum(j).eq.4)then
       if((tdcnum(j).eq.1).or.(tdcnum(j).eq.2))then
         code = 0 
         do bit = 5,7
           bi = ibits(vecta(ic),bit,1)
           if (bi.eq.1) code = ibset(code,bit-5)
         enddo         
       if (code.eq.coldadd(1)) dspcold = 1
       if (code.eq.tdcadd(1)) dsphot = 1
       endif
       endif
       endif
c 
c      print *,'++++++++++ Tof Unpack 4 ++++++++++++++++'
c     

       do i=1,16
       do iword=0,17 
          vectof(ic+iword,i)=0
       enddo
       enddo
c
       do iword=0,17 ! le parole successive sono copiate in vectof

          if ((ic+iword).gt.lung) then
c            print *,' 4 ME' 
             me=1
             return
          endif

          vectof(ic+iword,1)=vecta(ic+iword)
       enddo
c
       if (dspcold.eq.1) then
       if (boardnum(j).eq.4)then
       if((tdcnum(j).eq.1).or.(tdcnum(j).eq.2))then
         do iword=0,17 
            if ((ic+iword).gt.lung) then
c        print *,' 5 ME' 
               me=1
               return
            endif
            vectof(ic+iword,1)= 255 - vecta(ic+iword)
         enddo
       endif
       endif
       endif

c      print *,'++++++++++ Tof Unpack 5 ++++++++++++++++'

c
c -----fine cura inversione cavo FE to DSP cold
c
c
       ind=1               !  all'inizio parto da vecta(ic)==vectof(ic,1)
       tdc_ch1=0
       adc_ch2=0
       icorr=0
c
        
       do i=1,4                 !  loop on TDC 4 channels
c        print *,'====================='
c        print *,'TDC =',J,'  CANALE =',I

C
C   -------------- inizio correzione per le coppie di zeri  ------------
C
c   if first 3 bits of the word RAWADC(TDC) are not equal to 
c                              tdcadd(iadd) 
c   the data storage has to be shifted by 2 bit (are spurious zeroes
c   introduced by the F.E.board)
c
c
c   Check only on the MSB (bit 13,14 e 15 of 2(+2) word RAWADC(TDC): 
c         vecta(ic) and vecta(ic+2))
c
       iadd = 2*(i-1)+1   ! = 1,3,5,7
       ii=i
C
C ----------------- Controllo sulla parola ADC
C
11     continue
      if (ind.lt.15)then
       tdccodeq = 0  
c      print *,'++++++++++ Tof Unpack 6 (11 continue) ++++++++++++'
C
       do bit = 5,7
         bi = ibits(vectof(ic,ind),bit,1)
         if (bi.eq.1) tdccodeq = ibset(tdccodeq,bit-5)
       enddo     
C  
       if (tdccodeq.ne.tdcadd(iadd)) then
c
c------------- controllo che la colpa dei 2 zeri non sia di TDC(ch1)
c 
         if ((iadd.eq.3).and.(tdc_ch1.eq.0))then
          if (tdccodeq.ne.0) then
c            print *,'2 zeri in ADC, ma la colpa forse e` del TDC'
            ic=ic-4
            iadd=1
            ii=1
            adc_ch2=99
            goto 12
	    endif
	 endif
c--------------- fine controllo 
c                    
c         PRINT *, '---------shift di 2 zeri sui dati ADC ! '
c         print *,'numero di tdc = ',j,', ind = ',ind
c         print *,'vectof(ic,ind),tdccodeq,tdcadd(iadd),iadd'
c         print *,vectof(ic,ind),tdccodeq,tdcadd(iadd),iadd
c
         iup = 17-4*(ii-1) 
         idw = 0
         do iw=idw,iup
           do bit = 0, 7
             if(bit.le.5)then
               bi = ibits(vectof(ic+iw,ind),bit,1)
               if (bi.eq.1) vectof(ic+iw,ind+1) = 
     +                      ibset(vectof(ic+iw,ind+1),bit+2)
             else
               bi = ibits(vectof(ic+1+iw,ind),bit,1)
               if (bi.eq.1) vectof(ic+iw,ind+1) = 
     +                      ibset(vectof(ic+iw,ind+1),bit-6)
             endif
           enddo ! loop sui bit
         enddo ! loop sulle parole
c
c----+---1---------2---------3---------4---------5---------6---------7---------8
c
           if(iadd.eq.3)adc_ch2=adc_ch2+1
           ind = (ind + 1)   !  aggiorno l' indice
           if (ind.lt.15)then
              go to 11           !  rifaccio il check
           else
c             print *,'exit ADC senza soluzione',ind
             continue
           endif
       endif

c ------- se e' l'ADC channel 2 bisogna ricontrollare il TDC precedente

       if ((iadd.eq.3).and.(adc_ch2.ge.2).and.(tdc_ch1.eq.0))then
c         print *,'ci sono!!ADC ch 2 torno al TDC!!!'
c         print *,'adc_ch2 = ',adc_ch2
c         print *,'ind = ',ind

          ind=ind-adc_ch2
c         print *,'ind - adc_ch2 = ',ind
            do ik=1,adc_ch2
            do iword=0,17 
               vectof(ic+iword,ind+ik)=0
            enddo
            enddo
         ic=ic-4
         iadd=1
         ii=1
         goto 12
       endif

       endif ! fine controllo su ind < 15

c      print *,'++++++++++ Tof Unpack 6,5 registro ADC ++++++++++++++'
c
c ---  registro RAWADC
c
         rawadc(i,j) = 0
         do bit = 0, 7
           bi = ibits(vectof(ic,ind),bit,1)
           if (bi.eq.1) rawadc(i,j) = ibset(rawadc(i,j),bit+8)
           bi = ibits(vectof(ic+1,ind),bit,1)
           if (bi.eq.1) rawadc(i,j) = ibset(rawadc(i,j),bit)
         enddo
C
C
C ----------------- Controllo sulla parola TDC
C     
12    continue
      if (ind.lt.16)then
      tdccodet = 0  
c      print *,'++++++++++ Tof Unpack 7 (12 continue) ++++++++++++++++'
       do bit = 5,7
         bi = ibits(vectof(ic+2,ind),bit,1)
         if (bi.eq.1) tdccodet = ibset(tdccodet,bit-5)
       enddo     
       if ((tdccodet.ne.tdcadd(iadd+1)).or.
     +     ((adc_ch2.ge.2).and.(iadd.eq.1)))then
c----+---1---------2---------3---------4---------5---------6---------7---------8
c
          if(adc_ch2.ge.2)then
             adc_ch2=0
             icorr=1
          endif
c          PRINT *, '---------shift di 2 zeri sui dati TDC  ! '
c          print *,'vectof(ic+2,ind),tdccodet,tdcadd(iadd+1),(iadd+1)'
c          print *,vectof(ic+2,ind),tdccodet,tdcadd(iadd+1),(iadd+1)
c 
          iup = 17-4*(ii-1)
          idw =2
          do iw=idw,iup
           do bit = 0, 7
             if(bit.le.5)then
               bi = ibits(vectof(ic+iw,ind),bit,1)
               if (bi.eq.1) vectof(ic+iw,ind+1) = 
     +                      ibset(vectof(ic+iw,ind+1),bit+2)
             else
               bi = ibits(vectof(ic+1+iw,ind),bit,1)
               if (bi.eq.1) vectof(ic+iw,ind+1) = 
     +                      ibset(vectof(ic+iw,ind+1),bit-6)
             endif
           enddo ! loop sui bit
          enddo ! loop sulle parole
c----+---1---------2---------3---------4---------5---------6---------7---------8
c 
            if(iadd.eq.1)tdc_ch1=tdc_ch1+1
            ind = (ind + 1)   !  aggiorno l' indice
            if (ind.lt.16) then
               go to 12         !  rifaccio il check
            else
c               print *,'exit TDC senza successo ',ind
               continue
            endif         
       endif

       endif ! fine controllo su ind < 16

c      print *,'++++++++++ Tof Unpack 8 registro TDC  +++++++++++++'
c
c --- registro RAWTDC
c 
        rawtdc(ii,j) = 0
        do bit = 0, 7
           bi = ibits(vectof(ic+2,ind),bit,1)
           if (bi.eq.1) rawtdc(ii,j) = ibset(rawtdc(ii,j),bit+8)
           bi = ibits(vectof(ic+3,ind),bit,1)
           if (bi.eq.1) rawtdc(ii,j) = ibset(rawtdc(ii,j),bit)
        enddo
c
c
c ------- se e' il TDC channel 1 e' il risultato dell'iterazione prodotta
c          dall' ADC channel 2 bisogna ripassare all' ADC successivo 

       if ((iadd.eq.1).and.(icorr.eq.1).and.(tdc_ch1.gt.0))then
         iadd=3
         ii=2
         ic=ic+4
c         print *,'sto tornando all ADC dopo aver corr. il TDC',ind
         goto 11
       endif
C
C   -------------- fine correzione per le coppie di zeri  ------------
C

        ic=ic+4
      enddo   !  fine loop sui 4 TDC channel

c      print *,'++++++ Tof Unpack 9 fine primo loop sui ch +++++++++'

c
c   adc e tdc data have to be translated from Gray code to binary (bit 0-11)
c    bit 12 is added after conversion  (control bit)
c    bit 13 is 1(0) for charge(time) information
c    bits 14-15 give the channel 1-4 on the board.
c
       do i=1,4                 !  loop on TDC 4 channels
c        print *,'=========== graytobin e registro =========='
c        print *,'TDC =',J,'  CANALE =',I
        grayadc(i,j) = 0
        graytdc(i,j) = 0

        grayadc(i,j)=ibits(rawadc(i,j),0,12)
        graytdc(i,j)=ibits(rawtdc(i,j),0,12)
c     
        call graytobin(grayadc(i,j),adc(i,j),12)
        call graytobin(graytdc(i,j),tdc(i,j),12)
c     
        bi = ibits(rawtdc(i,j),12,1)
        if (bi.eq.1) tdc(i,j) = ibset(tdc(i,j),12)
        bi = ibits(rawadc(i,j),12,1)
        if (bi.eq.1) adc(i,j) = ibset(adc(i,j),12)
c
c        PRINT *,'i, j, RAWadc(i,j) ,RAWtdc(i,j)'
c        PRINT *, i, j, rawadc(i,j) ,rawtdc(i,j)
c        PRINT *,'i, j, ADC(i,j), TDC(i,j)'
c        PRINT *, i, j, adc(i,j), tdc(i,j)
c     
      enddo

c----+---1---------2---------3---------4---------5---------6---------7---------8

c      print *,'+++++++ Tof Unpack 10 fine secondo loop sui ch +++++++'

c     
      temp1(j) = 0
      temp2(j) = 0
      do bit = 0, 7
         bi = ibits(vectof(ic,ind),bit,1)
         if (bi.eq.1) temp1(j) = ibset(temp1(j),bit)
      enddo
      ic=ic+1
c
      if (flag2.eqv..true.) then     
         do bit = 0, 7         
            bi = ibits(vectof(ic,ind),bit,1)
            if (bi.eq.1) temp2(j) = ibset(temp2(j),bit)
         enddo
         ic=ic+1 
      else
         temp2(j) = 99
      endif 
c      print *,'++++++++++ Tof Unpack 11 fine temperatura ++++++++'
c     
c    vecta(ic) is the CRC
c Check consistency of CRC.
c
ccc      if(vecta(ic).lt.0)vecta(ic)=vecta(ic)+256
      if (ic.gt.lung) then
c        print *,' 6 ME' 
         me=1
         return
      endif

      ibuf=0
      do bit = 0, 7         
         bi = ibits(vecta(ic),bit,1)
         if (bi.eq.1) ibuf = ibset(ibuf,bit)
      enddo
c
      check = 0
      inf = ic0
      sup = ic - 1
      do i = inf,sup
         if ((i.gt.lung).or.(i.lt.1)) then
c        print *,' 7 ME' 
            me=1
            return
         endif
         check=crctof(check,vecta(i))
      enddo
c      if (check.ne.vecta(ic)) then
c        print *,' 8 ME = 0' 
      me = 0
      if (check.ne.ibuf) then
c         print *,'crc wrong ',ibuf, check
c        print *,' 9 ME' 
         me = 1
      endif

c
c        print *,'---------> ic, j' ,ic,j
        ic=ic+1
        if (ic.gt.lung) then
c        print *,' 10 ME' 
           me=1
           return
        endif

      enddo       !  j = 1,ntdc

c      print *,'++++++++++ Tof Unpack escooo ! ++++++++++++++++'

   
      RETURN
      END
