 SUBROUTINE u_penalisation(calc)
!=========================== DOCUMENTATION ============================= 
!DOCDEBh*    NUM/u_penalisation *
!
!    NOM
! u_penalisation(calc)
!
!    DESCRIPTION  
!*
!  Penalisation par methode de projection de la vitesse dans le solide afin de correspondre a un champ de vitesse solide rigide.
!  Calcul des differents termes de l equation de la vitesse penalisee, calcul du centre de gravite, matrice 
!  d inertie puis deplacement des marqueurs solides et mise a jour de l indicatrice.
!
!  Penalisation using the method of projecting the velocity into the solid to correspond to a velocity field rigid solid.
!  Calculate the various terms in the equation of penalized velocity, calculate the centre of gravity, the inertia matrix,
!  then move the solid markers and update the indicator.
!*
! 
!    ENTREES / INPUT
! calc : objet calcul / calcul object
!    SORTIES / OUTPUT
! calc : objet calcul / calcul object
!
!    SOURCE
! Ce logiciel est regi par la licence [CeCILL-B]
! This software is governed by the [CeCILL-B] license
!=========================== DEBUT DES DECLARATIONS ==================== 
!.1-----  Implicit, Use
  use num_typ
  use num, only: raycasting
  IMPLICIT NONE
#ifdef _MPI
  include 'mpif.h'
#endif
!.2-----  Declaration
  type(calcul), intent(inout)          :: calc !! Objet calcul / Calcul object
  
  integer                              :: i=0,ierr=0,isol=0,ic1,ic2
  real(kind=kind(0.d0))                :: r1=0.d0,r2=0.d0,r3=0.d0,chi_ini,rv
  real(kind=kind(0.d0))                :: i11=0.d0,i22=0.d0,i33=0.d0,i12=0.d0,i13=0.d0,i23=0.d0
  real(kind=kind(0.d0))                :: det=0.d0
  real(kind=kind(0.d0)), dimension(3,nb_sol_max)  :: omega
  real(kind=kind(0.d0)), dimension(17,nb_sol_max) :: vl,vg  
  real(kind=kind(0.d0)), dimension(:),allocatable :: rf  
!=========================== DEBUT DU CODE EXECUTABLE ==================
! vecteur de travail local au domaine ou global somme sur les domaines pour chaque solide
! vl(i,isol) ou vg(i,isol) : 
! i= 1,2,3 :translation moyenne suivant x,y,z
! i= 4,5,6 :centre de gravite
! i= 7     :volume du solide
! i= 8     :volume * densite du solide
! i= 9-14  :matrice d inertie du solide j11,j22,j33,j12,j13,j23
! i= 15-17 : moment de densite*vitesse 
omega(:,:)=0.d0
vl(:,:)=0.d0
vg(:,:)=0.d0
  
!---- A la frontiere du solide rho_A ou rho_W ??
    allocate(rf(calc%msh%nb_cell))
    rf=0.5d0*(rho0w_phy+rho0a_phy)
    DO i=1,calc%msh%nb_face
      ic1=calc%msh%list_face(i)%ic1
      ic2=calc%msh%list_face(i)%ic2
      if(ic2.gt.0)then
        if(calc%msh%list_cell(ic1)%w%chi.gt.0.d0.and.calc%msh%list_cell(ic2)%w%chi.eq.0.d0)then
          if(calc%msh%list_cell(ic2)%w%vprim%v(1).gt.(rho0A_phy+rho0A_phy)*0.51d0)then
            rf(ic1)=rho0W_phy
          else
            rf(ic1)=rho0A_phy
          endif
        endif
        if(calc%msh%list_cell(ic2)%w%chi.gt.0.d0.and.calc%msh%list_cell(ic1)%w%chi.eq.0.d0)then
          if(calc%msh%list_cell(ic1)%w%vprim%v(1).gt.(rho0A_phy+rho0A_phy)*0.5d0)then
            rf(ic2)=rho0W_phy
          else
            rf(ic2)=rho0A_phy
          endif
        endif
      endif
    ENDDO

! Calcul des paramètres de penalisation

    DO i=1,calc%msh%nb_cell
      DO isol=1,calc%nb_sol ! Boucle sur les solides
        IF (calc%msh%list_cell(i)%idom .EQ. calc%msh%numdom .AND. int(calc%msh%list_cell(i)%w%chi) .EQ.calc%list_sol(isol)%id)THEN
        ! Translation moyenne suivant x,y,z      
          vl(1,isol)=vl(1,isol)+calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%w%vprim%v(2)* calc%msh%list_cell(i)%vol 
          vl(2,isol)=vl(2,isol)+calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%w%vprim%v(3)* calc%msh%list_cell(i)%vol 
          vl(3,isol)=vl(3,isol)+calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%w%vprim%v(4)* calc%msh%list_cell(i)%vol 
        ! Centre de gravité en x,y,z
          vl(4,isol)=vl(4,isol)+calc%msh%list_cell(i)%center%x*calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%vol 
          vl(5,isol)=vl(5,isol)+calc%msh%list_cell(i)%center%y*calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%vol 
          vl(6,isol)=vl(6,isol)+calc%msh%list_cell(i)%center%z*calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%vol 
      
          vl(7,isol)=vl(7,isol) + calc%msh%list_cell(i)%vol
          vl(8,isol)=vl(8,isol) + calc%msh%list_cell(i)%w%vprim%v(1) * calc%msh%list_cell(i)%vol
        END IF
      ENDDO
    END DO
 
