!     **********************************************************************
!     *                                                                    *
!     *              PROGRAM GEOMETRY_AND_MOTION_PRE_PROCESSOR             *
!     *                                                                    *
!     *   CALCULATES THE GEOMETRIC FEATURES OF A WING, THE POSITION AND    *
!     *   VELOCITY OF EVERY CONTROL POINT FOR EVERY TIME STEP AS WELL AS   *
!     *   A UNIT BASE ON A GIVEN PATH AND DIRECTION                        *
!     *                                                                    *
!     *              CONSTANTINE ILIOPOULOS (NM11038)                      *
!     *                                                                    *
!     **********************************************************************

      MODULE NRTYPE
         INTEGER, PARAMETER :: I4B = SELECTED_INT_KIND(9) !Four bytes INTEGER(I4B) number -> from~ -10^9 to~ 10^9 (nine digits)
         INTEGER, PARAMETER :: I2B = SELECTED_INT_KIND(4) !Two  bytes INTEGER(I4B) number -> from~ -10^4 to~ 10^4 (four digits)
         INTEGER, PARAMETER :: I1B = SELECTED_INT_KIND(2) !One  byte  INTEGER(I4B) number -> from~ -10^2 to~ 10^2 (two  digits)
         INTEGER, PARAMETER :: SP  = KIND(1.0)            !Single precision real number
         INTEGER, PARAMETER :: DP  = KIND(1.0d0)          !REAL(DP) real number
         INTEGER, PARAMETER :: SPC = KIND((1.0,1.0))      !Single precision complex number
         INTEGER, PARAMETER :: DPC = KIND((1.0d0,1.0d0))  !REAL(DP) complex number
         INTEGER, PARAMETER :: LGT = KIND(.TRUE.)         !Memory needed for logical variable
         REAL(SP), PARAMETER :: PI     = 3.141592653589793238462643383279502884197_SP
         REAL(SP), PARAMETER :: PIO2   = 1.57079632679489661923132169163975144209858_SP
         REAL(SP), PARAMETER :: TWOPI  = 6.283185307179586476925286766559005768394_SP
         REAL(SP), PARAMETER :: SQRT2  = 1.41421356237309504880168872420969807856967_SP
         REAL(SP), PARAMETER :: EULER  = 0.5772156649015328606065120900824024310422_SP
         REAL(DP), PARAMETER :: PI_D   = 3.141592653589793238462643383279502884197_DP
         REAL(DP), PARAMETER :: PIO2_D = 1.57079632679489661923132169163975144209858_DP
         REAL(DP), PARAMETER :: TWOPI_D= 6.283185307179586476925286766559005768394_DP
      END MODULE NRTYPE


     !-------------------------------------------------------------------
      MODULE GEOMETRY_AND_MOTION_PRE_PROCESSOR

      USE NRTYPE
      IMPLICIT NONE

      INTEGER(I4B) :: I,J,L  ,NR,NCH,NBL ,MOMENTS 
      PARAMETER(NR=13,NCH=17,NBL=1,MOMENTS=200) 
   
      REAL(DP) :: T(MOMENTS) ,DT ,SPAN ,TH    
      REAL(DP),DIMENSION(3)     :: A,UNIT1,UNIT2,UNIT3,PIVOT,ORIGIN
      REAL(DP),DIMENSION(NR)    :: U1 ,XS,DXS ,SKEW,DSKEW  ,FI,DFI   ,CHORD,DCHORD
      REAL(DP),DIMENSION(NR,NCH):: U2  ,DEC_DU1,DEC_DU2  ,DET_DU1,DET_DU2
      REAL(DP),DIMENSION(NR)    :: ETMAX ,ECMAX 
      REAL(DP),DIMENSION(NCH)   :: CH,ETCH ,ECCH  
      REAL(DP),DIMENSION(NR,NCH,NBL,3):: FACE ,BACK,VFACE  ,VBACK 
     !Input data for interpolation
      INTEGER(I4B) :: NR1,NCH1
      REAL(DP),ALLOCATABLE,DIMENSION(:) :: R1,CHORD1,XS1,SKEW1,FI1,ETMAX1,ECMAX1
      REAL(DP),ALLOCATABLE,DIMENSION(:) :: CH1,ETCH1,ECCH1
      
      CONTAINS 
      
      SUBROUTINE MAIN

      OPEN (UNIT=3 ,FILE='INPUT_FOR_SOLVER.TXT')
     !OPEN (UNIT=4 ,FILE='WING_GRAPH.TXT')
     !OPEN (UNIT=5 ,FILE='MY_OUTPUT_FOR_GRAPH.TXT')
      OPEN (UNIT=6 ,FILE='INPUT_WING.TXT')

     !Face and back are the position vectors of the points lying on the face and the back of the blades
     !respectively and their components are coordinates  with respect to the inertial system.
                              
      CALL WING_GEOMETRY
      
      DT  = 0.03D0 !DT = time step [sec]
      T(1)= 0.D0  
      DO I=2,MOMENTS 
        T(I) = T(I-1) + DT 
      END DO         		 
				 
      CALL OUTPUT_GEOMETRY(NR,NCH,NBL,SPAN,U1,U2 ,DEC_DU1,DEC_DU2,MOMENTS,T, &
                         DET_DU1,DET_DU2 ,XS,DXS ,SKEW,DSKEW ,FI,DFI ,CHORD,DCHORD)
      DO J=1,3 ; A(J) = 0.D0 ;END DO
     !Initial vector base of the moving cartesian
     !system ,projected on the inertial system
      UNIT1(1) = 1.D0 ;UNIT1(2) = 0.D0 ;UNIT1(3) = 0.D0 
      UNIT2(1) = 0.D0 ;UNIT2(2) = 1.D0 ;UNIT2(3) = 0.D0 
      UNIT3(1) = 0.D0 ;UNIT3(2) = 0.D0 ;UNIT3(3) = 1.D0 
     !The origin point of the moving cartesian system
     !coincides initially with that of the inertial
      ORIGIN(1)= 0.D0 ;ORIGIN(2)= 0.D0 ;ORIGIN(3)= 0.D0
        
      DO L=1,MOMENTS 
        CALL VELOCITIES(T(L) , FACE,BACK,NR,NCH,NBL ,VFACE,VBACK,PIVOT)
        CALL OUTPUT_POSITION_VELOCITY(FACE,BACK,VFACE,VBACK, &
                                          UNIT1,UNIT2,UNIT3,NR,NCH,NBL,A)
        CALL MOVEMENT(FACE,BACK,UNIT1,UNIT2,UNIT3,ORIGIN,PIVOT,NR,NCH,NBL  ,T(L)+DT,T(L))
        A(:) = PATH(T(L)+DT)            
      END DO

      END SUBROUTINE MAIN
      
