c sizeshims3.f - read RM from separate files
c sizeshims2.f - modified from sizeshims.f to incorporate realignement of data
c                Realignment Matrices added up front to metrology data of flanges
c
c sizeshims.f
c routine to size MCWF shims (thickness) based onsmetrology of flanges
c shims defined by points at four corners on flange surface
c measured flange point coordinates are at the center of ball (ie .75" off surface)
c Solving 2D problem of point in rectangle
c
c NOTE: Metrology data in ProE Default Csys with y up
c      file1 is the first coil - ie B for BC fitup
c      file2 is the second coil - ie C for BC fitup
c      shimfile contains the four corner data for each shim
c      ang is the joint location 0=AA, 20=AB, 40=BC, 60=CC
c      For AA fitup, first coil requires flipping.
c      For CC fitup, second coil requires flipping.
c
      implicit real*8(a-h,o-z)
      real*8, dimension(:,:,:), allocatable :: shims
      real*8, dimension(:,:), allocatable :: pts1, pts2
      real*8, dimension(:), allocatable :: tshim1,tshim2,tshim12
      integer, dimension(:), allocatable :: kount1, kount2 ! #points in shim
      real*8, dimension(3) :: v12, v23
      real*8, dimension(4,4) :: ay, a1_realign, a2_realign
      logical :: in
      character*80 file1, file2, shimfile, arg6, RM1, RM2
      character*5 lab
c
c get command line arguments
c
      ns=iargc()
      if(ns.ne.6)then
        write(*,*) "usage: sizeshims.x RM1 file1 RM2 file2 shimfile ang"
        stop
      endif
c
      id=getarg(1,RM1)
      id=getarg(2,file1)
      id=getarg(3,RM2)
      id=getarg(4,file2)
      id=getarg(5,shimfile)
      id=getarg(6,arg6)
      open(unit=17,file=RM1,status="old")
      open(unit=7,file=file1,status="old")
      open(unit=18,file=RM2,status="old")
      open(unit=8,file=file2,status="old")
      open(unit=9,file=shimfile,status="old")
      read(arg6,*) ang
c
c create rotation matrix in ProE Default Csys
c
      dtor=asin(1.d0)/90.d0
      ang_rad=-ang*dtor
      co=cos(ang_rad)
      si=sin(ang_rad)
      ay(1,:) = (/   co,  0.d0,    si,  0.d0 /)
      ay(2,:) = (/ 0.d0,  1.d0,  0.d0,  0.d0 /)
      ay(3,:) = (/  -si,  0.d0,    co,  0.d0 /)
      ay(4,:) = (/ 0.d0,  0.d0,  0.d0,  1.d0 /)
c
c read in shims and toroidal location
c
      read(9,*) nshims
      allocate(shims(4,4,nshims))
      allocate(tshim1(nshims), tshim2(nshims))
      allocate(tshim12(nshims), kount1(nshims), kount2(nshims))
c
      do ishims=1,nshims
        do j=1,4
          read(9,*) shims(1:3,j,ishims)
          shims(4,j,ishims)=1.d0
          shims(:,j,ishims)=matmul(ay,shims(:,j,ishims))
        enddo
c
c    check shims for positive definition
        v12=shims(:,2,ishims)-shims(:,1,ishims)
        v23=shims(:,3,ishims)-shims(:,2,ishims)
        vxv=v12(1)*v23(2)-v12(2)*v23(1)
        if(vxv.lt.0.d0)then ! reverse
          v12=shims(:,1,ishims)
          shims(:,1,ishims)=shims(:,4,ishims)
          shims(:,4,ishims)=v12
          v12=shims(:,2,ishims)
          shims(:,2,ishims)=shims(:,3,ishims)
          shims(:,3,ishims)=v12
        endif
      enddo
c
c read in points on first flange and realign
c
      read(17,*)
      read(17,*) ((a1_realign(i,j),j=1,4),i=1,4)
c
!      read(7,*) npts1
      call getsize(7,npts1)
      allocate(pts1(4,npts1))
      do ipts=1,npts1
        read(7,*) ii, lab, pts1(1:3,ipts)
      enddo
c
      pts1(4,:)=1.d0
      pts1=matmul(a1_realign,pts1)
        do i=1,npts1
          write(11,*) pts1(1:3,i)
        enddo
      pts1=matmul(ay,pts1)
      if(ang.eq.0.d0)then ! AA Joint - Flip first coil
          pts1(2:3,:) = - pts1(2:3,:)
      endif
c
c read in points on second flange and realign
c
      read(18,*)
      read(18,*) ((a2_realign(i,j),j=1,4),i=1,4)
c
!      read(8,*) npts2
      call getsize(8,npts2)
      allocate(pts2(4,npts2))
      do ipts=1,npts2
        read(8,*) ii, lab, pts2(1:3,ipts)
      enddo
      pts2(4,:)=1.d0
      pts2=matmul(a2_realign,pts2)
        do i=1,npts2
          write(12,*) pts2(1:3,i)
        enddo
      pts2=matmul(ay,pts2)
      if(ang.eq.60.d0)then ! CC Joint - Flip second coil
          pts2(2:3,:) = - pts2(2:3,:)
      endif
c
c Sort flange points into shims and average
c
      do ishims=1,nshims
        tshim1(ishims)=0.d0
        kount1(ishims)=0
        do ipts=1,npts1
          call ptinrect(pts1(:,ipts), shims(:,:,ishims), in)
          if(in)then
            tshim1(ishims)=tshim1(ishims)+pts1(3,ipts)
            kount1(ishims)=kount1(ishims)+1
          endif
        enddo
        if(kount1(ishims).ne.0)then
          tshim1(ishims)=tshim1(ishims)/kount1(ishims)
        endif
c
        tshim2(ishims)=0.d0
        kount2(ishims)=0
        do ipts=1,npts2
          call ptinrect(pts2(:,ipts), shims(:,:,ishims), in)
          if(in)then
            tshim2(ishims)=tshim2(ishims)+pts2(3,ipts)
            kount2(ishims)=kount2(ishims)+1
          endif
        enddo
        if(kount2(ishims).ne.0)then
          tshim2(ishims)=tshim2(ishims)/kount2(ishims)
        endif
c
        tshim12(ishims)=tshim1(ishims)-tshim2(ishims)+2.d0*0.75d0
        write(*,"(2i5,f10.3)") kount1(ishims),kount2(ishims),
     +     tshim12(ishims)
      enddo
       write(*,*) "sum(kount1)=",sum(kount1)
       write(*,*) "sum(kount2)=",sum(kount2)
c
      stop
      end
c***********************************************
c
      subroutine ptinrect(pts1, shims, in)
      real*8, dimension(4,4) :: shims
      real*8, dimension(3) :: pts1
      real*8, dimension(3) :: v1, v2
      logical :: in
c
      in=.true.
      do i=1,4
        i1=i+1
        if(i.eq.4)i1=1
        v1=pts1 - shims(1:3,i)
        v2=shims(1:3,i1) - shims(1:3,i)
        v12=v1(1)*v2(2)-v1(2)*v2(1)
        if(v12.ge.0.d0)in=.false.
      enddo
c
      return
      end
c***********************************************
      subroutine getsize(iunit,npts)
c routine to just count lines in file and rewind
      npts=0
      do
        read(iunit,*,end=99)
        npts=npts+1
      enddo
 99   rewind(iunit)
      return
      end