!----Synchronisation avec les autres processus
    DO isol=1,calc%nb_sol 
#ifdef _MPI
      DO i=1,8
        CALL MPI_ALLREDUCE(vl(i,isol),vg(i,isol),1,MPI_DOUBLE_PRECISION, MPI_SUM,MPI_COMM_WORLD,ierr)
      END DO
#endif
    ! Position du centre de gravité
      calc%list_sol(isol)%xg%v(1) = vg(4,isol) / vg(8,isol) 
      calc%list_sol(isol)%xg%v(2) = vg(5,isol) / vg(8,isol) 
      calc%list_sol(isol)%xg%v(3) = vg(6,isol) / vg(8,isol) 
    ! Vitesse de translation moyenne
      vg(1,isol) = vg(1,isol) / vg(8,isol) 
      vg(2,isol) = vg(2,isol) / vg(8,isol) 
      vg(3,isol) = vg(3,isol) / vg(8,isol) 
    
     !if(calc%msh%numdom.eq.1)write(12,'(9e13.5)')tc,calc%list_sol(isol)%xg%v,vg(1,isol),vg(2,isol),vg(3,isol),vg(7,isol),vg(8,isol)
    ENDDO 
    
    
! Calcul de la matrice d inertie du solide j 
    DO i=1,calc%msh%nb_cell
      DO isol=1,calc%nb_sol 
        IF (calc%msh%list_cell(i)%idom .EQ. calc%msh%numdom .AND. int(calc%msh%list_cell(i)%w%chi) .EQ.calc%list_sol(isol)%id &
        .AND. calc%list_sol(isol)%imv .EQ. 0) THEN
          r1=calc%msh%list_cell(i)%center%x-calc%list_sol(isol)%xg%v(1)
          r2=calc%msh%list_cell(i)%center%y-calc%list_sol(isol)%xg%v(2)
          r3=calc%msh%list_cell(i)%center%z-calc%list_sol(isol)%xg%v(3)
          IF (abs(r3)<1.d-10) r3=0.d0
          
        ! Calcul de la matrice d inertie en G centre de gravite de l assemblage j
          rv=calc%msh%list_cell(i)%w%vprim%v(1)*calc%msh%list_cell(i)%vol
          vl( 9,isol) = vl( 9,isol) + rv * (r2**2 + r3**2)
          vl(10,isol) = vl(10,isol) + rv * (r1**2 + r3**2)
          vl(11,isol) = vl(11,isol) + rv * (r1**2 + r2**2)
          vl(12,isol) = vl(12,isol) - rv * r1 * r2
          vl(13,isol) = vl(13,isol) - rv * r1 * r3
          vl(14,isol) = vl(14,isol) - rv * r2 * r3
        
        !----Calcul de int(GM ^ rho u)
          vl(15,isol) = vl(15,isol) + rv * &
                (- calc%msh%list_cell(i)%w%vprim%v(3)* r3  + calc%msh%list_cell(i)%w%vprim%v(4)* r2 )
        
          vl(16,isol) = vl(16,isol) + rv * &
                (+ calc%msh%list_cell(i)%w%vprim%v(2)* r3 - calc%msh%list_cell(i)%w%vprim%v(4)* r1 )
        
          vl(17,isol) = vl(17,isol) + rv * &
                (- calc%msh%list_cell(i)%w%vprim%v(2)* r2 + calc%msh%list_cell(i)%w%vprim%v(3)* r1 )
        END IF
       ENDDO
    END DO
    
    DO isol=1,calc%nb_sol 
      IF (calc%list_sol(isol)%imv .EQ. 0)THEN
  !----Synchronisation avec les autres processus
#ifdef _MPI
        DO i=9,17
          CALL MPI_ALLREDUCE(vl(i,isol),vg(i,isol),1,MPI_DOUBLE_PRECISION, MPI_SUM,MPI_COMM_WORLD,ierr)
        END DO