!     ******************************************************************
!     *                                                                *
!     *                 SUBROUTINE OUTPUT_GEOMETRY                     *
!     *                                                                *
!     *           OUTPUT OF BASIC GEOMETRIC AND TIME STEPS             *
!     *                                                                *
!     ******************************************************************       

      SUBROUTINE OUTPUT_GEOMETRY(NR,NCH,NBL,SPAN,U1,U2 ,DEC_DU1,DEC_DU2,MOMENTS,T, &
                         DET_DU1,DET_DU2 ,XS,DXS ,SKEW,DSKEW ,FI,DFI ,CHORD,DCHORD)

      INTEGER(I4B),INTENT(IN) :: NR,NCH,NBL ,MOMENTS    
      REAL(DP) :: T(MOMENTS),SPAN   
      REAL(DP),DIMENSION(NR)          :: U1 ,XS,DXS ,SKEW,DSKEW  ,FI,DFI   ,CHORD,DCHORD
      REAL(DP),DIMENSION(NR,NCH)      :: U2  ,DEC_DU1,DEC_DU2  ,DET_DU1,DET_DU2
      !Locals
      INTEGER(I4B):: I
      REAL(DP) :: ROUT,RHUB

                
      ROUT = SPAN ;RHUB=0.D0

      WRITE(3,*)  ROUT ,RHUB ,NBL ,NR ,NCH ,MOMENTS 
      DO I=1,NR
      WRITE(3,*) U1(I), CHORD(I),XS(I),SKEW(I),FI(I), DCHORD(I),DXS(I),DSKEW(I),DFI(I)
      END DO      

      DO I=1,NR
      WRITE(3,15) (U2(I,J),J=1,NCH),(DEC_DU1(I,J),J=1,NCH),(DEC_DU2(I,J),J=1,NCH)
      WRITE(3,15) (DET_DU1(I,J),J=1,NCH) ,(DET_DU2(I,J),J=1,NCH)
      END DO
   15 FORMAT (11F7.4/6F7.4) 
   
      WRITE(3,'(A7)') 'MOMENTS'
      DO I=1,MOMENTS
        WRITE(3,'(F7.4)') T(I)
      END DO
      
      END SUBROUTINE OUTPUT_GEOMETRY


!     ******************************************************************
!     *                                                                *
!     *              SUBROUTINE OUTPUT POSITION VELOCITY               *
!     *                                                                *
!     *         OUTPUT OF POSITION,VELOCITIES AND UNIT VECTORS         *
!     *                                                                *
!     ******************************************************************

      SUBROUTINE OUTPUT_POSITION_VELOCITY(FACE,BACK,VFACE,VBACK, &
                                          UNIT1,UNIT2,UNIT3,NR,NCH,NBL,A)
      
      INTEGER(I4B) ,INTENT(IN) :: NR,NCH,NBL                                                       
      REAL(DP),DIMENSION(NR,NCH,NBL,3),INTENT(IN) :: FACE ,BACK  
      REAL(DP),DIMENSION(NR,NCH,NBL,3),INTENT(IN) :: VFACE ,VBACK 
      REAL(DP),DIMENSION(3),INTENT(IN) :: UNIT1(3),UNIT2(3),UNIT3(3),A(3)
     !Locals 
      INTEGER(I4B) :: I,J,K
      
     20 FORMAT(3F16.5)
        WRITE(3,'(A20)') 'POSITION VECTOR FACE'
        DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH
          WRITE(3,20) FACE(I,J,K,1) ,FACE(I,J,K,2) ,FACE(I,J,K,3)
        END DO ;END DO ;END DO
        WRITE(3,'(A20)') 'POSITION VECTOR BACK'
        DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH
          WRITE(3,20) BACK(I,J,K,1) ,BACK(I,J,K,2) ,BACK(I,J,K,3)
        END DO ;END DO ;END DO   
        WRITE(3,'(A20)') 'POSITION VECTOR PATH' !Origin point of the moving cartesian system 
        WRITE(3,20) ORIGIN(1),ORIGIN(2),ORIGIN(3)
        WRITE(3,'(A13)') 'UNIT VECTOR 1'
        WRITE(3,20) UNIT1(1)-ORIGIN(1),UNIT1(2)-ORIGIN(2),UNIT1(3)-ORIGIN(3)
        WRITE(3,'(A13)') 'UNIT VECTOR 2'
        WRITE(3,20) UNIT2(1)-ORIGIN(1),UNIT2(2)-ORIGIN(2),UNIT2(3)-ORIGIN(3)
        WRITE(3,'(A13)') 'UNIT VECTOR 3'
        WRITE(3,20) UNIT3(1)-ORIGIN(1),UNIT3(2)-ORIGIN(2),UNIT3(3)-ORIGIN(3)

        WRITE(3,'(A15)') 'VELOCITIES FACE'  
        DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH
          WRITE(3,20) VFACE(I,J,K,1) ,VFACE(I,J,K,2) ,VFACE(I,J,K,3)
        END DO ;END DO ;END DO
        WRITE(3,'(A15)') 'VELOCITIES BACK'
        DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH
          WRITE(3,20) VBACK(I,J,K,1) ,VBACK(I,J,K,2) ,VBACK(I,J,K,3)
        END DO ;END DO ;END DO

      END SUBROUTINE OUTPUT_POSITION_VELOCITY


