## general methods of moments (GMM)
library(MASS)

MMGpoidsNW <- function(param,modele)
{
  donnees <- modele$donnees
  q <- modele$q
  donneesF <- donnees[-1]
  donneesL <- donnees[-length(donnees)]
  deltaTemps <- modele$deltaTemps
  a <- param[1]
  b <- param[2]
  Gamma <- array(0,c(4,4,q+1))
  if(modele$nomModele=="CKLS")
    {
      sigma <- param[3]
      gamma <- param[4]
      g1t <- donneesF - a - b * donneesL
      g2t <- (donneesF - a - b * donneesL) ^ 2 - sigma^2 * donneesL ^ (2*gamma) *
        deltaTemps
      g3t <- (donneesF - a - b * donneesL) * donneesL
      g4t <- ((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL ^ (2*gamma) *
              deltaTemps) * donneesL
    }
  
  if(modele$nomModele=="CIR")
    {
      sigma <- param[3]
      g1t <- donneesF - a - b * donneesL
      g2t <- (donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL * deltaTemps
      g3t <- (donneesF - a - b * donneesL) * donneesL
      g4t <- ((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL * deltaTemps) *
        donneesL
    }
  
  if(modele$nomModele=="Vasicek")
    {
      sigma <- param[3]
      g1t <- donneesF - a - b * donneesL
      g2t <- (donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * deltaTemps
      g3t <- (donneesF - a - b * donneesL) * donneesL
      g4t <- ((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * deltaTemps) * donneesL  
    }
  gt <- cbind(g1t,g2t,g3t,g4t)
  n <- length(g1t)
  ## en attendant
  #W <- solve(cov(gt))
  ## Newey-West
  gtc <- apply(gt,2,function(x) x-mean(x))
  for(v in 0:q)
    {
      gtF <- gtc[(1+v):n,]
      gtL <- gtc[1:(n-v),]
      Gamma[,,(v+1)] <- t(gtF) %*% gtL / n
    }
  S <- Gamma[,,1]
  for(v in 1:q)
    {
      Snext <- (1-v/(q+1)) * (Gamma[,,v+1] + t(Gamma[,,v+1]))
      S <- S+Snext
    }
  W <- ginv(S)
}

MomentsJacobien <- function(param, modele)
{
  donnees <- modele$donnees
  donneesF <- donnees[-1]
  donneesL <- donnees[-length(donnees)]
  n <- length(donneesL)
  deltaTemps <- modele$deltaTemps
  if(modele$nomModele=="CKLS")
    {
      a <- param[1]
      b <- param[2]
      sigma <- param[3]
      gamma <- param[4]
        
      g1a <- -n
      g2a <- -2*sum(donneesF - a - b*donneesL)
      g3a <- -sum(donneesL)
      g4a <- -2*sum((donneesF - a - b*donneesL)*donneesL)

      g1b <- -sum(donneesL)
      g2b <- -2*sum((donneesF - a - b*donneesL)*donneesL)
      g3b <- -sum(donneesL^2)
      g4b <- -2*sum((donneesF - a - b*donneesL)*donneesL^2)

      g1s <- 0
      g2s <- -deltaTemps*sum(donneesL^(2*gamma))
      g3s <- 0
      g4s <- -deltaTemps*sum(donneesL^(2*gamma+1))

      g1g <- 0
      g2g <- -2*sigma^2*deltaTemps*sum(log(donneesL)*donneesL^(2*gamma))
      g3g <- 0
      g4g <- -2*sigma^2*deltaTemps*sum(log(donneesL)*donneesL^(2*gamma+1))
        
        d <- cbind(c(g1a, g1b, g1s, g1g),
             c(g2a, g2b, g2s, g2g),
             c(g3a, g3b, g3s, g3g),
             c(g4a, g4b, g4s, g4g))/n
    }
  if(modele$nomModele=="CIR")
    {
      a <- param[1]
      b <- param[2]
        
      g1a <- -n
      g2a <- -2*sum(donneesF - a - b*donneesL)
      g3a <- -sum(donneesL)
      g4a <- -2*sum((donneesF - a - b*donneesL)*donneesL)

      g1b <- -sum(donneesL)
      g2b <- -2*sum((donneesF - a - b*donneesL)*donneesL)
      g3b <- -sum(donneesL^2)
      g4b <- -2*sum((donneesF - a - b*donneesL)*donneesL^2)
      
      g1s <- 0
      g2s <- -sum(deltaTemps*donneesL)
      g3s <- 0
      g4s <- -sum(deltaTemps*donneesL*donneesL)

      d <- cbind(c(g1a, g1b, g1s),
             c(g2a, g2b, g2s),
             c(g3a, g3b, g3s),
             c(g4a, g4b, g4s))/n         
    }
  if(modele$nomModele=="Vasicek")
    {
      a <- param[1]
      b <- param[2]
      
      g1a <- -n
      g2a <- -2*sum(donneesF - a - b*donneesL)
      g3a <- -sum(donneesL)
      g4a <- -2*sum((donneesF - a - b*donneesL)*donneesL)
      
      g1b <- -sum(donneesL)
      g2b <- -2*sum((donneesF - a - b*donneesL)*donneesL)
      g3b <- -sum(donneesL^2)
      g4b <- -2*sum((donneesF - a - b*donneesL)*donneesL^2)
      
      g1s <- 0
      g2s <- -deltaTemps*n
      g3s <- 0
      g4s <- -sum(deltaTemps*donneesL)

      d <- cbind(c(g1a, g1b, g1s),
                 c(g2a, g2b, g2s),
                 c(g3a, g3b, g3s),
                 c(g4a, g4b, g4s))/n  
    }
  d
}

MMGobjectif<- function(param, modele, W)
  {
    donnees <- modele$donnees
    donneesF <- donnees[-1]
    donneesL <- donnees[-length(donnees)]
    n <- length(donnees)-2
    deltaTemps <- modele$deltaTemps
    a <- param[1]
    b <- param[2]
    
    if(modele$nomModele=="CKLS")
      {
        sigma <- param[3]
        gamma <- param[4]
        g1 <- sum(donneesF - a - b * donneesL)
        g2 <- sum((donneesF - a - b * donneesL) ^ 2 - sigma^2 * donneesL ^ (2*gamma) *
                  deltaTemps)
        g3 <- sum((donneesF - a - b * donneesL) * donneesL)
        g4 <- sum(((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL ^ (2*gamma) *
                   deltaTemps) * donneesL )      
      }
        
    if(modele$nomModele=="CIR")
      {
        sigma <- param[3]
        g1 <- sum(donneesF - a - b * donneesL)
        g2 <- sum((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL * deltaTemps)
        g3 <- sum((donneesF - a - b * donneesL) * donneesL)
        g4 <- sum(((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * donneesL * deltaTemps)*
                  donneesL)  
      }
    
    if(modele$nomModele=="Vasicek")
      {
        sigma <- param[3]
        g1 <- sum(donneesF - a - b * donneesL)
        g2 <- sum((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * deltaTemps)
        g3 <- sum((donneesF - a - b * donneesL) * donneesL)
        g4 <- sum(((donneesF - a - b * donneesL) ^ 2 - sigma ^ 2 * deltaTemps) *
                  donneesL)     
      }
    g <- c(g1,g2,g3,g4)/n
    t(g) %*% W %*% g
  }

MMGestimation <- function(modele)
  {
    deltaTemps <- modele$deltaTemps
    ## paramètres initiaux
    if(modele$nomModele=="CKLS")
      {
        alpha <- 0.5
        beta <- -0.5
        sigma <- 0.5
        gamma <- 0.5
        a <- alpha * deltaTemps
        b <- beta * deltaTemps + 1
        Initialparam <- c(a,b,sigma,gamma)
      }
    if(modele$nomModele=="CIR" || modele$nomModele=="Vasicek")
      {
        alpha <- 0.5
        beta <- -0.5
        sigma <- 0.5
        a <- alpha * deltaTemps
        b <- beta * deltaTemps + 1
        Initialparam <- c(a,b,sigma)
      }
    ## Première étape avec matrice identité
    W <- diag(4)
    estim <- nlminb(Initialparam,MMGobjectif,gr=NULL,hessian=NULL,modele,W)
    param <- estim$par
    Fval <- estim$objective
    Exitflag <- estim$convergence
    if(modele$nomModele=="CKLS")
      {
        Ralpha <- estim$par[1] / deltaTemps 
        Rbeta  <- (estim$par[2] - 1) / deltaTemps 
        Rsigma2 <- estim$par[3] ^ 2  
        Rgamma <- estim$par[4] 
      }
    if(modele$nomModele=="CIR")
      {
        Ralpha <- estim$par[1] / deltaTemps 
        Rbeta  <- (estim$par[2] - 1) / deltaTemps 
        Rsigma2 <- estim$par[3] ^ 2 
        Rgamma <- 0.5 
      }
    
    if(modele$nomModele=="Vasicek")
      {
        Ralpha <- estim$par[1] / deltaTemps 
        Rbeta  <- (estim$par[2] - 1) / deltaTemps 
        Rsigma2 <- estim$par[3] ^ 2 
        Rgamma <- 0 
      }
    ## Seconde étape avec matrice W
    if(modele$Iterations>0)
      {
        for (i in 1:modele$Iterations)
        {
          Initialparam <- param
          W <- MMGpoidsNW(param, modele)
          estim <- nlminb(Initialparam,MMGobjectif,gr=NULL,hessian=NULL,modele,W)
          param <- estim$par
          Fval <- estim$objective
          Exitflag <- estim$convergence
          if(modele$nomModele=="CKLS")
            {
              Ralpha <- estim$par[1] / deltaTemps 
              Rbeta  <- (estim$par[2] - 1) / deltaTemps 
              Rsigma2 <- estim$par[3] ^ 2  
              Rgamma <- estim$par[4] 
            }
          if(modele$nomModele=="CIR")
            {
              Ralpha <- estim$par[1] / deltaTemps 
              Rbeta  <- (estim$par[2] - 1) / deltaTemps 
              Rsigma2 <- estim$par[3] ^ 2 
              Rgamma <- 0.5 
            }
          
          if(modele$nomModele=="Vasicek")
            {
              Ralpha <- estim$par[1] / deltaTemps 
              Rbeta  <- (estim$par[2] - 1) / deltaTemps 
              Rsigma2 <- estim$par[3] ^ 2 
              Rgamma <- 0 
            }
        }
      }
    
    ## Statistique T
    n <- length(modele$donnees)-1
    d <- MomentsJacobien(param,modele)
    Varparam <- ginv(d %*% W %*% t(d))
    Tstat <- solve(chol(Varparam),param)/sqrt(n)
    Tpvalue <- 1-pt(Tstat,n-length(param))
    list(Tstat=Tstat,Tpvalue=Tpvalue,
         Varparam=Varparam,param = c(Ralpha,Rbeta,Rsigma2,Rgamma), Fval=Fval,
         Exitflag=Exitflag)
    
  }
