#include "Config.fpp"

module mod_linchirp_signal

 !===================================================================================85
 !---------------------------- DESCRIPTION ------------------------------------------85
 !
 !
 !
 !          Module  name:
 !                         'mod_linchirp_signal'
 !
 !          Purpose:
 !                   Linear Chirp Signal
 !                   Implementation of linear chirp signal
 !                   in complex domain.
 !
 !          History:
 !                        Date: 13-08-2017
 !                        Time: 09:17 GMT+2
 !           Modified:
 !                      By Bernard Gingold on: 10-03-2018, 14:49 GMT+2
 !                   1) Refactoring to Fortran 2003 standard.
 !                   2) Removing unnecessary type-bound members
 !                   3) Adding computational subroutines (additive noise)
 !                   4) Removing unnecessary destructor,copy and initialization subroutines.
 !
 !          Version:
 !
 !                      Major: 1
 !                      Minor: 0
 !                      Micro: 0
 !
 !          Author:  Bernard Gingold
 !
 !
 !
 !
 !
 !          E-mail:
 !
 !                      beniekg@gmail.com
 !==================================================================================85

 ! Tab:5 col - Type and etc.. definitions
 ! Tab:10,11 col - Type , function and subroutine code blocks.

    use module_kinds , only : I32P, R64P
    use mod_code_timing
    use mod_complex_arithm , only : vcmag
    use mod_jonesvec
    implicit none
    private

    !=====================================================59
    !  File and module information:
    !  version,creation and build date, author,description
    !=====================================================59

    ! Major version
    integer(I32P) ,parameter, public :: MOD_LINCHIRP_SIGNAL_MAJOR = 1

    ! Minor version
    integer(I32P), parameter, public :: MOD_LINCHIRP_SIGNAL_MINOR = 0

    ! Micro version
    integer(I32P), parameter, public :: MOD_LINCHIRP_SIGNAL_MICRO = 0

    ! Module/file full version
    integer(I32P), parameter, public :: MOD_LINCHIRP_SIGNAL_FULLVER = 1000*MOD_LINCHIRP_SIGNAL_MAJOR+100*MOD_LINCHIRP_SIGNAL_MINOR + &
                                                                      10*MOD_LINCHIRP_SIGNAL_MICRO

    ! Module/file creation date
    character(*),  parameter, public :: MOD_LINCHIRP_SIGNAL_CREATE_DATE = "13-08-2017 09:21 +00200 (SUN 13 AUG 2017 GMT+2)"

    ! Moudle build date (should be set after every successful build)
    character(*),  parameter, public :: MOD_LINCHIRP_SIGNAL_BUILD_DATE = " "

    ! Module author info
    character(*),  parameter, public :: MOD_LINCHIRP_SIGNAL_AUTHOR = "Programmer: Bernard Gingold e-mail: beniekg@gmail.com"

    ! Module short description
    character(*),  parameter, public :: MOD_LINCHIRP_SIGNAL_DESCRIPT = "Linear Chirp complex-domain implementation."

    !======================================================60
    ! Type: LinChirpSignal_t
    !======================================================60

    type, public :: LinChirpSignal_t

        private

        ! Signal name
        character(len=64)   :: m_name

        ! Signal ID
        integer(I32P)       :: m_sid

        ! Number of samples
        integer(I32P)       :: m_nsamp

        ! Signal duration
        real(R64P)          :: m_dur

        ! Signal starting frequency (f0)
        real(R64P)          :: m_sfreq

        ! Signal ending frequency (f1)
        real(R64P)          :: m_efreq

        ! Signal chirp rate
        real(R64P)          :: m_chrate

        ! Signal initial phase i.e phi
        real(R64P)          :: m_siphi

        ! Signal initial sampling point
        real(R64P)          :: m_sinit

        ! Signal time-point interval increment
        real(R64P)          :: m_stsinc

        ! Polarization of Electrical field
        type(JonesVector_t), allocatable, dimension(:), public :: m_jvec

        ! Electric field far from radar source
        complex(R64P), allocatable, dimension(:),       public :: m_E
!DIR$   ATTRIBUTES ALIGN : 32 :: m_E

        ! Signal enevelope i.e natural envelope  set to 1
        real(R64P), allocatable, dimension(:),          public :: m_nenvp
!DIR$   ATTRIBUTES ALIGN : 32 :: m_nenvp

        ! Signal phase (phi(t))
        real(R64P), allocatable, dimension(:),          public :: m_phi
!DIR$   ATTRIBUTES ALIGN : 32 :: m_phi

        ! Signal canonical form i.e. S(t) = gc(t)*cos(wct)-gs(t)*sin(wct)
        complex(R64P), allocatable, dimension(:),       public :: m_canform
!DIR$   ATTRIBUTES ALIGN : 32 :: m_canform

        ! Signal basic form i.e. S(t) = g(t)*cos[wct+phi(t)]
        real(R64P), allocatable, dimension(:),          public :: m_basform
!DIR$   ATTRIBUTES ALIGN : 32 :: m_basform
        ! Signal complex envelope
        complex(R64P), allocatable, dimension(:),       public :: m_cenvp
!DIR$   ATTRIBUTES ALIGN : 32 :: m_cenvp

        ! Signal amplitude  A(phi,theta)/r used forcalculation electric field
        ! far from emmiter(antenna)
        real(R64P), allocatable, dimension(:),          public :: m_amp
!DIR$   ATTRIBUTES ALIGN : 32 :: m_amp

        ! Time-averaged power density
        ! To be properly implemented
        complex(R64P), allocatable, dimension(:),       public :: m_S