!     ******************************************************************
!     *                                                                *
!     *                   SUBROUTINE VELOCITIES                        *
!     *                                                                *
!     *   CALCULATION OF RELATIVE FUID VELOCITIES ON FACE AND BACK     *
!     *                                                                *
!     ******************************************************************       
       
      SUBROUTINE VELOCITIES(T , FACE,BACK,NR,NCH,NBL ,VFACE,VBACK,PIVOT)

      INTEGER(I4B) ,INTENT(IN) :: NR,NCH,NBL
      REAL(DP) ,INTENT(IN) :: T ,PIVOT(3)
      REAL(DP),DIMENSION(NR,NCH,NBL,3),INTENT(IN)    :: FACE ,BACK  
      REAL(DP),DIMENSION(NR,NCH,NBL,3),INTENT(INOUT) :: VFACE ,VBACK       
     !Locals
      INTEGER(I4B) :: I,J,K,L
      REAL(DP) :: DT ,UNIT11(3),UNIT21(3),UNIT31(3),ORIGIN1(3),PIVOT1(3)
 
     !We don't actully use these here. Filler arguments for "MOVEMENT"
      UNIT11(:) =0.D0 ;UNIT21(:)=0.D0 ;UNIT31(:) =0.D0 ;ORIGIN1(:)=0.D0 
     !Subroutine "MOVEMENT" changes the position of the pivot, but we don't
     !want that when we are calculating the velocities 
      PIVOT1(:) =PIVOT(:)

      DO L=1,3 ;DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH 
       !Save the 'about to move' blade in the velocity variables  
        VFACE(I,J,K,L) = FACE(I,J,K,L)
        VBACK(I,J,K,L) = BACK(I,J,K,L) 
      END DO ;END DO ;END DO ;END DO
      
      DT = 0.0001D0  !Time smaller than the time step ,for the calculation of velocities
      CALL MOVEMENT(VFACE,VBACK,UNIT11,UNIT21,UNIT31,ORIGIN1,PIVOT1,NR,NCH,NBL  ,T+DT,T) 
     !VFACE ,VBACK are the moved blade for now

      DO L=1,3 ;DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH
        VFACE(I,J,K,L) = (VFACE(I,J,K,L) - FACE(I,J,K,L))/DT 
        VBACK(I,J,K,L) = (VBACK(I,J,K,L) - BACK(I,J,K,L))/DT 
      END DO ;END DO ;END DO ;END DO

      END SUBROUTINE VELOCITIES

      
!     ******************************************************************
!     *                                                                *
!     *                   SUBROUTINE MOVEMENT                          *
!     *                                                                *
!     *      CALCULATION OF POSITION VECTORS IN EVERY TIME STEP        *
!     *                                                                *
!     ******************************************************************      
     
      SUBROUTINE MOVEMENT(FACE,BACK,UNIT1,UNIT2,UNIT3,ORIGIN,PIVOT ,NR ,NCH ,NBL ,T2,T1) 

      INTEGER(I4B) ,INTENT(IN) :: NR ,NCH ,NBL
      REAL(DP) ,INTENT(IN)  :: T1,T2
      REAL(DP) ,INTENT(INOUT) :: FACE(NR,NCH,NBL,3) ,BACK(NR,NCH,NBL,3) 
      REAL(DP) ,INTENT(INOUT),DIMENSION(3) :: UNIT1,UNIT2,UNIT3,ORIGIN,PIVOT
     !Locals
      INTEGER(I4B) :: K,I,J,L
      REAL(DP) ,DIMENSION(3) :: R1,R2,DR ,A,E1 ,AA 
      
     !Angle of rotation in every time-step      
      TH = 0.D0*(T2-T1) ! (14.0*PI/180.D0)*(SIN(2.0*PI*0.5*T2) - SIN(2.0*PI*0.5*T1)) ! 

!-------------------------- ROTATION ---------------------------------- 

      E1(:) = DIRECTION(T2) 
      A(:)  = PATH(T2)   

     !ROTATION OF BLADES
      DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH       
        DO L = 1,3 ;AA(L) = FACE(I,J,K,L) ;END DO
        FACE(I,J,K,:) = ROTATION(TH,AA ,A,E1)

        DO L = 1,3 ; AA(L) = BACK(I,J,K,L) ;END DO
        BACK(I,J,K,:) = ROTATION(TH,AA ,A,E1)          
      END DO ;END DO ;END DO
     !ROTATION OF UNIT VECTORS OF THE MOVING CARTESIAN SYSTEM
      UNIT1(:) = ROTATION(TH,UNIT1 ,A,E1)
      UNIT2(:) = ROTATION(TH,UNIT2 ,A,E1)
      UNIT3(:) = ROTATION(TH,UNIT3 ,A,E1)
     !ROATATION OF ORIGIN POINT AROUND THE PIVOT POINT
      ORIGIN(:) = ROTATION(TH,ORIGIN ,A,E1)
         