#endif   
! Déterminant de J
        det = vg( 9,isol)*vg(10,isol)*vg(11,isol) + 2.d0*vg(12,isol)*vg(13,isol)*vg(14,isol) &
            - vg(10,isol)*vg(13,isol)*vg(13,isol) -      vg( 9,isol)*vg(14,isol)*vg(14,isol) &
            - vg(11,isol)*vg(12,isol)*vg(12,isol)
    
        if(det.eq.0.d0)then
        call print_err('U_PENALISATION', 'Null determinant!!')
            stop
        endif
! Co-matrice de J
        i11 = ( vg(10,isol)*vg(11,isol)-vg(14,isol)*vg(14,isol))/det
        i12 = (-vg(12,isol)*vg(11,isol)+vg(14,isol)*vg(13,isol))/det
        i13 = ( vg(12,isol)*vg(14,isol)-vg(10,isol)*vg(13,isol))/det
        i22 = ( vg( 9,isol)*vg(11,isol)-vg(13,isol)*vg(13,isol))/det
        i23 = (-vg( 9,isol)*vg(14,isol)+vg(12,isol)*vg(13,isol))/det
        i33 = ( vg( 9,isol)*vg(10,isol)-vg(12,isol)*vg(12,isol))/det
    
!----Vecteur vitesse instantanée de rotation
        omega(1,isol) = i11 * vg(15,isol) + i12 * vg(16,isol) + i13 * vg(17,isol) ! Vitesse angulaire autour de x
        omega(2,isol) = i12 * vg(15,isol) + i22 * vg(16,isol) + i23 * vg(17,isol) ! Vitesse angulaire autour de y
        omega(3,isol) = i13 * vg(15,isol) + i23 * vg(16,isol) + i33 * vg(17,isol) ! Vitesse angulaire autour de z
      ENDIF
      if(calc%list_sol(isol)%imv.eq.-1)then
        vg(1,isol)    = 0.d0 
        vg(2,isol)    = 0.d0 
        vg(3,isol)    = 0.d0 
        omega(1,isol) = 0.d0
        omega(2,isol) = 0.d0
        omega(3,isol) = 0.d0
      endif
      if(calc%list_sol(isol)%imv.gt.0)then
        call fct_mvsol(vg(1,isol),vg(2,isol),vg(3,isol),omega(1,isol),omega(2,isol),omega(3,isol),calc%list_sol(isol)%imv)
      endif  
 ENDDO




    
! Pénalisation en vitesse
    DO i=1,calc%msh%nb_cell
      DO isol=1,calc%nb_sol 
        IF (calc%msh%list_cell(i)%idom .EQ. calc%msh%numdom .AND. int(calc%msh%list_cell(i)%w%chi) .EQ.calc%list_sol(isol)%id) THEN   
          r1=calc%msh%list_cell(i)%center%x-calc%list_sol(isol)%xg%v(1)
          r2=calc%msh%list_cell(i)%center%y-calc%list_sol(isol)%xg%v(2)
          r3=calc%msh%list_cell(i)%center%z-calc%list_sol(isol)%xg%v(3)
        
          calc%msh%list_cell(i)%w%vprim%v(1)=  calc%list_sol(isol)%rho  
          calc%msh%list_cell(i)%w%vprim%v(2) = vg(1,isol) + omega(2,isol)*r3 - omega(3,isol)*r2 
          calc%msh%list_cell(i)%w%vprim%v(3) = vg(2,isol) + omega(3,isol)*r1 - omega(1,isol)*r3 
          calc%msh%list_cell(i)%w%vprim%v(4) = vg(3,isol) + omega(1,isol)*r2 - omega(2,isol)*r1 
          CALL prim2bal(calc%msh%list_cell(i)%w)
        END IF
      ENDDO
    END DO
    
    DO isol=1,calc%nb_sol 
    
! Nouvelle position des marqueurs
      DO i=1,calc%list_sol(isol)%nb_vert
        calc%list_sol(isol)%list_vertex(i)%x = calc%list_sol(isol)%list_vertex(i)%x + &
        (vg(1,isol) - omega(3,isol)*(calc%list_sol(isol)%list_vertex(i)%y-calc%list_sol(isol)%xg%v(2))) * calc%dt ! x
        calc%list_sol(isol)%list_vertex(i)%y = calc%list_sol(isol)%list_vertex(i)%y + &
        (vg(2,isol) + omega(3,isol)*(calc%list_sol(isol)%list_vertex(i)%x-calc%list_sol(isol)%xg%v(1))) * calc%dt ! y
        calc%list_sol(isol)%list_vertex(i)%z = calc%list_sol(isol)%list_vertex(i)%z + &
        (vg(3,isol) + omega(1,isol)*(calc%list_sol(isol)%list_vertex(i)%y-&
        calc%list_sol(isol)%xg%v(2))-omega(2,isol)*(calc%list_sol(isol)%list_vertex(i)%x-calc%list_sol(isol)%xg%v(1))) * calc%dt ! z
      END DO