!DIR$   ATTRIBUTES ALIGN : 32 :: m_S

        contains

        ! Creates linear chirp signal
        ! Main constructor
        procedure, pass(this), public :: linchirp_signal

        ! Creates signal with noise modulated phase
        procedure, pass(this), public :: phase_noise_linchirp_signal

        ! Creates signal with noise modulated phase
        ! coupled with background additive noise.
        procedure, pass(this), public :: additive_noise_linchirp_signal

        !==========================================52
        !   Getter pure functions
        !==========================================52

        procedure, pass(this), public :: get_name

        procedure, pass(this), public :: get_sid

        procedure, pass(this), public :: get_nsamp

        procedure, pass(this), public :: get_dur

        procedure, pass(this), public :: get_sfreq

        procedure, pass(this), public :: get_efreq

        procedure, pass(this), public :: get_chrate

        procedure, pass(this), public :: get_siphi

        procedure, pass(this), public :: get_sinit

        procedure, pass(this), public :: get_stsinc

        !==================================================60
        ! Computational procedures
        !==================================================60

        procedure, pass(this), public :: dphi_dt

        procedure, pass(this), public :: analytic_signal

        !==================================================60
        !  read/write procedures
        !==================================================60
        procedure, nopass, public :: read_state

        procedure, nopass, public :: write_state

        !==================================================60
        !     Class helpr methods
        !==================================================60
        procedure, pass(this),     public :: dbg_info

        !============================================= 56
        !   Generic operators
        !==============================================56
        procedure, public :: copy_assign

        generic :: assignment (=) => copy_assign

        end type LinChirpSignal_t

    interface LinChirpSignal_t
         procedure :: constructor
    end interface LinChirpSignal_t

    contains

    !======================================================60
    ! @function: constructor
    !             default initialization (no physical meaning)
    !======================================================60
    type(LinChirpSignal_t) function constructor(name,sid,nsamp,logging, &
                               verbose, filename,append    )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_MINSAMP, LAM_PINF

          character(len=*),        intent(in)    :: name
          integer(I32P),           intent(in)    :: sid,nsamp
          logical(I32P),           intent(in)    :: logging,verbose
          character(len=*),        intent(in)    :: filename
          logical(I32P),           intent(in)    :: append
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: aerr
          ! Start of executable statements

          if(nsamp.LT.LAM_MINSAMP) then
              nsamp = LAM_MINSAMP
          end if
          ! Begin construction
          constructor%m_name   = name
          constructor%m_sid    = sid
          constructor%m_nsamp  = nsamp
          constructor%m_dur    = LAM_PINF
          constructor%m_sfreq  = LAM_PINF
          constructor%m_efreq  = LAM_PINF
          constructor%m_chrate = LAM_PINF
          constructor%m_siphi  = LAM_PINF
          constructor%m_sinit  = LAM_PINF
          constructor%m_stsinc = LAM_PINF

          associate(n=>constructor%m_nsamp)
              allocate(constructor%m_jvec(n),   &
                       constructor%m_E(n),       &
                       constructor%m_nenvp(n),   &
                       constructor%m_phi(n),     &
                       constructor%m_canform(n), &
                       constructor%m_basform(n), &
                       constructor%m_cenvp(n),   &
                       constructor%m_amp(n),     &
                       constructor%m_S(n),       &
                       STAT=aerr,         &
                       ERRMSG=emsg )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error(logging, verbose, append,filename,  &
                                             "logger:297 --> mod_linchirp_signal/constructor: Memory Allocation Failure!!" , &
                                             "mod_linchirp_signal/constructor:297 -- Memory Allocation Failure!" ,  &
                                             emsg,__LINE__ )
          end if
          constructor%m_jvec    = JonesVector_t()
          constructor%m_E       = CMPLX(LAM_PINF,LAM_PINF,R64P)
          constructor%m_nenvp   = LAM_PINF
          constructor%m_phi     = LAM_PINF
          constructor%m_canform = CMPLX(LAM_PINF,LAM_PINF,R64P)
          constructor%m_basform = LAM_PINF
          constructor%m_cenvp   = CMPLX(LAM_PINF,LAM_PINF,R64P)
          constructor%m_amp     = LAM_PINF
          constructor%m_S       = CMPLX(LAM_PINF,LAM_PINF,R64P)
    end function constructor

    !======================================================60
    !  @subroutine: linchirp_signal
    !              Physical representation of complex linear
    !              chirp signal.
    !======================================================60
    subroutine linchirp_signal(this,ierr,dur,sfreq,   &
                             efreq,chrate,siphi,sinit,stsinc, &
                             r,h,v,verbose,logging,filename,append,   &
                             profiling,qpctimer          )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_PINF, LAM_c, LAM_ZINIT, LAM_NS, LAM_HR64P, LAM_2PI
          class(LinChirpSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,sfreq,efreq
          real(R64P),                  intent(in)    :: stsinc,siphi,  &
                                                        chrate,sinit
          real(R64P), dimension(:),    intent(in)    :: r ! range
!DIR$     ASSUME_ALIGNED r:64
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64, v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: i,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,tc,ts
          real(R64P),    allocatable, dimension(:) :: ct
!DIR$     ATTRIBUTES ALIGN : 64 :: ct
          complex(R64P), allocatable, dimension(:) :: tmp
!DIR$     ATTRIBUTES ALIGN : 64 :: tmp
          ! Start of executable statements.
          if(ierr < 0_I32P) ierr = 0_I32P
          if(efreq <= sfreq) then
              ierr = -1_I32P
              return
          end if
          if(dur <= LAM_ZINIT) then
              dur = LAM_NS
          end if
          ! begin construction
          this%m_dur    = dur
          this%m_sfreq  = sfreq
          this%m_efreq  = efreq
          this%m_chrate = chrate
          this%m_siphi  = siphi
          this%m_sinit  = sinit
          this%m_stsinc = stsinc
          associate(n=>this%m_nsamp)
              allocate( ct(n),             &
                        tmp(n),            &
                        STAT=aerr,         &
                        ERRMSG=emsg     )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error(logging,verbose,append,filename,   &
                                             "logger:375 --> mod_linchirp_singnal/linchirp_signal: Memory Allocation Failure!!"  , &
                                             "mod_linchirp_signal/linchirp_signal:375 -- Memory Allocation Failure!!",  &
                                             emsg,__LINE__ )
          end if
          ct = LAM_PINF
          tmp = CMPLX(LAM_PINF,LAM_PINF,R64P)
          t = 0._R64P
          t2 = 0._R64P
          delta = 0._R64P
          insamp = 1._R64P/DBLE(this%m_nsamp)
          if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_linchirp_signal/linchirp_signal: qpctimer_start failed to query performance frequency counter!"
              end if
          end if
           ! Construct electric field
          do i = 1_I32P, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t = this%m_init-delta
              t2 = t-r(i)/LAM_c
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*t2*t2+this%m_sfreq)*LAM_2PI+this%m_siphi

              this%m_E(i) = DCMPLX(DCOS(this%m_phi(i)),DSIN(this%m_phi(i)))
          end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
          call vcmag(this%m_E,this%m_amp)
          ! Create signal complex components
          do i = 1, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t =  this%m_sinit*delta
              ct(i) = t ! use later in canonical form
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*t*t+this%m_sfreq) * &
                               LAM_2PI+this%m_siphi
              tmp(i) = DCMPLX(DCOS(this%m_phi(i)),DSIN(this%m_phi(i)))
          end do
           ! Compute natural envelope
          call vcmag(tmp,this%m_nenvp)
          tc = 0._R64P
          ts = 0._R64P
          ! Create canonical representation
          do i_I32P = 1, this%m_nsamp
              tc = this%m_nenvp(i)*DCOS(this%m_phi(i))
              ts = this%m_nenvp(i)*DSIN(this%m_phi(i))
              this%m_cenvp(i) = DCMPLX(tc,ts)
              this%m_canform(i) = DCMPLX(tc*DCOS(ct(i)),ts*DSIN(ct(i)))
              this%m_basform(i) = DREAL(this%m_canform(i))
          end do
            ! Compute time-average power density i.e. 0.5xExE*/n
          do i_I32P = 1, this%m_nsamp
              this%m_S(i) = LAM_HR64P*this%m_E(i)*DCONJG(this%m_E(i))*0.0026525198938992_R64P
          end do
          if(profiling) then
                if(ifail.EQ.0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail == .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_linchirp_signal/linchirp_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_linchirp_signal/linchirp_signal: Unable to read performance counter -- fatal!!"
                end if
          end if
   end subroutine linchirp_signal

    !======================================================60
    !  @subroutine: phase_noise_linchirp_signal
    !              Physical representation of Linear Chirp
    !              signal.
    !              This signal is corrupted by phase additive
    !              noise.
    !======================================================60
    subroutine phase_noise_linchirp_ignal(this,ierr,dur,sfreq,   &
                                  efreq,chrate,siphi,sinit,stsinc, &
                                  phnoise,r,h,v,verbose,logging,filename,append,   &
                                  profiling,qpctimer  )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_PINF, LAM_c, LAM_ZINIT, LAM_NS, LAM_HR64P, LAM_2PI
          class(LinChirpSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,sfreq,efreq
          real(R64P),                  intent(in)    :: stsinc,siphi,  &
                                                        chrate,sinit
          real(R64P), dimension(:),    intent(in)    :: phnoise,r ! range
!DIR$     ASSUME_ALIGNED phnoise:32,r:32
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64,v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: i,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,tc,ts,tn
          real(R64P),    allocatable, dimension(:) :: ct
!DIR$     ATTRIBUTES ALIGN : 64 :: ct
          complex(R64P), allocatable, dimension(:) :: tmp
!DIR$     ATTRIBUTES ALIGN : 64 :: tmp
          ! Start of executable statements
          if(ierr < 0_I32P) ierr = 0_I32P
          if(efreq <= sfreq) then
              ierr = -1_I32P
              return
          end if
          if(dur.LE.LAM_ZINIT) then
              dur = LAM_NS
          end if
          ! begin construction
          this%m_sfreq  = sfreq
          this%m_efreq  = efreq
          this%m_chrate = chrate
          this%m_siphi  = siphi
          this%m_sinit  = sinit
          this%m_stsinc = stsinc
          associate(n=>this%m_nsamp)
              allocate( ct(n),             &
                        tmp(n),            &
                       STAT=aerr,         &
                       ERRMSG=emsg )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error(logging,verbose,append,filename,   &
                                             "logger:510 --> mod_linchirp_singnal/phase_noise_linchirp_signal: Memory Allocation Failure!!" , &
                                             "mod_linchirp_signal/phase_noise_linchirp_signal:510 -- Memory Allocation Failure!" , &
                                             emsg,__LINE__ )
          end if
          ct = LAM_PINF
          tmp = CMPLX(LAM_PINF,LAM_PINF,R64P)

          t = 0._R64P
          t2 = 0._R64P
          delta = 0._R64P
          insamp = 1._R64P/DBLE(this%m_nsamp)
          if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_linchirp_signal/phase_noise_linchirp_signal: qpctimer_start failed to query performance frequency counter!"
              end if
          end if
          ! Construct electric field
          do i = 1_I32P, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t = this%m_init-delta
              t2 = t-r(i)/LAM_c
              tn = t2+phnoise(i)
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*tn*tn+this%m_sfreq)*LAM_2PI+this%m_siphi

              this%m_E(i) = DCMPLX(DCOS(this%m_phi(i)),DSIN(this%m_phi(i)))
          end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
          call vcmag(this%m_E,this%m_amp)
          ! Create signal complex components
          do i = 1_I32P, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t =  this%m_sinit*delta
              tn = t+phnoise(i)
              ct(i) = tn ! use later in canonical form
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*tn*tn+this%m_sfreq) * &
                               LAM_2PI+this%m_siphi
              tmp(i) = DCMPLX(DCOS(this%m_phi(i)),DSIN(this%m_phi(i)))
          end do
          ! Compute natural envelope
          call vcmag(tmp,this%m_nenvp)
          tc = 0._R64P
          ts = 0._R64P
          ! Create canonical representation
          do i = 1_I32P, this%m_nsamp
              tc = this%m_nenvp(i)*DCOS(this%m_phi(i))
              ts = this%m_nenvp(i)*DSIN(this%m_phi(i))
              this%m_cenvp(i) = DCMPLX(tc,ts)
              this%m_canform(i) = DCMPLX(tc*DCOS(ct(i)),ts*DSIN(ct(i)))
              this%m_basform(i) = DREAL(this%m_canform(i))
          end do
            ! Compute time-average power density i.e. 0.5xExE*/n
          do i = 1_I32P, this%m_nsamp
              this%m_S(i) = LAM_HR64P*this%m_E(i)*DCONJG(this%m_E(i))*0.0026525198938992_R64P
          end do

          if(profiling) then
                if(ifail == 0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail == .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_linchirp_signal/phase_noise_linchirp_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_linchirp_signal/phase_noise_linchirp_signal: Unable to read performance counter -- fatal!!"
                end if
          end if

    end subroutine  phase_noise_linchirp_signal

    !======================================================60
    !  @subroutine: additive_noise_linchirp_signal
    !              Physical representation of Linear Chirp
    !              signal.
    !              This signal is corrupted by phase additive
    !              noise and by background additive noise.
    !======================================================60
    subroutine additive_noise_linchirp_signal(this,ierr,dur,sfreq,   &
                                     efreq,chrate,siphi,sinit,stsinc, &
                                     phnoise,bnoise,r,h,v,verbose,logging,filename,append,   &
                                     profiling,qpctimer   )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_PINF, LAM_c, LAM_ZINIT, LAM_NS, LAM_HR64P, LAM_2PI
          class(LinChirpSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,sfreq,efreq
          real(R64P),                  intent(in)    :: stsinc,siphi,  &
                                                        chrate,sinit
          real(R64P), dimension(:),    intent(in)    :: phnoise,bnoise,r ! range
!DIR$     ASSUME_ALIGNED phnoise:64,bnoise:64,r:64
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64,v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: i,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,tc,ts,tn
          real(R64P),    allocatable, dimension(:) :: ct
!DIR$     ATTRIBUTES ALIGN : 64 :: ct
          complex(R64P), allocatable, dimension(:) :: tmp
!DIR$     ATTRIBUTES ALIGN : 64 :: tmp
          ! Start of executable statements
          if(ierr < 0_I32P) ierr = 0_I32P
          if(efreq <= sfreq) then
              ierr = -1_I32P
              return
          end if
          if(dur <= LAM_ZINIT) then
              dur = LAM_NS
          end if
          ! begin construction
          this%m_dur    = dur
          this%m_sfreq  = sfreq
          this%m_efreq  = efreq
          this%m_chrate = chrate
          this%m_siphi  = siphi
          this%m_sinit  = sinit
          this%m_stsinc = stsinc
          associate(n=>this%m_nsamp)
              allocate(  ct(n),             &
                         tmp(n),            &
                         STAT=aerr,         &
                         ERRMSG=emsg )

          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error(logging,verbose,append,filename,   &
                                             "logger:650 --> mod_linchirp_singnal/additive_noise_linchirp_signal: Memory allocation failure!!" , &
                                             "mod_linchirp_signal/phase_noise_linchirp_signal:650 -- Memory Allocation Failure!",  &
                                             emsg,__LINE__ )
          end if
          ct = LAM_PINF
          tmp = CMPLX(LAM_PINF,LAM_PINF,R64P)

          t = 0._R64P
          t2 = 0._R64P
          delta = 0._R64P
          insamp = 1._R64P/DBLE(this%m_nsamp)
          if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_linchirp_signal/additive_noise_linchirp_signal: qpctimer_start failed to query performance frequency counter!"
              end if
          end if
          ! Construct electric field
          do i = 1_I32P, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t = this%m_init-delta
              t2 = t-r(i)/LAM_c
              tn = t2+phnoise(i)
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*tn*tn+this%m_sfreq)*LAM_2PI+this%m_siphi

              this%m_E(i) = DCMPLX(bnoise(i)+DCOS(this%m_phi(i)),bnoise(i)+DSIN(this%m_phi(i)))
          end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
          call vcmag(this%m_E,this%m_amp)
          ! Create signal complex components
          do i = 1_I32P, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(i)*insamp
              t =  this%m_sinit*delta
              tn = t+phnoise(i)
              ct(i) = tn ! use later in canonical form
              this%m_phi(i) = (LAM_HR64P*this%m_chrate*tn*tn+this%m_sfreq) * &
                               LAM_2PI+this%m_siphi
              tmp(i) = DCMPLX(bnoise(i)+DCOS(this%m_phi(i)),bnoise(i)+DSIN(this%m_phi(i)))
          end do
          ! Compute natural envelope
          call vcmag(tmp,this%m_nenvp)
          tc = 0._R64P
          ts = 0._R64P
          ! Create canonical representation
          do i = 1_I32P, this%m_nsamp
              tc = this%m_nenvp(i)*DCOS(this%m_phi(i))
              ts = this%m_nenvp(i)*DSIN(this%m_phi(i))
              this%m_cenvp(i) = DCMPLX(tc,ts)
              this%m_canform(i) = DCMPLX(tc*DCOS(ct(i)),ts*DSIN(ct(i)))
              this%m_basform(i) = DREAL(this%m_canform(i))
          end do
            ! Compute time-average power density i.e. 0.5xExE*/n
          do i = 1_I32P, this%m_nsamp
              this%m_S(i) = LAM_HR64P*this%m_E(i)*DCONJG(this%m_E(i))*0.0026525198938992_R64P
          end do
          this%m_isbuilt = .true.
          if(profiling) then
                if(ifail.EQ.0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail == .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_linchirp_signal/additive_noise_linchirp_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_linchirp_signal/additive_noise_linchirp_signal: Unable to read performance counter -- fatal!!"
                end if
          end if

    end subroutine  additive_noise_linchirp_signal

    !======================================================60
    ! Getter pure functions
    !======================================================60
    pure function get_name(this) result(name)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          character(len=*) :: name
          ! Start of executable statements
          name = this%m_name
    end function

    pure function get_sid(this) result(sid)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          integer(I32P) :: sid
          ! Start of executable statements
          sid = this%m_sid
    end function

    pure function get_nsamp(this) result(nsamp)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          integer(I32P) :: nsamp
          ! Start of executable statements
          nsamp = this%m_nsamp
    end function

    pure function get_dur(this) result(dur)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: dur
          ! Start of executable statements
          dur = this%m_dur
    end function

    pure function get_sfreq(this) result(sfreq)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: sfreq
          ! Start of executable statements
          sfreq = this%m_sfreq
    end function

    pure function get_efreq(this) result(efreq)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: efreq
          ! Start of executable statements
          efreq = this%m_efreq
    end function

    pure function get_chrate(this) result(chrate)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: chrate
          ! Start of executable statements
          chrate = this%m_chrate
    end function

    pure function get_siphi(this) result(siphi)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: siphi
          ! Start of executable statements
          siphi = this%m_siphi
    end function

    pure function get_sinit(this) result(sinit)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: sinit
          ! Start of executable statements
          sinit = this%m_sinit
    end function

    pure function get_stsinc(this) result(stsinc)
          implicit none
          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: stsinc
          ! Start of executable statements
          stsinc = this%m_stsinc
    end function

    !======================================================60
    !    Computational procedures
    !======================================================60
    ! TODO:
    !        ! Consider using CADNA to test for cancellation errors
    subroutine dphi_dt(this,dphi,sfac)
          use mod_constants, only : LAM_IZER8, LAM_MEPS8
          class(LinChirpSignal_t),  intent(in)    :: this
          real(R64P), dimension(:), intent(out)   :: dphi
!DIR$     ASSUME_ALIGNED dphi:32
          integer(I64P),            intent(inout) :: sfac
          ! Locals
          integer(I32P) :: i
          real(R64P)    :: eps,isfac,tmp
          ! Start of executable statements
          if(sfac <= LAM_IZER8) then
              sfac = 1000000000000000_I64P
          end if
          isfac = 1._R64P/DBLE(sfac)
          eps = LAM_MEPS8**0.3333333333333333333333333333_R64P
          do i = 2, this%m_nsamp-1
              tmp = this%m_phi(i+1)-this%m_phi(i-1)
              dphi(i) = this%m_phi(i+1)-this%m_phi(i-1)/ &
                        (2._R64P*eps*DMAX1(DABS(tmp),isfac)*DSIGN(tmp,tmp))
          end do
    end subroutine dphi_dt

    !======================================================60
    ! subroutine: analytic signal
    !======================================================60
    subroutine analytic_signal(this,asig,tmplen,wlen,iplen, &
                                profiling,qpctimer )
          use mod_fftsg, only : rdft

          class(LinChirpSignal_t),     intent(inout) :: this
          complex(R64P), dimension(:), intent(out)   :: asig
!DIR$     ASSUME_ALIGNED asig:64
          integer(I32P),               intent(in)    :: tmplen   ! nsamp-1
          integer(I32P),               intent(in)    :: wlen     ! nsamp/2-1
          integer(I32P),               intent(in)    :: iplen    ! 2+2**(int(log(n/2+0.5)/log(2.0))/2)
          logical(I32P),               intent(in)    :: profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals
          integer(BOOL) :: ifail
          logical(I32P) :: bfail
          integer(I32P) :: i
          real(R64P), dimension(0:tmplen)   :: tmp,tmp2
          real(R64P), dimension(0:wlen)     :: w
          integer(I32P), dimension(0:iplen) :: ip
!DIR$     ATTRIBUTES ALIGN : 64 :: tmp
!DIR$     ATTRIBUTES ALIGN : 64 :: tmp2
!DIR$     ATTRIBUTES ALIGN : 64 :: w
          ! Start of executable sattements
          if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_expchirp_signal/analytic_signal:858, qpctimer_start failed to query performance counter!"
              end if
          end if
           ! Copy real part of canonical representation
!DIR$     SIMD VECTORLENGTHFOR(REAL(KIND=8))
          do i = 1_I32P, tmplen
              tmp(i) = DREAL(this%m_canform(i))
          end do
          ! Real FFT
          ip(0) = 0
          call rdft(tmplen,1,tmp,ip,w)
          do i = 0_I32P, tmplen
              tmp2(i) = tmp(i)+DSIGN(tmp(i),tmp(i)))*tmp(i)  !   Construct argument to Inverse FFT
          end do
          ! Inverse real FFT
          call rdft(tmplen,-1,tmp2,ip,w)
          ! Construct analytic signal content
          do i = 0_I32P, tmplen
              asig(i) = DCMPLX(DREAL(this%m_canform(i)),tmp2(i))
          end do
          if(profiling) then
               if(ifail.EQ.0) then
                   call qpctimer_stop(qpctimer,ifail)
                   call qpctimer_delta(qpctimer,bfail)
                   if(bfail == .false.) then
                        call qpctimer_print(qpctimer)
                   else
                        write(stderr,*) "mod_linchirp_signal/analytic_signal:901, qpctimer_delta: failed to compute delta measurement!!"
                   end if
               else
                       write(stderr,*) "module_linchirp_signal/analytic_signal:901 Unable to read performance counter -- fatal!!"
               end if
          end if
    end subroutine analytic_signal

     !============================================
    !   Read/write procedures
    !============================================
    subroutine read_state(this,form,unit,ioerr)

          class(LinChirpSignal_t), intent(in) :: this
          character(len=*),        intent(in) :: form
          integer(I32P),           intent(in) :: unit
          integer(I32P),           intent(inout) :: ioerr
          ! Start of executable statements
          select case(adjustl(trim(form)))
          case ("*")
              READ(unit,*,iostat=ioerr) this
          case default
              READ(unit,adjustl(trim(form)),iostat=ioerr) this
          end select
    end subroutine read_state

    subroutine write_state(this,form,unit,ioerr)

          class(LinChirpSignal_t), intent(in) :: this
          character(len=*),        intent(in) :: form
          integer(I32P),           intent(in) :: unit
          integer(I32P),           intent(inout) :: ioerr
          ! Start of executable statements
          select case(adjustl(trim(form)))
          case ("*")
              WRITE(unit,*,iostat=ioerr) this
          case default
              WRITE(unit,adjustl(trim(form)),iostat=ioerr) this
          end select
    end subroutine write_state

     !======================================================60
    !  subroutine: dbg_info
    !======================================================60
    subroutine dbg_info(this)

          class(LinChirpSignal_t), intent(in) :: this
          ! Locals
          character(len=40) :: dstr,tstr
          ! Start of executable statements
          call DATE_AND_TIME(date=dstr,time=tstr)
          print*, "======================================================"
          print*, "   ***Printing components of LinChirpSignal_t***"
          print*, "   printing subroutine called at: " , &
                     sdate(1:4),"-",sdate(5:6),"-",sdate(7:8), " ", &
                     stime(1:2),":",stime(3:4),":",stime(5:6)
          print*, "           Dumping scalar components "
          print*, "======================================================"
          print*, "Name:               ", this%m_name
          print*, "Signal ID:          ", this%m_sid
          print*, "Number of samples:  ", this%m_nsamp
          print*, "Signal duration:    ", this%m_dur
          print*, "Frequency (f0):     ", this%m_sfreq
          print*, "Frequency (f1):     ", this%m_efreq
          print*, "Time step:          ", this%m_stsinc
          print*, "Initial phase:      ", this%m_siphi
          print*, "Chirp rate(f1-f0/T),", this%m_chrate
          print*, "Start time-point:   ", this%m_sinit
          print*, "           Dumping array components  "
          print*, "===================================================="

          print*, "Signal polarization:", this%m_jvec
          print*, "Electric field:     ", this%m_E
          print*, "Natural Envelope:   ", this%m_nenvp
          print*, "Phase:              ", this%m_phi
          print*, "Canonical form:     ", this%m_canform
          print*, "Base form:          ", this%m_basform
          print*, "Complex envelope:   ", this%m_cenvp
          print*, "Amplitude:          ", this%m_amp
          print*, "Time-average power: ", this%m_S
          print*,"======================================================"

          print*, "           End                     "
    end subroutine  dbg_info

    !============================================
    !  Operator assignment (=)
    !============================================
    subroutine copy_assign(lhs,rhs)
          class(LinChirpSignal_t),  intent(inout) :: lhs
          class(LinChirpSignal_t),  intent(in)    :: rhs
          ! Start of executable statements
          lhs%m_name    = rhs%m_name
          lhs%m_sid     = rhs%m_sid
          lhs%m_nsamp   = rhs%m_nsamp
          lhs%m_dur     = rhs%m_dur
          lhs%m_sfreq   = rhs%m_sfreq
          lhs%m_efreq   = rhs%m_efreq
          lhs%m_stsinc  = rhs%m_stsinc
          lhs%m_siphi   = rhs%m_siphi
          lhs%m_chrate  = rhs%m_chrate
          lhs%m_sinit   = rhs%m_sinit
          lhs%m_jvec    = rhs%m_jvec
          lhs%m_E       = rhs%m_E
          lhs%m_nenvp   = rhs%m_nenvp
          lhs%m_phi     = rhs%m_phi
          lhs%m_canform = rhs%m_canform
          lhs%m_basform = rhs%m_basform
          lhs%m_cenvp   = rhs%m_cenvp
          lhs%m_amp     = rhs%m_amp
          lhs%m_S       = rhs%m_S
    end subroutine copy_assign

end module mod_linchirp_signal


*DECK COMQR
      SUBROUTINE COMQR (NM, N, LOW, IGH, HR, HI, WR, WI, IERR)
C***BEGIN PROLOGUE  COMQR
C***PURPOSE  Compute the eigenvalues of complex upper Hessenberg matrix
C            using the QR method.
C***LIBRARY   SLATEC (EISPACK)
C***CATEGORY  D4C2B
C***TYPE      COMPLEX (HQR-S, COMQR-C)
C***KEYWORDS  EIGENVALUES, EIGENVECTORS, EISPACK
C***AUTHOR  Smith, B. T., et al.
C***DESCRIPTION
C
C     This subroutine is a translation of a unitary analogue of the
C     ALGOL procedure  COMLR, NUM. MATH. 12, 369-376(1968) by Martin
C     and Wilkinson.
C     HANDBOOK FOR AUTO. COMP., VOL.II-LINEAR ALGEBRA, 396-403(1971).
C     The unitary analogue substitutes the QR algorithm of Francis
C     (COMP. JOUR. 4, 332-345(1962)) for the LR algorithm.
C
C     This subroutine finds the eigenvalues of a COMPLEX
C     upper Hessenberg matrix by the QR method.
C
C     On INPUT
C
C        NM must be set to the row dimension of the two-dimensional
C          array parameters, HR and HI, as declared in the calling
C          program dimension statement.  NM is an INTEGER variable.
C
C        N is the order of the matrix H=(HR,HI).  N is an INTEGER
C          variable.  N must be less than or equal to NM.
C
C        LOW and IGH are two INTEGER variables determined by the
C          balancing subroutine  CBAL.  If  CBAL  has not been used,
C          set LOW=1 and IGH equal to the order of the matrix, N.
C
C        HR and HI contain the real and imaginary parts, respectively,
C          of the complex upper Hessenberg matrix.  Their lower
C          triangles below the subdiagonal contain information about
C          the unitary transformations used in the reduction by  CORTH,
C          if performed.  HR and HI are two-dimensional REAL arrays,
C          dimensioned HR(NM,N) and HI(NM,N).
C
C     On OUTPUT
C
C        The upper Hessenberg portions of HR and HI have been
C          destroyed.  Therefore, they must be saved before calling
C          COMQR  if subsequent calculation of eigenvectors is to
C          be performed.
C
C        WR and WI contain the real and imaginary parts, respectively,
C          of the eigenvalues of the upper Hessenberg matrix.  If an
C          error exit is made, the eigenvalues should be correct for
C          indices IERR+1, IERR+2, ..., N.  WR and WI are one-
C          dimensional REAL arrays, dimensioned WR(N) and WI(N).
C
C        IERR is an INTEGER flag set to
C          Zero       for normal return,
C          J          if the J-th eigenvalue has not been
C                     determined after a total of 30*N iterations.
C                     The eigenvalues should be correct for indices
C                     IERR+1, IERR+2, ..., N.
C
C     Calls CSROOT for complex square root.
C     Calls PYTHAG(A,B) for sqrt(A**2 + B**2).
C     Calls CDIV for complex division.
C
C     Questions and comments should be directed to B. S. Garbow,
C     APPLIED MATHEMATICS DIVISION, ARGONNE NATIONAL LABORATORY
C     ------------------------------------------------------------------
C
C***REFERENCES  B. T. Smith, J. M. Boyle, J. J. Dongarra, B. S. Garbow,
C                 Y. Ikebe, V. C. Klema and C. B. Moler, Matrix Eigen-
C                 system Routines - EISPACK Guide, Springer-Verlag,
C                 1976.
C***ROUTINES CALLED  CDIV, CSROOT, PYTHAG
C***REVISION HISTORY  (YYMMDD)
C   760101  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  COMQR
C
      INTEGER I,J,L,N,EN,LL,NM,IGH,ITN,ITS,LOW,LP1,ENM1,IERR
      REAL HR(NM,*),HI(NM,*),WR(*),WI(*)
      REAL SI,SR,TI,TR,XI,XR,YI,YR,ZZI,ZZR,NORM,S1,S2
      REAL PYTHAG
C
C***FIRST EXECUTABLE STATEMENT  COMQR
      IERR = 0
      IF (LOW .EQ. IGH) GO TO 180
C     .......... CREATE REAL SUBDIAGONAL ELEMENTS ..........
      L = LOW + 1
C
      DO 170 I = L, IGH
         LL = MIN(I+1,IGH)
         IF (HI(I,I-1) .EQ. 0.0E0) GO TO 170
         NORM = PYTHAG(HR(I,I-1),HI(I,I-1))
         YR = HR(I,I-1) / NORM
         YI = HI(I,I-1) / NORM
         HR(I,I-1) = NORM
         HI(I,I-1) = 0.0E0
C
         DO 155 J = I, IGH
            SI = YR * HI(I,J) - YI * HR(I,J)
            HR(I,J) = YR * HR(I,J) + YI * HI(I,J)
            HI(I,J) = SI
  155    CONTINUE
C
         DO 160 J = LOW, LL
            SI = YR * HI(J,I) + YI * HR(J,I)
            HR(J,I) = YR * HR(J,I) - YI * HI(J,I)
            HI(J,I) = SI
  160    CONTINUE
C
  170 CONTINUE
C     .......... STORE ROOTS ISOLATED BY CBAL ..........
  180 DO 200 I = 1, N
         IF (I .GE. LOW .AND. I .LE. IGH) GO TO 200
         WR(I) = HR(I,I)
         WI(I) = HI(I,I)
  200 CONTINUE
C
      EN = IGH
      TR = 0.0E0
      TI = 0.0E0
      ITN = 30*N
C     .......... SEARCH FOR NEXT EIGENVALUE ..........
  220 IF (EN .LT. LOW) GO TO 1001
      ITS = 0
      ENM1 = EN - 1
C     .......... LOOK FOR SINGLE SMALL SUB-DIAGONAL ELEMENT
C                FOR L=EN STEP -1 UNTIL LOW E0 -- ..........
  240 DO 260 LL = LOW, EN
         L = EN + LOW - LL
         IF (L .EQ. LOW) GO TO 300
         S1 = ABS(HR(L-1,L-1)) + ABS(HI(L-1,L-1))
     1             + ABS(HR(L,L)) +ABS(HI(L,L))
         S2 = S1 + ABS(HR(L,L-1))
         IF (S2 .EQ. S1) GO TO 300
  260 CONTINUE
C     .......... FORM SHIFT ..........
  300 IF (L .EQ. EN) GO TO 660
      IF (ITN .EQ. 0) GO TO 1000
      IF (ITS .EQ. 10 .OR. ITS .EQ. 20) GO TO 320
      SR = HR(EN,EN)
      SI = HI(EN,EN)
      XR = HR(ENM1,EN) * HR(EN,ENM1)
      XI = HI(ENM1,EN) * HR(EN,ENM1)
      IF (XR .EQ. 0.0E0 .AND. XI .EQ. 0.0E0) GO TO 340
      YR = (HR(ENM1,ENM1) - SR) / 2.0E0
      YI = (HI(ENM1,ENM1) - SI) / 2.0E0
      CALL CSROOT(YR**2-YI**2+XR,2.0E0*YR*YI+XI,ZZR,ZZI)
      IF (YR * ZZR + YI * ZZI .GE. 0.0E0) GO TO 310
      ZZR = -ZZR
      ZZI = -ZZI
  310 CALL CDIV(XR,XI,YR+ZZR,YI+ZZI,XR,XI)
      SR = SR - XR
      SI = SI - XI
      GO TO 340
C     .......... FORM EXCEPTIONAL SHIFT ..........
  320 SR = ABS(HR(EN,ENM1)) + ABS(HR(ENM1,EN-2))
      SI = 0.0E0
C
  340 DO 360 I = LOW, EN
         HR(I,I) = HR(I,I) - SR
         HI(I,I) = HI(I,I) - SI
  360 CONTINUE
C
      TR = TR + SR
      TI = TI + SI
      ITS = ITS + 1
      ITN = ITN - 1
C     .......... REDUCE TO TRIANGLE (ROWS) ..........
      LP1 = L + 1
C
      DO 500 I = LP1, EN
         SR = HR(I,I-1)
         HR(I,I-1) = 0.0E0
         NORM = PYTHAG(PYTHAG(HR(I-1,I-1),HI(I-1,I-1)),SR)
         XR = HR(I-1,I-1) / NORM
         WR(I-1) = XR
         XI = HI(I-1,I-1) / NORM
         WI(I-1) = XI
         HR(I-1,I-1) = NORM
         HI(I-1,I-1) = 0.0E0
         HI(I,I-1) = SR / NORM
C
         DO 490 J = I, EN
            YR = HR(I-1,J)
            YI = HI(I-1,J)
            ZZR = HR(I,J)
            ZZI = HI(I,J)
            HR(I-1,J) = XR * YR + XI * YI + HI(I,I-1) * ZZR
            HI(I-1,J) = XR * YI - XI * YR + HI(I,I-1) * ZZI
            HR(I,J) = XR * ZZR - XI * ZZI - HI(I,I-1) * YR
            HI(I,J) = XR * ZZI + XI * ZZR - HI(I,I-1) * YI
  490    CONTINUE
C
  500 CONTINUE
C
      SI = HI(EN,EN)
      IF (SI .EQ. 0.0E0) GO TO 540
      NORM = PYTHAG(HR(EN,EN),SI)
      SR = HR(EN,EN) / NORM
      SI = SI / NORM
      HR(EN,EN) = NORM
      HI(EN,EN) = 0.0E0
C     .......... INVERSE OPERATION (COLUMNS) ..........
  540 DO 600 J = LP1, EN
         XR = WR(J-1)
         XI = WI(J-1)
C
         DO 580 I = L, J
            YR = HR(I,J-1)
            YI = 0.0E0
            ZZR = HR(I,J)
            ZZI = HI(I,J)
            IF (I .EQ. J) GO TO 560
            YI = HI(I,J-1)
            HI(I,J-1) = XR * YI + XI * YR + HI(J,J-1) * ZZI
  560       HR(I,J-1) = XR * YR - XI * YI + HI(J,J-1) * ZZR
            HR(I,J) = XR * ZZR + XI * ZZI - HI(J,J-1) * YR
            HI(I,J) = XR * ZZI - XI * ZZR - HI(J,J-1) * YI
  580    CONTINUE
C
  600 CONTINUE
C
      IF (SI .EQ. 0.0E0) GO TO 240
C
      DO 630 I = L, EN
         YR = HR(I,EN)
         YI = HI(I,EN)
         HR(I,EN) = SR * YR - SI * YI
         HI(I,EN) = SR * YI + SI * YR
  630 CONTINUE
C
      GO TO 240
C     .......... A ROOT FOUND ..........
  660 WR(EN) = HR(EN,EN) + TR
      WI(EN) = HI(EN,EN) + TI
      EN = ENM1
      GO TO 220
C     .......... SET ERROR -- NO CONVERGENCE TO AN
C                EIGENVALUE AFTER 30*N ITERATIONS ..........
 1000 IERR = EN
 1001 RETURN
      END


      PROGRAM xsimplx
C     driver for routine simplx
      INTEGER N,M,NP,MP,M1,M2,M3,NM1M2
      PARAMETER(N=4,M=4,NP=5,MP=6,M1=2,M2=1,M3=1,NM1M2=N+M1+M2)
      INTEGER i,icase,j,jj,jmax,izrov(N),iposv(M)
      REAL a(MP,NP),anum(NP)
      CHARACTER txt(NM1M2)*2,alpha(NP)*2
      LOGICAL rite
      DATA txt/'x1','x2','x3','x4','y1','y2','y3'/
      DATA a/0.0,740.0,0.0,0.5,9.0,0.0,1.0,-1.0,0.0,0.0,-1.0,0.0,
     *     1.0,0.0,-2.0,-1.0,-1.0,0.0,3.0,-2.0,0.0,1.0,-1.0,0.0,
     *     -0.5,0.0,7.0,-2.0,-1.0,0.0/
      call simplx(a,M,N,MP,NP,M1,M2,M3,icase,izrov,iposv)
      if (icase.eq.1) then
        write(*,*) 'Unbounded objective function'
      else if (icase.eq.-1) then
        write(*,*) 'No solutions satisfy constraints given'
      else
        jj=1
        do 11 i=1,N
          if (izrov(i).le.NM1M2) then
            alpha(jj)=txt(izrov(i))
            jj=jj+1
          endif
11      continue
        jmax=jj-1
        write(*,'(/3x,5a10)') '  ',(alpha(jj),jj=1,jmax)
        do 13 i=1,M+1
          if (i.eq.1) then
            alpha(1)='  '
            rite=.true.
          else if (iposv(i-1).le.NM1M2) then
            alpha(1)=txt(iposv(i-1))
            rite=.true.
          else
            rite=.false.
          endif
          if (rite) then
            anum(1)=a(i,1)
            jj=2
            do 12 j=2,N+1
              if (izrov(j-1).le.NM1M2) then
                anum(jj)=a(i,j)
                jj=jj+1
              endif
12          continue
            jmax=jj-1
            write(*,'(1x,a3,(5f10.2))') alpha(1),(anum(jj),jj=1,jmax)
          endif
13      continue
      endif
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xsimplx.for


module elemental_mod
contains
  elemental subroutine s(a,b,c)
    integer, intent(in) :: a, b
    integer, intent(out) :: c
    c = a + b
  end subroutine s
  elemental function f(a,b) result(f_res)
    integer, intent(in) :: a, b
    integer :: f_res
    f_res = a - b
  end function f
end module elemental_mod

program p
  use elemental_mod
  integer, dimension(3) :: x, y, z
  integer, dimension(2) :: w
  integer :: n
  read *, x, y
  call s(x,y,z)                 !elemental subroutine: OK
  print *, x, '+', y, '=', z
  call s(x,2,z)                 !elemental subroutine: OK
  print *, x, '+ 2 2 2 =', z
  z = f(x,y)                    !elemental function: OK
  print *, x, '-', y, '=', z
! Up to here OK, runs and prints vector sum & diff
! Now for some errors

  call s(x,y,w)      !elemental subroutine: arg size mismatch
  call s(x,y,n)      !elemental subroutine: out arg size mismatch
  z = f(x,w)         !elemental function: arg size mismatch
  w = f(x,y)         !elemental function: size mismatch on assignment
end program p


      subroutine qc25f(f,a,b,omega,integr,nrmom,maxp1,ksave,result,
     *   abserr,neval,resabs,resasc,momcom,chebmo)
c***begin prologue  qc25f
c***date written   810101   (yymmdd)
c***revision date  830518   (yymmdd)
c***category no.  h2a2a2
c***keywords  integration rules for functions with cos or sin
c             factor, clenshaw-curtis, gauss-kronrod
c***author  piessens,robert,appl. math. & progr. div. - k.u.leuven
c           de doncker,elise,appl. math. & progr. div. - k.u.leuven
c***purpose  to compute the integral i=integral of f(x) over (a,b)
c            where w(x) = cos(omega*x) or (wx)=sin(omega*x)
c            and to compute j=integral of abs(f) over (a,b). for small
c            value of omega or small intervals (a,b) 15-point gauss-
c            kronrod rule used. otherwise generalized clenshaw-curtis us
c***description
c
c        integration rules for functions with cos or sin factor
c        standard fortran subroutine
c        real version
c
c        parameters
c         on entry
c           f      - real
c                    function subprogram defining the integrand
c                    function f(x). the actual name for f needs to
c                    be declared e x t e r n a l in the calling program.
c
c           a      - real
c                    lower limit of integration
c
c           b      - real
c                    upper limit of integration
c
c           omega  - real
c                    parameter in the weight function
c
c           integr - integer
c                    indicates which weight function is to be used
c                       integr = 1   w(x) = cos(omega*x)
c                       integr = 2   w(x) = sin(omega*x)
c
c           nrmom  - integer
c                    the length of interval (a,b) is equal to the length
c                    of the original integration interval divided by
c                    2**nrmom (we suppose that the routine is used in an
c                    adaptive integration process, otherwise set
c                    nrmom = 0). nrmom must be zero at the first call.
c
c           maxp1  - integer
c                    gives an upper bound on the number of chebyshev
c                    moments which can be stored, i.e. for the
c                    intervals of lengths abs(bb-aa)*2**(-l),
c                    l = 0,1,2, ..., maxp1-2.
c
c           ksave  - integer
c                    key which is one when the moments for the
c                    current interval have been computed
c
c         on return
c           result - real
c                    approximation to the integral i
c
c           abserr - real
c                    estimate of the modulus of the absolute
c                    error, which should equal or exceed abs(i-result)
c
c           neval  - integer
c                    number of integrand evaluations
c
c           resabs - real
c                    approximation to the integral j
c
c           resasc - real
c                    approximation to the integral of abs(f-i/(b-a))
c
c         on entry and return
c           momcom - integer
c                    for each interval length we need to compute the
c                    chebyshev moments. momcom counts the number of
c                    intervals for which these moments have already been
c                    computed. if nrmom.lt.momcom or ksave = 1, the
c                    chebyshev moments for the interval (a,b) have
c                    already been computed and stored, otherwise we
c                    compute them and we increase momcom.
c
c           chebmo - real
c                    array of dimension at least (maxp1,25) containing
c                    the modified chebyshev moments for the first momcom
c                    momcom interval lengths
c
c***references  (none)
c***routines called  qcheb,qk15w,qwgtf,r1mach,sgtsl
c***end prologue  qc25f
c
      real a,abserr,ac,an,an2,as,asap,ass,b,centr,chebmo,
     *  cheb12,cheb24,conc,cons,cospar,d,qwgtf,
     *  d1,r1mach,d2,estc,ests,f,fval,hlgth,oflow,omega,parint,par2,
     *  par22,p2,p3,p4,resabs,resasc,resc12,resc24,ress12,ress24,
     *  result,sinpar,v,x
      integer i,iers,integr,isym,j,k,ksave,m,maxp1,momcom,neval,
     *  noequ,noeq1,nrmom
c
      dimension chebmo(maxp1,25),cheb12(13),cheb24(25),d(25),d1(25),
     *  d2(25),fval(25),v(28),x(11)
c
      external f,qwgtf
c
c           the vector x contains the values cos(k*pi/24)
c           k = 1, ...,11, to be used for the chebyshev expansion of f
c
      data x(1),x(2),x(3),x(4),x(5),x(6),x(7),x(8),x(9),
     *  x(10),x(11)/
     *     0.9914448613738104e+00,     0.9659258262890683e+00,
     *     0.9238795325112868e+00,     0.8660254037844386e+00,
     *     0.7933533402912352e+00,     0.7071067811865475e+00,
     *     0.6087614290087206e+00,     0.5000000000000000e+00,
     *     0.3826834323650898e+00,     0.2588190451025208e+00,
     *     0.1305261922200516e+00/
c
c           list of major variables
c           -----------------------
c
c           centr  - mid point of the integration interval
c           hlgth  - half-length of the integration interval
c           fval   - value of the function f at the points
c                    (b-a)*0.5*cos(k*pi/12) + (b+a)*0.5,
c                    k = 0, ..., 24
c           cheb12 - coefficients of the chebyshev series expansion
c                    of degree 12, for the function f, in the
c                    interval (a,b)
c           cheb24 - coefficients of the chebyshev series expansion
c                    of degree 24, for the function f, in the
c                    interval (a,b)
c           resc12 - approximation to the integral of
c                    cos(0.5*(b-a)*omega*x)*f(0.5*(b-a)*x+0.5*(b+a))
c                    over (-1,+1), using the chebyshev series
c                    expansion of degree 12
c           resc24 - approximation to the same integral, using the
c                    chebyshev series expansion of degree 24
c           ress12 - the analogue of resc12 for the sine
c           ress24 - the analogue of resc24 for the sine
c
c
c           machine dependent constant
c           --------------------------
c
c           oflow is the largest positive magnitude.
c
c***first executable statement  qc25f
      oflow = r1mach(2)
c
      centr = 0.5e+00*(b+a)
      hlgth = 0.5e+00*(b-a)
      parint = omega*hlgth
c
c           compute the integral using the 15-point gauss-kronrod
c           formula if the value of the parameter in the integrand
c           is small.
c
      if(abs(parint).gt.0.2e+01) go to 10
      call qk15w(f,qwgtf,omega,p2,p3,p4,integr,a,b,result,
     *  abserr,resabs,resasc)
      neval = 15
      go to 170
c
c           compute the integral using the generalized clenshaw-
c           curtis method.
c
   10 conc = hlgth*cos(centr*omega)
      cons = hlgth*sin(centr*omega)
      resasc = oflow
      neval = 25
c
c           check whether the chebyshev moments for this interval
c           have already been computed.
c
      if(nrmom.lt.momcom.or.ksave.eq.1) go to 120
c
c           compute a new set of chebyshev moments.
c
      m = momcom+1
      par2 = parint*parint
      par22 = par2+0.2e+01
      sinpar = sin(parint)
      cospar = cos(parint)
c
c           compute the chebyshev moments with respect to cosine.
c
      v(1) = 0.2e+01*sinpar/parint
      v(2) = (0.8e+01*cospar+(par2+par2-0.8e+01)*sinpar/
     *  parint)/par2
      v(3) = (0.32e+02*(par2-0.12e+02)*cospar+(0.2e+01*
     *  ((par2-0.80e+02)*par2+0.192e+03)*sinpar)/
     *  parint)/(par2*par2)
      ac = 0.8e+01*cospar
      as = 0.24e+02*parint*sinpar
      if(abs(parint).gt.0.24e+02) go to 30
c
c           compute the chebyshev moments as the
c           solutions of a boundary value problem with 1
c           initial value (v(3)) and 1 end value (computed
c           using an asymptotic formula).
c
      noequ = 25
      noeq1 = noequ-1
      an = 0.6e+01
      do 20 k = 1,noeq1
        an2 = an*an
        d(k) = -0.2e+01*(an2-0.4e+01)*(par22-an2-an2)
        d2(k) = (an-0.1e+01)*(an-0.2e+01)*par2
        d1(k+1) = (an+0.3e+01)*(an+0.4e+01)*par2
        v(k+3) = as-(an2-0.4e+01)*ac
        an = an+0.2e+01
   20 continue
      an2 = an*an
      d(noequ) = -0.2e+01*(an2-0.4e+01)*(par22-an2-an2)
      v(noequ+3) = as-(an2-0.4e+01)*ac
      v(4) = v(4)-0.56e+02*par2*v(3)
      ass = parint*sinpar
      asap = (((((0.210e+03*par2-0.1e+01)*cospar-(0.105e+03*par2
     *  -0.63e+02)*ass)/an2-(0.1e+01-0.15e+02*par2)*cospar
     *  +0.15e+02*ass)/an2-cospar+0.3e+01*ass)/an2-cospar)/an2
      v(noequ+3) = v(noequ+3)-0.2e+01*asap*par2*(an-0.1e+01)*
     *   (an-0.2e+01)
c
c           solve the tridiagonal system by means of gaussian
c           elimination with partial pivoting.
c
      call sgtsl(noequ,d1,d,d2,v(4),iers)
      go to 50
c
c           compute the chebyshev moments by means of forward
c           recursion.
c
   30 an = 0.4e+01
      do 40 i = 4,13
        an2 = an*an
        v(i) = ((an2-0.4e+01)*(0.2e+01*(par22-an2-an2)*v(i-1)-ac)
     *  +as-par2*(an+0.1e+01)*(an+0.2e+01)*v(i-2))/
     *  (par2*(an-0.1e+01)*(an-0.2e+01))
        an = an+0.2e+01
   40 continue
   50 do 60 j = 1,13
        chebmo(m,2*j-1) = v(j)
   60 continue
c
c           compute the chebyshev moments with respect to sine.
c
      v(1) = 0.2e+01*(sinpar-parint*cospar)/par2
      v(2) = (0.18e+02-0.48e+02/par2)*sinpar/par2
     *  +(-0.2e+01+0.48e+02/par2)*cospar/parint
      ac = -0.24e+02*parint*cospar
      as = -0.8e+01*sinpar
      if(abs(parint).gt.0.24e+02) go to 80
c
c           compute the chebyshev moments as the
c           solutions of a boundary value problem with 1
c           initial value (v(2)) and 1 end value (computed
c           using an asymptotic formula).
c
      an = 0.5e+01
      do 70 k = 1,noeq1
        an2 = an*an
        d(k) = -0.2e+01*(an2-0.4e+01)*(par22-an2-an2)
        d2(k) = (an-0.1e+01)*(an-0.2e+01)*par2
        d1(k+1) = (an+0.3e+01)*(an+0.4e+01)*par2
        v(k+2) = ac+(an2-0.4e+01)*as
        an = an+0.2e+01
   70 continue
      an2 = an*an
      d(noequ) = -0.2e+01*(an2-0.4e+01)*(par22-an2-an2)
      v(noequ+2) = ac+(an2-0.4e+01)*as
      v(3) = v(3)-0.42e+02*par2*v(2)
      ass = parint*cospar
      asap = (((((0.105e+03*par2-0.63e+02)*ass+(0.210e+03*par2
     *  -0.1e+01)*sinpar)/an2+(0.15e+02*par2-0.1e+01)*sinpar-
     *  0.15e+02*ass)/an2-0.3e+01*ass-sinpar)/an2-sinpar)/an2
      v(noequ+2) = v(noequ+2)-0.2e+01*asap*par2*(an-0.1e+01)
     *  *(an-0.2e+01)
c
c           solve the tridiagonal system by means of gaussian
c           elimination with partial pivoting.
c
      call sgtsl(noequ,d1,d,d2,v(3),iers)
      go to 100
c
c           compute the chebyshev moments by means of
c           forward recursion.
c
   80 an = 0.3e+01
      do 90 i = 3,12
        an2 = an*an
        v(i) = ((an2-0.4e+01)*(0.2e+01*(par22-an2-an2)*v(i-1)+as)
     *  +ac-par2*(an+0.1e+01)*(an+0.2e+01)*v(i-2))
     *  /(par2*(an-0.1e+01)*(an-0.2e+01))
        an = an+0.2e+01
   90 continue
  100 do 110 j = 1,12
        chebmo(m,2*j) = v(j)
  110 continue
  120 if (nrmom.lt.momcom) m = nrmom+1
       if (momcom.lt.maxp1-1.and.nrmom.ge.momcom) momcom = momcom+1
c
c           compute the coefficients of the chebyshev expansions
c           of degrees 12 and 24 of the function f.
c
      fval(1) = 0.5e+00*f(centr+hlgth)
      fval(13) = f(centr)
      fval(25) = 0.5e+00*f(centr-hlgth)
      do 130 i = 2,12
        isym = 26-i
        fval(i) = f(hlgth*x(i-1)+centr)
        fval(isym) = f(centr-hlgth*x(i-1))
  130 continue
      call qcheb(x,fval,cheb12,cheb24)
c
c           compute the integral and error estimates.
c
      resc12 = cheb12(13)*chebmo(m,13)
      ress12 = 0.0e+00
      k = 11
      do 140 j = 1,6
        resc12 = resc12+cheb12(k)*chebmo(m,k)
        ress12 = ress12+cheb12(k+1)*chebmo(m,k+1)
        k = k-2
  140 continue
      resc24 = cheb24(25)*chebmo(m,25)
      ress24 = 0.0e+00
      resabs = abs(cheb24(25))
      k = 23
      do 150 j = 1,12
        resc24 = resc24+cheb24(k)*chebmo(m,k)
        ress24 = ress24+cheb24(k+1)*chebmo(m,k+1)
        resabs = abs(cheb24(k))+abs(cheb24(k+1))
        k = k-2
  150 continue
      estc = abs(resc24-resc12)
      ests = abs(ress24-ress12)
      resabs = resabs*abs(hlgth)
      if(integr.eq.2) go to 160
      result = conc*resc24-cons*ress24
      abserr = abs(conc*estc)+abs(cons*ests)
      go to 170
  160 result = conc*ress24+cons*resc24
      abserr = abs(conc*ests)+abs(cons*estc)
  170 return
      end


      double complex function corr(nb,c,q,p,s)

      use cdat, only : im,al,pi

      implicit real*8 (a-h,o-z)

      real*8, intent(in) :: q(nb),p(nb),s(nb)
      complex*16, intent(in) :: c(nb)

      complex*16 :: mat(nb,nb),z,z0,c0(nb)
      common/wave/q0,p0,a0,sigma

      a = al
      alfa = 2d0*a0
      as=a+alfa
      av=a*alfa/as
      an=dsqrt(2d0*dsqrt(av/as))
      an0=dsqrt(dsqrt(a/pi))
      an2=dsqrt(dsqrt(alfa/pi))
!---- compute c0_k = <psi0|g_k>
      do j=1,nb
        dq=q(j)-q0
        dp=p(j)-p0
        d2=-0.5d0*av*dq*dq
        d0=-0.5d0/as*dp*dp
        d1=(alfa*p(j)+a*p0)/as*dq
        c0(j)=exp(d2+d0+im*d1)*an
      enddo

      corr = (0d0,0d0)
      do i=1,nb
        corr = corr+conjg(c0(i))*exp(im*s(i))*c(i)
      enddo

      end function corr


!     UMAT - Eleni Gerolymatou
      SUBROUTINE CompLib_New(STRESS,STATEV,DDSDDE,SSE,SPD,SCD,
     1 RPL,DDSDDT,DRPLDE,DRPLDT,
     2 STRAN,DSTRAN,TIME,DTIME,TEMP,DTEMP,PREDEF,DPRED,CMNAME,
     3 NDI,NSHR,NTENS,NSTATEV,PROPS,NPROPS,COORDS,DROT,PNEWDT,
     4 CELENT,DFGRD0,DFGRD1,NOEL,NPT,LAYER,KSPT,KSTEP,KINC)
!

      USE tensors
!      USE nlmodule   !! non local?

      IMPLICIT NONE
!
!---------------------------------------------------------------
!     Declarating UMAT vaiables and constants
      CHARACTER*80 CMNAME
      INTEGER NTENS,NSTATEV,NPROPS,LAYER,NOEL,NPT,KSPT,KSTEP,KINC,
     1 NDI,NSHR
      DOUBLE PRECISION  STRESS(NTENS),STATEV(NSTATEV),SSE,SPD,SCD,
     1 DDSDDE(NTENS,NTENS),DDSDDT(NTENS),DRPLDE(NTENS),
     2 STRAN(NTENS),DSTRAN(NTENS),TIME(2),PREDEF(1),DPRED(1),
     3 PROPS(NPROPS),COORDS(3),DROT(3,3),DFGRD0(3,3),DFGRD1(3,3),
     4 MAT(2,2)
! ---------------------------------------------------------------
      DOUBLE PRECISION RPL, DRPLDT, DTIME,
     1 TEMP, PNEWDT, CELENT, DTEMP
! ---------------------------------------------------------------
      DOUBLE PRECISION T(3,3), DEPS(3,3), JAC(3,3,3,3),
     1 evoid, epsP(3,3), pc, pt,
     2 ELMOD(3,3,3,3), Idev(3,3,3,3), TrialSTRESS(3,3)
     3 , TrialTh, uu,param(2),derve(6),dervp(6),dervs(6),dervh(6)
     4 , TrialGth,Trialp,Trialq,TrialF,A(13,13),B(13),dum,xx(13)
     5 ,theta,TolG, TolF,epsG,epsF
     6 ,normaldev(3,3) ,Depsp(3,3), NormDepsp, Isym(3,3,3,3)
      integer maxiter, kcounter,indx(13)
!     parameters for the material
      DOUBLE PRECISION E,nu,alpha,beta,Mf,cc,m,rhos,ksis,rhom,ksim,pc0
      DOUBLE PRECISION dep,deq,ps,pm,pss,pmm,denom,rrm
      DOUBLE PRECISION dfds(3,3),dfdep(3,3),aux(3,3),Hplast(3,3,3,3)
      DOUBLE PRECISION DSTRANV(6)
!     preparing the information

      CALL Initial(STRESS,T, DSTRAN, DEPS, NTENS,NDI, NSHR)
! ---------------------------------------------------------------
!     Defining material parameters
      E     =  PROPS(1) !young modulus
      nu    =  PROPS(2) !poisson modulus
!     other material properties: alpha,beta...
      alpha =  PROPS(3) !alpha variable of the exponential
      beta  =  PROPS(4) !beta variable of the exponential
      Mf    =  PROPS(5) !Mf
      cc    =  PROPS(6) !c for the Lode angle
      m     =  PROPS(7) !m value for pt
      rhos  =  PROPS(8) !rhos
      ksis  =  PROPS(9) !ksis
      rhom  =  PROPS(10)!rhom
      ksim  =  PROPS(11)!ksim
      pc0   =  PROPS(12)!dummy variable to make units fit
! ---------------------------------------------------------------
!     State variables
!     Void ratio as state variable (scalar)
      evoid=STATEV(1)
!     Plastic strains as state variable (tensor R3xR3)
      CALL Vectortomatrix(STATEV(2:7),epsP)
!     Compression Strength
      pc= STATEV(8)+STATEV(9)
      pt= m*STATEV(9)
!     ps
      param(1) = STATEV(8)
!     pm
      param(2) = STATEV(9)
! ---------------------------------------------------------------
!     Evaluating the elastic stiffness tensor
      CALL ELMOD1(E, nu, ELMOD)
! ---------------------------------------------------------------
!     Trial Elastic trial step
!     Trial stress
      TrialSTRESS=(ELMOD.double.DEPS)+T
!     the invariants need to be evaluated next
!     Deviator fourth unit tensor/ (ds/dsigma)
      CALL Idev1(Idev)
!     Trial deviator stress
!      TrialDevSTRESS=Idev.double.TrialSTRESS
!     Trial yield function F
      call Fyield(TrialSTRESS,STATEV,NSTATEV,NTENS,
     &  PROPS,NPROPS,param,TrialF)
! ---------------------------------------------------------------
      if (TrialF<0.0d0) then
!        Elastic Steps, Trial step is ok
         T=TrialSTRESS
         JAC=ELMOD
         evoid=evoid+(1.0d0+evoid)*(Tr(Deps))
         STATEV(1)=evoid
!        No need to save state variables
!        They are already correct
!         write(6,"(E10.5)") DSTRAN
! ---------------------------------------------------------------
      else ! Plastic corrector step

!         write(6,*) 'plastic', TrialF
! ---------------------------------------------------------------
!     Defining tolerances for G and F and maximum iterations
         TolG=1.0E-4               ! Tolerance for residuals
         TolF=1.0E-9               ! Tolerance for increments
         maxiter=50                ! maximum iterations
!     Initializing variables
!     Elastic strain increment
         xx(1:6) = DSTRAN
         xx(4:6) = xx(4:6)/2.0
		 DSTRANV = xx(1:6)
!     Plastic strain increment
         xx(7:12)= 0.0
!     Plastic multiplier
         xx(13)  = 0.0

         if (abs( STATEV(8))  <1.0d-8) write(*,*) 'ps not set '
         if (abs( STATEV(9))  <1.0d-8) write(*,*) 'pm not set '
!     ps
      param(1) = STATEV(8)
!     pm
      param(2) = STATEV(9)
! ---------------------------------------------------------------
!        Cycle for plastic increment
         do kcounter=1,maxiter
! ---------------------------------------------------------------
!            calling the yield function
             call Fyield(TrialSTRESS,xx(7:12),NSTATEV,NTENS,
     &            PROPS,NPROPS,param,TrialF)
!            calling the derivative evaluation
             call DerivF(TrialSTRESS,xx,NTENS,
     &            PROPS,NPROPS,param,derve,dervp,dervs)

!            defining the matrices A and B
!            initializing
             A=0.0
             B=0.0
             do i=1,6
                 A(i,i)     = 1.0
                 A(i,i+6)   = 1.0
                 A(i+6,i+6) = 1.0
                 A(i+6,13)  =-dervs(i)
                 A(13,i)    = derve(i)
                 A(13,i+6)  = dervp(i)
                 B(i)  = DSTRANV(i)-xx(i)-xx(i+6)
                 B(i+6)= abs(xx(13))*dervs(i)-xx(i+6)
             enddo
             B(13)=-TrialF

!            evaluate residuals
             epsG = sqrt(dot3(B,B,13))

             call ludcmp(A,13,13,indx,dum)
             call lubksb(A,13,13,indx,B)
!            update the solution
             xx = xx + B

!             write(6,*) B(1), B(2), B(3)
!             write(6,*) B(4), B(5), B(6)
!             write(6,*) B(7), B(8), B(9)
!             write(6,*) B(10), B(11), B(12)
!             write(6,*) B(13)

!			 stop
!            evaluate residuals
             epsF = sqrt(dot3(B,B,13))
!             write(6,*) TolG, TolF
             DEPS(1,1)=1.0d0*xx(1)
             DEPS(2,2)=1.0d0*xx(2)
             DEPS(3,3)=1.0d0*xx(3)
             DEPS(1,2)=1.0d0*xx(4)
             DEPS(2,1)=1.0d0*xx(4)
             DEPS(1,3)=1.0d0*xx(5)
             DEPS(3,1)=1.0d0*xx(5)
             DEPS(2,3)=1.0d0*xx(6)
             DEPS(3,2)=1.0d0*xx(6)
             TrialSTRESS = T + (ELMOD.double.DEPS)

!            Evaluating ep and eq
             dep = xx(7)+xx(8)+xx(9)
             deq = (xx(7)-dep/3.)**2+(xx(8)-dep/3.)**2+
     &             (xx(9)-dep/3.)**2+2.0*xx(10)**2+
     &             2.0*xx(11)**2+2.0*xx(12)**2
             deq = sqrt(2./3.*deq)
!            Getting ps
             ps = STATEV(8)*(1.+rhos*(-dep+ksis*deq))
!             ps = STATEV(8)*(1.+rhos*(-dep))
!            Getting pm
             pm = STATEV(9)*(1.+rhom*(-abs(dep)+ksim*deq))
             pm=min(pm,0.0)
             evoid=STATEV(1)+(1.0+STATEV(1))*
     &      (xx(1)+xx(2)+xx(3)+xx(7)+xx(8)+xx(9))
!             pm = STATEV(9)

!            Correcting pc
             param(1) = ps
!            Correcting pt
             param(2) = pm

!             write(6,*) NOEL,NPT,KSTEP,KINC
!             write(6,*) kcounter, epsF, epsG

! ---------------------------------------------------------------
! ---------------------------------------------------------------
             if (epsF<TolF.AND.epsG<TolG) then
                 exit
             endif
             if ( kcounter == maxiter-3) then
                 write(*,*) 'MaxIteration: ', epsF, epsG
            endif
         enddo
!         write(6,*) kcounter
!     End of iterations
! ---------------------------------------------------------------
!     Increment of plastic strains DepsP
! ---------------------------------------------------------------

! ---------------------------------------------------------------

! ---------------------------------------------------------------
!     Saving State variables
!     Alpha isotropic as state variable (scalar)
         STATEV(1)=evoid
!     Plastic strains as state variable (tensor R3xR3)
!         CALL Matrixtovector(STATEV(2:7),epsp)
         STATEV(2:4) = STATEV(2:4) + xx(7:9)
         STATEV(5:7) = STATEV(5:7) + 2.0*xx(10:12)
         STATEV(8)   = ps
         STATEV(9)   = pm
         STATEV(10)  = pc
		 STATEV(11:16) = dervs
! ---------------------------------------------------------------

! ---------------------------------------------------------------
!     Consistent elastoplastic modulus
         T=TrialSTRESS
!     Calling the derivatives
         call DerivF(TrialSTRESS,xx,NTENS,
     &            PROPS,NPROPS,param,derve,dervp,dervs)
!     Starting with the denominator
!     Will be needing dfds a lot...
         call Vectortomatrix(dervs,dfds)
!     and dfdep
         call Vectortomatrix(dervp,dfdep)
         aux = ELMOD.double.dfds
         denom = dfds.double.aux
!         write(6,*) denom
         dum=dfdep.double.dfds
         denom = denom - dum
!         write(6,*) dervs(1),dervp(1),dfdep.double.dfds,denom
         dfdep = dfds.double.ELMOD

         Hplast = aux.dyad.dfdep
!         write(6,*) Hplast(1,1,1,1)

         JAC=ELMOD - Hplast/denom
!         write(6,*) xx(13)

!         JAC=ELMOD
      endif
! ---------------------------------------------------------------
      Call Solution(NTENS, NDI, NSHR, T, STRESS, JAC
     1 , DDSDDE)

      !call give_nl(STATEV,NSTATEV,NOEL,NPT)
      ! give nonlocal behavior. matteo told me to comment it
! --------------------------------------------------------------
      END SUBROUTINE CompLib_New

      SUBROUTINE Fyield(STRESS,STATEV,NSTATEV,NTENS,
     &  PROPS,NPROPS,param,ff)
C
      USE tensors
      implicit none
C ---------------------------------------------------------------
      integer ntens, ndi, nshr, nstatev, nprops, noel, npt,
     & layer, kspt, kstep, kinc
      double precision stress(3,3), statev(nstatev),props(nprops),
     & param(2), ff
C ---------------------------------------------------------------
C     Declarating other vaiables and constants
      double precision p,q,uu,TrialTh,TrialGth,Mf,m,cc,alpha,beta,
     & pc,pt
C ---------------------------------------------------------------

!     other material properties: alpha,beta...
      alpha =  PROPS(3) !alpha variable of the exponential
      beta  =  PROPS(4) !beta variable of the exponential
      Mf    =  PROPS(5) !Mf
      cc    =  PROPS(6) !c for the Lode angle
      m     =  PROPS(7)

      pc    = param(1)+param(2)
      pt    = m*param(2)

      call pq(STRESS,p,q)
!     Lode angle
      TrialTh  = cos3theta1(STRESS)
      TrialGth = 2.0*cc/((1.0+cc)-(1.0-cc)*TrialTh)

!     auxilliary variable for the yield surface
      uu = exp(-((p-pt)/(pc-pt)-alpha)**2/beta)
      ff= q**2.0d0+Mf**2*TrialGth**2*uu*(p-pt)*(p-pc)
      ff=ff/PROPS(12)

      return
! ---------------------------------------------------------------
      END SUBROUTINE Fyield

      SUBROUTINE DerivF(STRESS,xx,NTENS,
     &  PROPS,NPROPS,param,derve,dervp,dervs)
C
      USE tensors
      implicit none
C ---------------------------------------------------------------
      integer ntens, ndi, nshr, nstatev, nprops, noel, npt,
     & layer, kspt, kstep, kinc
      double precision stress(3,3), xx(13),props(nprops),
     & param(2), ff,derve(6),dervp(6),dervs(6),ders(3,3)
C ---------------------------------------------------------------
C     Declarating other variables and constants
      double precision p,q,J2s,J3s,uu,TrialTh,TrialGth,STRAINDEV(3,3),
     & Mf,cc,alpha,beta,pc,pt,derinv(6),ELMOD(3,3,3,3),Idev(3,3,3,3),
     & STRESSDEV(3,3),temp2(3,3),Iunit(3,3,3,3),tempder(3,3)
      double precision dfdps,dfdpm,ps,pm,ep,eq,Iun(3,3),strain(3,3),TINY
      DOUBLE PRECISION E,nu,m,rhos,ksis,rhom,ksim,pc0
C ---------------------------------------------------------------
!     initializing parameters
      TINY  =  1.0D-14
!     Defining material parameters
      E     =  PROPS(1) !young modulus
      nu    =  PROPS(2) !poisson modulus
!     other material properties: alpha,beta...
      alpha =  PROPS(3) !alpha variable of the exponential
      beta  =  PROPS(4) !beta variable of the exponential
      Mf    =  PROPS(5) !Mf
      cc    =  PROPS(6) !c for the Lode angle
      m     =  PROPS(7) !m value for pt
      rhos  =  PROPS(8) !rhos
      ksis  =  PROPS(9) !ksis
      rhom  =  PROPS(10)!rhom
      ksim  =  PROPS(11)!ksim
      pc0   =  PROPS(12)!dummy variable to make units fit

      ps    = param(1)
      pm    = param(2)
      pc    = param(1)+param(2)
      pt    = m*param(2)
!     initializing derivatives
      derve = 0.0
      dervp = 0.0
      dervs = 0.0
      derinv = 0.0

      call ELMOD1(PROPS(1),PROPS(2), ELMOD)
      call pq(STRESS,p,q)
!     Evaluating plastic strain increment invariants
      strain(1,1)=xx(7)
      strain(2,2)=xx(8)
      strain(3,3)=xx(9)
      strain(1,2)=xx(10)
      strain(1,3)=xx(11)
      strain(2,3)=xx(12)
      strain(2,1)=xx(10)
      strain(3,1)=xx(11)
      strain(3,2)=xx(12)
      call pq(strain,ep,eq)
      ep = 3.0*ep
      eq = 2.0*eq/3.0
      Iun = 1.0
      call J23s(STRESS,J2s,J3s)
      call Idev1(Idev)
      STRESSDEV=double3333x33(Idev,STRESS)
	  STRAINDEV=double3333x33(Idev,strain)
!     Lode angle
      TrialTh  = cos3theta1(STRESS)
      TrialGth = 2.0*cc/((1.0+cc)-(1.0-cc)*TrialTh)
!     auxilliary variable for the yield surface
      uu = exp(-((p-pt)/(pc-pt)-alpha)**2/beta)
      ff= q**2.0d0+Mf**2*TrialGth**2*uu*(p-pt)*(p-pc)
!     evaluating the derivatives with respect to the invariants
!     p
      derinv(1) = Mf**2*TrialGth**2*uu*(2*p-pt-pc)
     & -2.0*(ff-q**2)*((p-pt)/(pc-pt)-alpha)/(beta*(pc-pt))
!     q
      derinv(2) = 2.0
!     J2s
      derinv(3) = -(ff-q**2)*(9*sqrt(3.0)*(1.0-cc)*J3s)/
     &(2.0*(1.0+cc)*J2s**(2.5)-sqrt(27.0)*(1.0-cc)*J2s*J3s)
!     J3s
      derinv(4) =  (ff-q**2)*(6.0*sqrt(3.0)*(1.0-cc))/
     &(2.0*(1.0+cc)*J2s**(1.5)-sqrt(27.0)*(1.0-cc)*J3s)
!     pt
      derinv(5) =-Mf**2*TrialGth**2*uu*(p-pc)
     &-2.0*(ff-q**2)*(p-pc)*((p-pt)/(pc-pt)-alpha)/(beta*(pc-pt)**2)
!     pc
      derinv(6) =-Mf**2*TrialGth**2*uu*(p-pt)
     &+2.0*(ff-q**2)*(p-pt)*((p-pt)/(pc-pt)-alpha)/(beta*(pc-pt)**2)

!     correcting for units
      derinv = derinv/PROPS(12)

!     evaluating the derivatives with respect to the plastic strains
      dervp = 0.0
      dfdps =  derinv(6)*ps*rhos
      dfdpm = (derinv(6)+m*derinv(5))*pm*rhom
!     with respect to ps - softening and hardening
!     volumetric strain
!      dervp(1) = dfdps*(rhos*ps/3.0)derinv(5)*0.0 +derinv(6)/3.0
      dervp(1) =-dfdps
      dervp(2) =-dfdps
      dervp(3) =-dfdps
      dervp(4) = 0.0
      dervp(5) = 0.0
      dervp(6) = 0.0
!     deviatoric strain
      if (eq>TINY) then
         temp2 = double33x3333(STRAINDEV,Idev)
!		 temp2(1,2) = temp2(1,2)+temp2(2,1)
!		 temp2(1,3) = temp2(1,3)+temp2(3,1)
!		 temp2(2,3) = temp2(3,2)+temp2(2,3)
         dervp(1) = dervp(1) + 2.0/3.0*dfdps*ksis/eq*temp2(1,1)
         dervp(2) = dervp(2) + 2.0/3.0*dfdps*ksis/eq*temp2(2,2)
         dervp(3) = dervp(3) + 2.0/3.0*dfdps*ksis/eq*temp2(3,3)
         dervp(4) = dervp(4) + 2.0/3.0*dfdps*ksis/eq*temp2(1,2)
         dervp(5) = dervp(5) + 2.0/3.0*dfdps*ksis/eq*temp2(1,3)
         dervp(6) = dervp(6) + 2.0/3.0*dfdps*ksis/eq*temp2(2,3)
      endif
!     with respect to pm - only softening
!     sign(a1,a2) = (sign of a2) * a1
!      dervp(1) = dervp(1) - dfdpm*sign(1.0D0,ep)
!      dervp(2) = dervp(1) - dfdpm*sign(1.0D0,ep)
!      dervp(3) = dervp(1) - dfdpm*sign(1.0D0,ep)

      dervp(1) = dervp(1) - dfdpm*sign(1.0D0,ep)
      dervp(2) = dervp(2) - dfdpm*sign(1.0D0,ep)
      dervp(3) = dervp(3) - dfdpm*sign(1.0D0,ep)
      dervp(4) = dervp(4) + 0.0
      dervp(5) = dervp(5) + 0.0
      dervp(6) = dervp(6) + 0.0
!     deviatoric strain
      if (eq>TINY) then
         temp2 = double33x3333(STRAINDEV,Idev)
!		 temp2(1,2) = temp2(1,2)+temp2(2,1)
!		 temp2(1,3) = temp2(1,3)+temp2(3,1)
!		 temp2(2,3) = temp2(3,2)+temp2(2,3)
         dervp(1) = dervp(1) + 2.0/3.0*dfdpm*ksim/eq*temp2(1,1)
         dervp(2) = dervp(2) + 2.0/3.0*dfdpm*ksim/eq*temp2(2,2)
         dervp(3) = dervp(3) + 2.0/3.0*dfdpm*ksim/eq*temp2(3,3)
         dervp(4) = dervp(4) + 2.0/3.0*dfdpm*ksim/eq*temp2(1,2)
         dervp(5) = dervp(5) + 2.0/3.0*dfdpm*ksim/eq*temp2(1,3)
         dervp(6) = dervp(6) + 2.0/3.0*dfdpm*ksim/eq*temp2(2,3)
      endif
!     evaluating the derivatives with respect to the stresses
      temp2 = double33x3333(STRESSDEV,Idev)
!	  temp2(1,2) = temp2(1,2)+temp2(2,1)
!	  temp2(1,3) = temp2(1,3)+temp2(3,1)
!	  temp2(2,3) = temp2(3,2)+temp2(2,3)
!      call Iunit1(Iunit)

      tempder = delta*derinv(1)/3.0+3./2.*derinv(2)*temp2
!      tempder = delta*derinv(1)/3.0+3./2.*derinv(2)*STRESSDEV

      dervs(1) = tempder(1,1)
      dervs(2) = tempder(2,2)
      dervs(3) = tempder(3,3)
      dervs(4) = tempder(1,2)
      dervs(5) = tempder(1,3)
      dervs(6) = tempder(2,3)

      temp2    = double33x3333(tempder,ELMOD)
!	  temp2(1,2) = temp2(1,2)+temp2(2,1)
!	  temp2(1,3) = temp2(1,3)+temp2(3,1)
!	  temp2(2,3) = temp2(3,2)+temp2(2,3)

      derve(1) = temp2(1,1)
      derve(2) = temp2(2,2)
      derve(3) = temp2(3,3)
      derve(4) = temp2(1,2)
      derve(5) = temp2(1,3)
      derve(6) = temp2(2,3)

      return
C ---------------------------------------------------------------
      END SUBROUTINE DerivF

      SUBROUTINE ludcmp(a,n,np,indx,d)
	  implicit none
      INTEGER n,np,indx(n),NMAX
      DOUBLE PRECISION d,a(np,np),TINY
!     Largest expected n, and a small number.
      PARAMETER (NMAX=500,TINY=1.0e-20)
!     Given a matrix a(1:n,1:n), with physical dimension np by np,
!     this routine replaces it by the LU decomposition of a rowwise
!     permutation of itself. a and n are input. a is output,
!     arranged as in equation (2.3.14) above; indx(1:n) is an output vector
!     that records the row permutation effected by the partial pivoting; d is
!     output as ±1 depending on whether the number of row interchanges was
!     even or odd, respectively. This routine is used in combination
!     with lubksb to solve linear equations or invert a matrix.
      INTEGER i,imax,j,k
!     vv stores the implicit scaling of each row.
      DOUBLE PRECISION aamax,dum,sum,vv(NMAX)
      d=1. !No row interchanges yet.
      do 12 i=1,n !Loop over rows to get the implicit scaling information
         aamax=0.
         do 11 j=1,n
             if (abs(a(i,j)).gt.aamax) aamax=abs(a(i,j))
 11      enddo
!        No nonzero largest element.
         if (aamax.lt.TINY) stop 'singular matrix in ludcmp'
         vv(i)=1./aamax !Save the scaling.
 12    enddo
       do 19 j=1,n !This is the loop over columns of Crout’s method.
!      This is equation (2.3.12) except for i = j.
         do 14 i=1,j-1
             sum=a(i,j)
             do 13 k=1,i-1
                 sum=sum-a(i,k)*a(k,j)
 13          enddo
             a(i,j)=sum
 14      enddo
!        Initialize for the search for largest pivot element.
         aamax=0.
         do 16 i=j,n
!        This is i = j of equation (2.3.12) and i = j+1. . .N
!        of equation (2.3.13).
             sum=a(i,j)
             do 15 k=1,j-1
                 sum=sum-a(i,k)*a(k,j)
 15          enddo
             a(i,j)=sum
             dum=vv(i)*abs(sum) !Figure of merit for the pivot.
             if (dum.ge.aamax) then !Is it better than the best so far?
                 imax=i
                 aamax=dum
             endif
 16      enddo
         if (j.ne.imax)then !Do we need to interchange rows?
             do 17 k=1,n !Yes, do so...
                 dum=a(imax,k)
                 a(imax,k)=a(j,k)
                 a(j,k)=dum
 17          enddo
             d=-d !...and change the parity of d.
             vv(imax)=vv(j) !Also interchange the scale factor.
         endif
         indx(j)=imax
         if(a(j,j).eq.0.) a(j,j)=TINY
!        If the pivot element is zero the matrix is singular (at least to
!        the precision of the algorithm).
!        For some applications on singular matrices, it is desirable to
!        substitute TINY for zero.
         if(j.ne.n)then !Now, finally, divide by the pivot element.
             dum=1./a(j,j)
             do 18 i=j+1,n
                 a(i,j)=a(i,j)*dum
 18          enddo
         endif
 19   enddo  !Go back for the next column in the reduction.
      return
      END

      SUBROUTINE lubksb(a,n,np,indx,b)
	  implicit none
      INTEGER n,np,indx(n)
      DOUBLE PRECISION a(np,np),b(n)
!     Solves the set of n linear equations A · X = B. Here a is input, not as
!     the matrix A but rather as its LU decomposition, determined by the
!     routine ludcmp. indx is input as the permutation vector returned by
!     ludcmp. b(1:n) is input as the right-hand side vector B,
!     and returns with the solution vector X. a, n, np, and indx are not
!     modified by this routine and can be left in place for successive calls
!     with different right-hand sides b. This routine takes into account the
!     possibility that b will begin with many zero elements, so it is
!     efficient for use in matrix inversion.
      INTEGER i,ii,j,ll
      DOUBLE PRECISION sum
      ii=0 !When ii is set to a positive value, it will become the index
!     of the first nonvanishing element of b. We now do
!     the forward substitution, equation (2.3.6). The only new
!     wrinkle is to unscramble the permutation as we go.
      do 12 i=1,n
         ll=indx(i)
         sum=b(ll)
         b(ll)=b(i)
         if (ii.ne.0)then
             do 11 j=ii,i-1
                 sum=sum-a(i,j)*b(j)
 11          enddo
         else if (sum.ne.0.) then
             ii=i !A nonzero element was encountered, so from now on we will
         endif !have to do the sums in the loop above.
         b(i)=sum
 12   enddo
      do 14 i=n,1,-1 !Now we do the backsubstitution, equation (2.3.7).
         sum=b(i)
         do 13 j=i+1,n
             sum=sum-a(i,j)*b(j)
 13      enddo
         b(i)=sum/a(i,i) !Store a component of the solution vector X.
 14   enddo
      return !All done!
      END

      SUBROUTINE USDFLD(FIELD,STATEV,PNEWDT,DIRECT,T,CELENT,
     1 TIME,DTIME,CMNAME,ORNAME,NFIELD,NSTATV,NOEL,NPT,LAYER,
     2 KSPT,KSTEP,KINC,NDI,NSHR,COORD,JMAC,JMATYP,MATLAYO,
     3 LACCFLA)
C

       implicit none
C
      CHARACTER*80 CMNAME,ORNAME
      CHARACTER*3  FLGRAY(15)
      integer nstatv,nfield,NOEL,NPT,LAYER,JRCD,
     1 KSPT,KSTEP,KINC,NDI,NSHR,MATLAYO,LACCFLA
      double precision FIELD(NFIELD),STATEV(NSTATV),DIRECT(3,3),
     1 T(3,3),TIME(2),ARRAY(15),COORD(*),PNEWDT,CELENT,DTIME
      integer JARRAY(15),JMAC(*),JMATYP(*)
      double precision n1,rho1,L1,T1,T1E,PHI1,P11,P12,P22,PI
      double precision n2,rho2,L2,T2,T2E,PHI2,RA1,RA2
      double precision POREP, STR(4) ,STRN,STRT,E,ANU,RES,RCOOR

  14  RETURN
      END