!---------------------- PARALLEL TRANSITION -------------------------- 
     
      R1(:) = PATH(T1)  
      R2(:) = PATH(T2) 
      DR = R2 - R1  
 
     !PARALLEL TRANSITION OF BLADES
      DO K=1,NBL ;DO I=1,NR ;DO J=1,NCH        
        FACE(I,J,K,:) = FACE(I,J,K,:) + DR           
        BACK(I,J,K,:) = BACK(I,J,K,:) + DR   
      END DO ;END DO ;END DO
      PIVOT(:) = PIVOT(:) + DR
     !PARALLEL TRANSITION OF UNIT VECTORS 
      UNIT1(:) = UNIT1(:) + DR  
      UNIT2(:) = UNIT2(:) + DR  
      UNIT3(:) = UNIT3(:) + DR 
     !PARALLEL TRASITION OF THE ORIGIN 
      ORIGIN(:)= ORIGIN(:)+ DR 
         
      END SUBROUTINE MOVEMENT
      

!     ******************************************************************
!     *                                                                *
!     *                   SUBROUTINE DIRECTION                         *
!     *                                                                *
!     *      DEFINITION OF UNIT VECTOR DESCRIBING THE DIRECTION        *
!     *                                                                *
!     ******************************************************************      

      FUNCTION DIRECTION(T) RESULT(E) 
  
      REAL(DP),INTENT(IN)   :: T
      REAL,DIMENSION(3) :: E
     !Locals
      REAL(DP) :: AZ ,EL ,PI
      
     !The direction towards which the wing is facing is described 
     !a unit vector along the x-axis fixed on the propeller.Input data are:  
     !
     !AZ = azimuth. Angel between the projection of the unit vector on the 
     !     xy plane and the x-axis of the inertial system [degrees]
     !EL = elavation. Agle between the unit vector and the z-axis of the 
     !     inertial systen [degrees]

      PI = 4.D0*ATAN(1.D0) ! Introdce PI  

      AZ = 90.D0   ![degrees]
      EL =  0.D0   ![degrees]
      
     !Calculation of components of unit vector on the inertial system
        
      AZ = PI/180.D0 * AZ  ! [radians] 
      EL = PI/180.D0 * EL  ! [radians]
  
      E(1) = COS(EL)*COS(AZ) 
      E(2) = COS(EL)*SIN(AZ) 
      E(3) = SIN(EL) 
  
      END FUNCTION DIRECTION


!     ******************************************************************
!     *                                                                *
!     *                       SUBROUTINE PATH                          *
!     *                                                                *
!     *          DEFINITION OF POSITION VECTOR OF THE PATH             *
!     *                                                                *
!     ******************************************************************

     !The path (3D-curve) on which the "center" of the wing moves , 
     !as it makes a parallel transition from one point to the next

      FUNCTION PATH(T) RESULT(R)

      REAL(DP) ,INTENT(IN) :: T
      REAL(DP) :: R(3)
  
     !PATH ALONG THE X-AXIS
      R(1) = 0.D0 !0.5D0*COS(2.0D0*PI*0.5D0*T) 
     !PATH ALONG THE Y-AXIS
      R(2) = 0.D0 
     !PATH ALONG THE Z-AXIS
      R(3) =-1.0D0*T
        
      END FUNCTION PATH      

      
!     ******************************************************************
!     *                                                                *
!     *                    SUBROUTINE ROTATION                         *
!     *                                                                *
!     *           ROTATION OF A POINT P AROUND A GIVEN AXIS            *
!     *                                                                *
!     ******************************************************************

      FUNCTION ROTATION(TH,P ,A,E1) RESULT(Q)
     
      REAL(DP),INTENT(IN)  :: TH ,P(3) ,E1(3) ,A(3)
      REAL(DP),DIMENSION(3):: Q 
     !Locals
      INTEGER(I4B) :: I
      REAL(DP) :: AP(3),AR(3),RP(3),E3(3) 
      REAL(DP) :: R ,DOT
      
     !P : Position vector at P
     !A,E1 : Define axis position by <point> and <direction unit vector>

      DO I=1,3
        AP(I) = (P(I) - A(I)) + (A(I) - PIVOT(I))
      END DO
      DO I=1,3
        DOT   = E1(1)*AP(1) + E1(2)*AP(2) + E1(3)*AP(3) !Dot product
        AR(I) = DOT*E1(I) 
        RP(I) = AP(I) - AR(I)
      END DO 
      R  = SQRT(RP(1)**2 + RP(2)**2 + RP(3)**2) 

      IF (R<10.D0**(-7.D0)) THEN !Degenerate case
        DO I=1,3 ; Q(I)  = P(I) ;END DO 
      ELSE
       !Cross Product
        E3(1) = E1(2)*AP(3) - E1(3)*AP(2) 
        E3(2) =-E1(1)*AP(3) + E1(3)*AP(1)
        E3(3) = E1(1)*AP(2) - E1(2)*AP(1)
         
        DO I=1,3
        Q(I)  = A(I) + AR(I) + COS(TH)*RP(I) + SIN(TH)*E3(I) - (A(I) - PIVOT(I))
        END DO         
      END IF
     
      END FUNCTION ROTATION

!     ******************************************************************
!     *                                                                *
!     *                     SUBROUTINE INPUT DATA                      *
!     *                                                                *
!     *              READING INPUT DATA FROM THE .TXT FILE             *
!     *                                                                *
!     ******************************************************************  

      SUBROUTINE INPUT_DATA
     !Locals
      INTEGER(I4B) :: I

      READ(6,*)  SPAN,NR1,NCH1

      ALLOCATE(R1(NR1),CHORD1(NR1),XS1(NR1),SKEW1(NR1),FI1(NR1),ETMAX1(NR1),ECMAX1(NR1) )
      ALLOCATE(CH1(NCH1),ETCH1(NCH1),ECCH1(NCH1) )    

      DO I=1,NR1
      READ(6,*) R1(I),CHORD1(I),XS1(I),SKEW1(I),FI1(I),ETMAX1(I),ECMAX1(I)
      END DO
      DO I=1,NCH1
      READ(6,*) CH1(I),ETCH1(I),ECCH1(I)
      END DO

      END SUBROUTINE INPUT_DATA