! Update de la bounding box
      calc%list_sol(isol)%xmin_s = minval(calc%list_sol(isol)%list_vertex(:)%x)
      calc%list_sol(isol)%xmax_s = maxval(calc%list_sol(isol)%list_vertex(:)%x)
      calc%list_sol(isol)%ymin_s = minval(calc%list_sol(isol)%list_vertex(:)%y)
      calc%list_sol(isol)%ymax_s = maxval(calc%list_sol(isol)%list_vertex(:)%y)
      calc%list_sol(isol)%zmin_s = minval(calc%list_sol(isol)%list_vertex(:)%z)
      calc%list_sol(isol)%zmax_s = maxval(calc%list_sol(isol)%list_vertex(:)%z)
      calc%list_sol(isol)%xmin_s = calc%list_sol(isol)%xmin_s-0.02d0*dabs(calc%list_sol(isol)%xmin_s)
      calc%list_sol(isol)%xmax_s = calc%list_sol(isol)%xmax_s+0.02d0*dabs(calc%list_sol(isol)%xmax_s)
      calc%list_sol(isol)%ymin_s = calc%list_sol(isol)%ymin_s-0.02d0*dabs(calc%list_sol(isol)%ymin_s)
      calc%list_sol(isol)%ymax_s = calc%list_sol(isol)%ymax_s+0.02d0*dabs(calc%list_sol(isol)%ymax_s)
      calc%list_sol(isol)%zmin_s = calc%list_sol(isol)%zmin_s-0.02d0*dabs(calc%list_sol(isol)%zmin_s)
      calc%list_sol(isol)%zmax_s = calc%list_sol(isol)%zmax_s+0.02d0*dabs(calc%list_sol(isol)%zmax_s)

    ENDDO
      
! Calcul de la nouvelle distribution de la fonction indicatrice du solide
!calc%msh%list_cell(calc%list_sol(isol)%list_cells)%w%chi=0 
    calc%list_sol(:)%nb_cell=0
    !rf=rho0A_phy  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    DO i=1,calc%msh%nb_cell
      chi_ini=calc%msh%list_cell(i)%w%chi
      DO isol=1,calc%nb_sol 

        if(calc%msh%list_cell(i)%w%chi.eq.calc%list_sol(isol)%id*1.d0)calc%msh%list_cell(i)%w%chi=0.d0
        !IF (calc%msh%list_cell(i)%idom .EQ. calc%msh%numdom .AND. &!
        IF ( &
            calc%msh%list_cell(i)%center%x<calc%list_sol(isol)%xmax_s .AND. &
            calc%msh%list_cell(i)%center%x>calc%list_sol(isol)%xmin_s .AND. &
            calc%msh%list_cell(i)%center%y<calc%list_sol(isol)%ymax_s .AND. &
            calc%msh%list_cell(i)%center%y>calc%list_sol(isol)%ymin_s .AND. &
            calc%msh%list_cell(i)%center%z<calc%list_sol(isol)%zmax_s .AND. &
            calc%msh%list_cell(i)%center%z>calc%list_sol(isol)%zmin_s) THEN 
! Faire le raycasting que dans la bounding box (grosse optimisation)
              CALL raycasting(calc%list_sol(isol),calc%msh%list_cell(i)%center%x,&
              calc%msh%list_cell(i)%center%y,calc%msh%list_cell(i)%center%z, &
              calc%msh%list_cell(i)%w%chi)
! Compte le nombre de cellules dans le solide j
              IF (calc%msh%list_cell(i)%w%chi .EQ. calc%list_sol(isol)%id) calc%list_sol(isol)%nb_cell=calc%list_sol(isol)%nb_cell+1 
        END IF
      ENDDO
!---  Fresh cell
      !~if(chi_ini.ne.0.d0.and.calc%msh%list_cell(i)%w%chi.eq.0.d0)then
      !~  calc%msh%list_cell(i)%w%vprim%v(1)=rf(i)
      !~  call prim2bal(calc%msh%list_cell(i)%w)
      !~  !if(rf(i).eq.0.d0)then
      !~  !write(*,'(i5,5e13.5)')i,calc%msh%list_cell(i)%center%x,calc%msh%list_cell(i)%center%y,&
      !~  !calc%msh%list_cell(i)%w%vprim%v(1),calc%msh%list_cell(i)%w%vbal%v(5)
      !~     !stop
      !~     !endif
      !~        !calc%msh%list_cell(i)%w%vprim%v(4)=rf(i)
      !~        !if(i==520)call display(calc%msh%list_cell(i))
      !~        !if(i== 88)call display(calc%msh%list_cell(i))
      !~endif

    END DO

    deallocate(rf)
  
!===========================   FIN DE LA ROUTINE    ====================
END SUBROUTINE u_penalisation
!DOCFIN