!     ******************************************************************
!     *                                                                *
!     *                    SUBROUTINE INTERPOLATION                    *
!     *                                                                *
!     *                   INTERPOLATE THE INPUT DATA                   *
!     *                                                                *
!     ******************************************************************         

      SUBROUTINE INTERPOLATION

     !Locals
      INTEGER(I4B) :: I,J
      REAL(DP) ::   PI,SPAN1,CSPAN,TIP_CUT,ARG
      	   
	  PI = 4.D0*ATAN(1.D0)
               
      TIP_CUT= 0.00D0   !To eixes TIP_CUT= 0.05D0
      SPAN1= R1(NR1)-R1(1)-TIP_CUT 
      DO I=1,NR
         ARG= PI/2.D0*DBLE(I-1)/DBLE(NR-1)                         !Argument [rad] 
         U1(I)=-SPAN1/2.D0*COS(2*ARG)+(R1(NR1)+R1(1))/2.D0         !Cosinus distribution (more points neat tip and hub)
        !U1(I)= SPAN1*SIN(ARG) + R1(1) +TIP_CUT/2.D0               !Sinus distribution (more points near the tip)
        !U1(I)= SPAN1*DBLE(I-1)/DBLE(NR-1) + R1(1) +TIP_CUT/2.D0   !Linear distribution  
      END DO
       
      CSPAN = CH1(NCH1)-CH1(1)    
      DO J=1,NCH 
        !CH(J) =-CSPAN/2.D0*COS(PI*DBLE(J-1)/DBLE(NCH-1)) +(CH1(NCH1)+CH1(1))/2.D0  !Cosinus distribution (more points near L.E and T.E)
         CH(J) =-CSPAN*COS(PI/2*DBLE(J-1)/DBLE(NCH-1)) +CH1(NCH1)                   !Cosinus distribution (more points near L.E)
        !CH(J) = CSPAN*SIN(PI/2*DBLE(J-1)/DBLE(NCH-1)) +CH1(1)                      !Sinus distribution (more points near T.E)
        !CH(J) = CSPAN*DBLE(J-1)/DBLE(NCH-1) +CH1(1)                                !Linear distribution
      END DO
      CH(1) = CH1(1) ;CH(NCH)= CH1(NCH1)  

      CHORD = LSA(NR,NR1,R1,CHORD1,U1,1,7)
      XS    = LSA(NR,NR1,R1,XS1   ,U1,1,3)
      SKEW  = LSA(NR,NR1,R1,SKEW1 ,U1,1,3)
      FI    = LSA(NR,NR1,R1,FI1   ,U1,1,3)      
      ETMAX = LSA(NR,NR1,R1,ETMAX1,U1,4,5)
      ECMAX = LSA(NR,NR1,R1,ECMAX1,U1,4,3)
      ETCH  = LSA(NCH,NCH1,CH1,ETCH1,CH,3,7)
      ECCH  = LSA(NCH,NCH1,CH1,ECCH1,CH,2,5)      

      END SUBROUTINE INTERPOLATION
      
!     ******************************************************************
!     *                                                                *
!     *                    SUBROUTINE WING_GEOMETRY                    *
!     *                                                                *
!     *             CALCULATION OF WING'S GEOMETRIC FEATURES           *
!     *                                                                *
!     ******************************************************************      
             
      SUBROUTINE WING_GEOMETRY
  
     !Locals
      INTEGER(I4B) :: I,J
      REAL(DP) ::   PI,PSI
      REAL(DP) ::   U_2(NCH),DE1(NR),DE2(NCH) !DIABOLE 
      REAL(DP) ,DIMENSION(NR,NCH)::  ET,EC ,X_FACE ,Y_FACE,X_BACK,Y_BACK
     !Pivot local variables
      INTEGER(I4B) :: I_P,J_P
      REAL(DP) :: U1_P, U2_P, CHORD_P, XS_P, SKEW_P, FI_P, ECMAX_P
      REAL(DP) :: B, EC_P, X_PIVOT, Y_PIVOT 
 
      	   
	  PI = 4.D0*ATAN(1.D0)
 
      CALL INPUT_DATA
      CALL INTERPOLATION

      DO I=1,NR
         U1(I)    = U1(I)*SPAN
         !CHORD(I) = CHORD(I)*SPAN
         !XS(I)    = XS(I)*SPAN
         SKEW(I)  = SKEW(I)*(PI/180.D0)
         FI(I)    = FI(I)  *(PI/180.D0)
      END DO
      DO I=1,NR ;DO J=1,NCH
         U2(I,J) = CH(J)-0.5D0   
         ET(I,J) = ETMAX(I)*ETCH(J)*CHORD(I)        
         EC(I,J) = ECMAX(I)*ECCH(J)*CHORD(I)    
      END DO ;END DO         

    ! CALCULATION OF DERIVATIVES
      DCHORD = DERIVATIVE(U1,CHORD,NR)
      DXS    = DERIVATIVE(U1,XS   ,NR)
      DSKEW  = DERIVATIVE(U1,SKEW ,NR)
      DFI    = DERIVATIVE(U1,FI   ,NR)

      DO J=1,NCH
	     DO I=1,NR ; DE1(I)=EC(I,J) ;END DO
         DEC_DU1(1:NR,J) = DERIVATIVE(U1,DE1,NR) 
         DO I=1,NR ; DE1(I)=ET(I,J) ;END DO
         DET_DU1(1:NR,J) = DERIVATIVE(U1,DE1,NR) 
      END DO
      DO I=1,NR
         DO J=1,NCH; U_2(J)=U2(I,J)*CHORD(I) ;END DO	     
	     DO J=1,NCH; DE2(J)=EC(I,J) ;END DO 
         DEC_DU2(I,1:NCH) = DERIVATIVE(U_2,DE2,NCH) 
         DO J=1,NCH; DE2(J)=ET(I,J) ;END DO 
         DET_DU2(I,1:NCH) = DERIVATIVE(U_2,DE2,NCH)         
      END DO

     !POSITION OF FACE AND BACK
      DO I=1,NR ;DO J=1,NCH      
         PSI = ATAN(DEC_DU2(I,J))  !For the linearized problem DEC_DU2<<1 --> PSI~0  
         X_FACE(I,J) = (U2(I,J)*CHORD(I) + ET(I,J)*SIN(PSI)) 
         Y_FACE(I,J) = (EC(I,J)*CHORD(I) - ET(I,J)*COS(PSI))
         X_BACK(I,J) = (U2(I,J)*CHORD(I) - ET(I,J)*SIN(PSI))
         Y_BACK(I,J) = (EC(I,J)*CHORD(I) + ET(I,J)*COS(PSI)) 
      END DO ;END DO

      DO I=1,NR ;DO J=1,NCH
         FACE(I,J,1,1)=-(XS(I) + U1(I)*TAN(SKEW(I))*TAN(FI(I))) &
                       +(-CHORD(I)*U2(I,J))*SIN(FI(I)) +Y_FACE(I,J)*COS(FI(I))
         FACE(I,J,1,2)= U1(I)
         FACE(I,J,1,3)= U1(I)*TAN(SKEW(I)) &
                       +X_FACE(I,J)*COS(FI(I))-Y_FACE(I,J)*SIN(FI(I))

         BACK(I,J,1,1)=-(XS(I) + U1(I)*TAN(SKEW(I))*TAN(FI(I))) &
        			   +(-CHORD(I)*U2(I,J))*SIN(FI(I)) +Y_BACK(I,J)*COS(FI(I))
         BACK(I,J,1,2)= U1(I)
         BACK(I,J,1,3)= U1(I)*TAN(SKEW(I)) &
                       +X_BACK(I,J)*COS(FI(I))-Y_BACK(I,J)*SIN(FI(I))         			 
      END DO;END DO	  
	  
     ! ------  CALCULATIONS FOR THE PIVOT POINT ------ 
           
      B = 0.10D0
      U1_P = SPAN/2.D0
      U2_P =-(1.D0/2.D0 - B)
      
      I_P = 1          
      DO I=2,NR
         IF ((U1(I)>U1_P).AND.(U1_P>=U1(I-1))) THEN
         I_P = I
         END IF
      END DO
      CHORD_P = (CHORD(I_P) + CHORD(I_P-1))/2.D0
      XS_P    = (XS(I_P)    + XS(I_P-1)   )/2.D0
      SKEW_P  = (SKEW(I_P)  + SKEW(I_P-1) )/2.D0
      FI_P    = (FI(I_P)    + FI(I_P-1)   )/2.D0      
      ECMAX_P = (ECMAX(I_P) + ECMAX(I_P-1))/2.D0

      J_P = 1
      DO J=2,NCH
        IF ((U2(I_P,J)>U2_P).AND.(U2_P>=U2(I_P,J-1))) THEN
        J_P = J
        END IF
      END DO      
      EC_P = ECMAX_P*(ECCH(J_P)+ECCH(J_P-1))/2.D0*CHORD_P 
          
      X_PIVOT = U2_P*CHORD_P 
      Y_PIVOT = EC_P

      PIVOT(1)=-(XS_P + U1_P*TAN(SKEW_P)*TAN(FI_P)) &
               +(-CHORD_P*U2_P)*SIN(FI_P) +Y_PIVOT*COS(FI_P)
      PIVOT(2)= U1_P
      PIVOT(3)= U1_P*TAN(SKEW_P) &
               +X_PIVOT*COS(FI_P)-Y_PIVOT*SIN(FI_P)

      END SUBROUTINE WING_GEOMETRY
      

!     ******************************************************************
!     *                                                                *
!     *                     SUBROUTINE DERIVATIVE                      *
!     *                                                                *
!     *               CALCULATE DERIVATIVE OF FUNCTIONS                *
!     *                                                                *
!     ******************************************************************
      
	  FUNCTION DERIVATIVE(X,Y,N) RESULT(DERIV)

      INTEGER(I4B) :: N 
      REAL(DP),INTENT(IN) ::  X(N),Y(N)
	  REAL(DP) :: DERIV(N) 
      
	 !LOCALS
      INTEGER(I4B) :: I
      REAL(DP) :: DY1,DY2,DY3
	  
      IF (N<3) THEN
        WRITE(*,*) 'DERIVATION ERROR.TOO FEW ARRAY ELEMENTS FOR 2ND ORDER LARGRANGE.'
      END IF
  
      DY1 = (Y(2)-Y(1)) /(X(2)-X(1)) 
      DY2 = (Y(3)-Y(2)) /(X(3)-X(2))
      DY3 = (Y(3)-Y(1)) /(X(3)-X(1))
      DERIV(1)= DY3 + DY1 - DY2 
  
      DO I=2,N-1
        DY1  = (Y(I)-Y(I-1)) /(X(I)-X(I-1))
        DY2  = (Y(I+1)-Y(I)) /(X(I+1)-X(I))
        DY3  = (Y(I+1)-Y(I-1)) /(X(I+1)-X(I-1))
        DERIV(I)= DY1 + DY2 - DY3 
      END DO
  
      DY1 = (Y(N-1)-Y(N-2)) /(X(N-1)-X(N-2)) 
      DY2 = (Y(N)-Y(N-1)) /(X(N)-X(N-1))
      DY3 = (Y(N)-Y(N-2)) /(X(N)-X(N-2))
      DERIV(N)= DY2 + DY3 - DY1 
	  
      END FUNCTION DERIVATIVE
      

!     ******************************************************************
!     *                                                                *
!     *                       SUBROUTINE LSA                           *
!     *                                                                *
!     *                 LEAST SQUARE APPROXIMATION                     *
!     *                                                                *
!     ******************************************************************    
      
      FUNCTION LSA(NN ,N ,X_I ,Y_I ,X ,FLAG ,M) RESULT(LSAP) 
	  
      INTEGER(I4B) ,INTENT(IN) :: NN ,N ,M ,FLAG
      INTEGER(I4B) :: ND ,I ,J ,K ,IOPT ,IREG   
      PARAMETER(ND=500)
      INTEGER(I4B) :: L(ND) ,MM
      REAL(DP) :: D
      REAL(DP),DIMENSION(NN):: X ,LSAP 
      REAL(DP),DIMENSION(N):: X_I ,Y_I  
      REAL(DP),DIMENSION(ND):: B
      REAL(DP),DIMENSION(ND,ND):: A 
	   
      IF (FLAG==1) THEN	
        !The interpolation function for FLAG=1 ,is a polynomial of order M :
        !F(X)=K_0 + K_1*X + K_2*X^2 + ... + K_M*X^M 
      ELSEIF (FLAG==2) THEN
        !The interpolation function for FLAG=2 ,is a polynomial of order M,
        !that intersects the origin point (0,0) and the (1,0) :
        !F(X)=(1-X)*X*(K_0 + K_1*X + K_2*X**2 + ... + K_M*X**(M-2)).
      ELSEIF (FLAG==3) THEN
        !The interpolation function for FLAG=3 intersects the points (0,0), 
        !(1,0) and it has infinite derivative at the origin point (0,1) :
        !F(X)=(1-X)*X**0.5*(K_0 + K_1*X + K_2*X**2 + ... + K_M*X**(M-2)).  
      ELSE
        !The interpolation function for FLAG=4 intersects the points (0,0), 
        !(1,0) and it has infinite derivative at them :
        !F(X)=(1-X)**0.5*X**0.5*(K_0 + K_1*X + K_2*X**2 + ... + K_M*X**(M-2)). 
      END IF

      
     !DEFINING THE MATRIX A , OF THE LINEAR SYSTEM A*C=B
      IF (FLAG==1) THEN
         DO J=1,M+1 ;DO K=1,M+1 ;A(J,K)=0;DO I=1,N
         IF ((X_I(I)<0.D0).OR.(X_I(I)>0.D0)) THEN 
           A(J,K)= A(J,K)+ X_I(I)**(K+J-2)
         END IF
         END DO ;END DO ;END DO
      ELSEIF (FLAG==2) THEN
         DO J=2,M ;DO K=2,M ;A(J-1,K-1)=0; DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
           A(J-1,K-1)= A(J-1,K-1)+ (1.D0-X_I(I))**2*X_I(I)**(K+J-2)
         END IF
         END DO;END DO;END DO
      ELSEIF (FLAG==3) THEN
         DO J=2,M ;DO K=2,M ;A(J-1,K-1)=0; DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
           A(J-1,K-1)= A(J-1,K-1)+ (1.D0-X_I(I))**2*X_I(I)**(K+J-3)
         END IF
         END DO ;END DO ;END DO
      ELSE
         DO J=2,M ;DO K=2,M ;A(J-1,K-1)=0; DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
           A(J-1,K-1)= A(J-1,K-1)+ (1.D0-X_I(I))*X_I(I)*X_I(I)**(K+J-4)
         END IF
         END DO ;END DO ;END DO
      END IF
     

!     DEFINING THE VECTOR B, OF THE LINEAR SYSTEM A*C=B
      IF (FLAG==1) THEN
         MM = M+1
         DO J=1,M+1 ;B(J)=0.D0 ;END DO
         DO J=1,M+1 ;DO I=1,N
         IF ((X_I(I)<0.D0).OR.(X_I(I)>0.D0)) THEN 
         B(J)= B(J)+ Y_I(I)*X_I(I)**(J-1)
         END IF 
         END DO ;END DO
      ELSEIF (FLAG==2) THEN
         MM = M-1
         DO J=2,M ;B(J-1)=0.D0 ;END DO
         DO J=2,M ;DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
         B(J-1)= B(J-1)+ Y_I(I)*(1.D0-X_I(I))*X_I(I)**(J-1) 
         END IF 
         END DO ;END DO
      ELSEIF (FLAG==3) THEN
         MM = M-1
         DO J=2,M ;B(J-1)=0.D0 ;END DO
         DO J=2,M ;DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
         B(J-1)= B(J-1)+ Y_I(I)*(1.D0-X_I(I))*X_I(I)**0.5D0*X_I(I)**(J-2) 
         END IF 
         END DO ;END DO
      ELSE
         MM = M-1
         DO J=2,M ;B(J-1)=0.D0 ;END DO
         DO J=2,M ;DO I=1,N
         IF ((X_I(I)>0.D0).AND.(X_I(I)<1.D0)) THEN
         B(J-1)= B(J-1)+ Y_I(I)*(1.D0-X_I(I))**0.5D0*X_I(I)**0.5D0*X_I(I)**(J-2) 
         END IF 
         END DO ;END DO 
      END IF
      
      IOPT=1
      CALL DGAUSS(ND,MM,A,B,IOPT,D,IREG,L)
      IOPT=2
      CALL DGAUSS(ND,MM,A,B,IOPT,D,IREG,L)
     !From this point on ,vector B is the solution of the system
 
      DO I=1,NN
      IF (FLAG==1) THEN
         LSAP(I)=B(1)
         IF ((X(I)>0.D0).OR.(X(I)<0.D0)) THEN 
           DO J=2,M+1
	       LSAP(I)= LSAP(I) + B(J)*X(I)**(J-1)
           END DO
         END IF
      ELSEIF (FLAG==2) THEN         
         LSAP(I)=0.D0
         IF ((X(I)>0.D0).AND.(X(I)<1.D0)) THEN 
	        DO J=1,M-1
	        LSAP(I)= LSAP(I) + B(J)*(1.D0-X(I))*X(I)**(J)
            END DO
         END IF
      ELSEIF (FLAG==3) THEN
         LSAP(I)=0.D0
         IF ((X(I)>0.D0).AND.(X(I)<1.D0)) THEN
	        DO J=1,M-1
	        LSAP(I)= LSAP(I) + B(J)*(1.D0-X(I))*X(I)**0.5D0*X(I)**(J-1)
            END DO
         END IF
      ELSE
         LSAP(I)=0.D0
         IF ((X(I)>0.D0).AND.(X(I)<1.D0)) THEN  
	        DO J=1,M-1
	        LSAP(I)= LSAP(I) + B(J)*(1.D0-X(I))**0.5D0*X(I)**0.5D0*X(I)**(J-1)
            END DO
         END IF
      END IF          
      END DO

      END FUNCTION 


!     ******************************************************************
!     *                                                                *
!     *                   SUBROUTINE DGAUSS                            *
!     *                                                                *
!     *         GAUSS ELIMINATION METHOD (SYSTEM SOLUTION)             *
!     *                                                                *
!     ******************************************************************  

      SUBROUTINE DGAUSS(ND,N,A,B,IOPT,D,IREG,L)
      
      INTEGER(I4B):: IREG ,N ,ND ,NM , IOPT ,K ,KP ,M ,I ,J ,L
      REAL(DP):: A,B,D,S,EE
      DIMENSION:: A(ND,ND),B(ND),L(ND) 
       
!     DECOMPOSITION OF THE MATRIX A AND/OR SOLUTION OF THE
!     LINEAR SYSTEM AX=B BY GAUSS' ELIMINATION METHOD

!     INPUT:
!     ND: MAXIMUM COLUMN DIMENSION OF ARRAYS
!     N: DIMENSION OF ARRAYS
!     A: SYSTEM MATRIX (NXN)
!     B: RIGHT-HAND SIDE VECTOR (N)
!     IOPT = 0, DECOMPOSITION OF A AND SOLUTION OF THE SYSTEM
!          = 1, DECOMPOSITION OF A ONLY
!          = 2, SOLUTION OF THE SYSTEM (SKIPS DECOMPOSITION)
        
!     OUTPUT:
!     A: DECOMPOSED MATRIX (LOWER\UPPER PART), IF IOPT=0 OR 1
!     B:=X: SOLUTION VECTOR STORED IN B (N)
!     D: DETERMINANT OF A                                                
!     IREG = 0, SINGULAR MATRIX A
!          = 1, REGULAR MATRIX A
!     L: VECTOR OF PIVOTING INDICES (N)



      
      EE=1.D-12
      IREG=1
      NM=N-1
      IF(IOPT.EQ.2) GO TO 60

!     TRIANGULAR DECOMPOSITION OF A WITH MAXIMUM COLUMN PIVOTING
!     (ELIMINATION WITH MULTIPLIERS STORED IN THE LOWER PART OF A)
	   
      D=1.D0
      DO K=1,NM
         KP=K+1
         M=K
         DO I=KP,N
             IF(DABS(A(I,K)).GT.DABS(A(M,K))) M=I
         END DO
         L(K)=M
         D=D*A(M,K)
         IF(DABS(A(M,K)).LE.EE) GO TO 100   
             IF(M.EQ.K) GO TO 30
                 D=-D
                 DO J=K,N
                     S=A(M,J)
                     A(M,J)=A(K,J)
                     A(K,J)=S
                 END DO
   30        DO I=KP,N
                 S=A(I,K)/A(K,K)
                 A(I,K)=S
                 DO J=KP,N
                     A(I,J)=A(I,J)-S*A(K,J)
                 END DO
             END DO
      END DO
      D=D*A(N,N)
      IF(DABS(A(N,N)).LE.EE) GO TO 100

      IF(IOPT.EQ.1) RETURN

!     ELIMINATION OPERATIONS ON B

   60 DO K=1,NM
         KP=K+1
         M=L(K)
         S=B(M)
         B(M)=B(K)
         B(K)=S
         DO I=KP,N
             B(I)=B(I)-A(I,K)*S
	     END DO
      END DO
      
!     BACK-SUBSTITUTION

      B(N)=B(N)/A(N,N)
      DO I=1,NM
         K=N-I
         KP=K+1
         S=0.D0
         DO J=KP,N
             S=S+A(K,J)*B(J)
         END DO
         B(K)=(B(K)-S)/A(K,K)
      END DO
      RETURN
  100 IREG=0
      RETURN
      
      END SUBROUTINE DGAUSS

      END MODULE GEOMETRY_AND_MOTION_PRE_PROCESSOR  


!     ******************************************************************
!     *                                                                *
!     *                    THE ACTUAL PROGRAM                          *
!     *                                                                *
!     *                                                                *
!     *                                                                *
!     ****************************************************************** 
 
      PROGRAM WING 
      USE GEOMETRY_AND_MOTION_PRE_PROCESSOR
      IMPLICIT NONE
      CALL MAIN
      END PROGRAM WING        