library(formula.tools)
library(matrixStats)

## df: dataframe to model
## outcome_formula: formula for y | x, z
## outcome_family: family for y | x, z
## proxy_formula: formula for w | x, z, y 
## proxy_family: family for w | x, z, y
## truth_formula: formula for x | z
## truth_family: family for x | z

### ideal formulas for example 1
# test.fit.1 <- measerr_mle(df, y ~ x + z, gaussian(), w_pred ~ x, binomial(link='logit'), x ~ z)

### ideal formulas for example 2
# test.fit.2 <- measerr_mle(df, y ~ x + z, gaussian(), w_pred ~ x + z + y + y:x, binomial(link='logit'), x ~ z)


## outcome_formula <- y ~ x + z; proxy_formula <- w_pred ~ y + x + z + x:z + x:y + z:y 
measerr_mle_dv <- function(df, outcome_formula, outcome_family=binomial(link='logit'), proxy_formula, proxy_family=binomial(link='logit')){

    nll <- function(params){
        df.obs <- model.frame(outcome_formula, df)
        proxy.variable <- all.vars(proxy_formula)[1]
        proxy.model.matrix <- model.matrix(proxy_formula, df)
        response.var <- all.vars(outcome_formula)[1]
        y.obs <- with(df.obs,eval(parse(text=response.var)))
        outcome.model.matrix <- model.matrix(outcome_formula, df.obs)

        param.idx <- 1
        n.outcome.model.covars <- dim(outcome.model.matrix)[2]
        outcome.params <- params[param.idx:n.outcome.model.covars]
        param.idx <- param.idx + n.outcome.model.covars

        if((outcome_family$family == "binomial") & (outcome_family$link == 'logit')){
            ll.y.obs <- vector(mode='numeric', length=length(y.obs))
            ll.y.obs[y.obs==1] <- plogis(outcome.params %*% t(outcome.model.matrix[y.obs==1,]),log=TRUE)
            ll.y.obs[y.obs==0] <- plogis(outcome.params %*% t(outcome.model.matrix[y.obs==0,]),log=TRUE,lower.tail=FALSE)
        }

        df.obs <- model.frame(proxy_formula,df)
        n.proxy.model.covars <- dim(proxy.model.matrix)[2]
        proxy.params <- params[param.idx:(n.proxy.model.covars+param.idx-1)]

        param.idx <- param.idx + n.proxy.model.covars
        proxy.obs <- with(df.obs, eval(parse(text=proxy.variable)))

        if( (proxy_family$family=="binomial") & (proxy_family$link=='logit')){
            ll.w.obs <- vector(mode='numeric',length=dim(proxy.model.matrix)[1])
            ll.w.obs[proxy.obs==1] <- plogis(proxy.params %*% t(proxy.model.matrix[proxy.obs==1,]),log=TRUE)
            ll.w.obs[proxy.obs==0] <- plogis(proxy.params %*% t(proxy.model.matrix[proxy.obs==0,]),log=TRUE, lower.tail=FALSE)
        }

        ll.obs <- sum(ll.y.obs + ll.w.obs)

        df.unobs <- df[is.na(df[[response.var]])]
        df.unobs.y1 <- copy(df.unobs)
        df.unobs.y1[[response.var]] <- 1
        df.unobs.y0 <- copy(df.unobs)
        df.unobs.y0[[response.var]] <- 1
        
        ## integrate out y
        outcome.model.matrix.y1 <- model.matrix(outcome_formula, df.unobs.y1)

        if((outcome_family$family == "binomial") & (outcome_family$link == 'logit')){
            ll.y.unobs.1 <- vector(mode='numeric', length=dim(outcome.model.matrix.y1)[1])
            ll.y.unobs.0 <- vector(mode='numeric', length=dim(outcome.model.matrix.y1)[1])
            ll.y.unobs.1 <- plogis(outcome.params %*% t(outcome.model.matrix.y1),log=TRUE)
            ll.y.unobs.0 <- plogis(outcome.params %*% t(outcome.model.matrix.y1),log=TRUE,lower.tail=FALSE)
        }

        proxy.model.matrix.y1 <- model.matrix(proxy_formula, df.unobs.y1)
        proxy.model.matrix.y0 <- model.matrix(proxy_formula, df.unobs.y0)
        proxy.unobs <- with(df.unobs, eval(parse(text=proxy.variable)))

        if( (proxy_family$family=="binomial") & (proxy_family$link=='logit')){
            ll.w.unobs.1 <- vector(mode='numeric',length=dim(proxy.model.matrix.y1)[1])
            ll.w.unobs.0 <- vector(mode='numeric',length=dim(proxy.model.matrix.y0)[1])
            ll.w.unobs.1[proxy.unobs==1] <- plogis(proxy.params %*% t(proxy.model.matrix.y1[proxy.unobs==1,]),log=TRUE)
            ll.w.unobs.1[proxy.unobs==0] <- plogis(proxy.params %*% t(proxy.model.matrix.y1[proxy.unobs==0,]),log=TRUE, lower.tail=FALSE)

            ll.w.unobs.0[proxy.unobs==1] <- plogis(proxy.params %*% t(proxy.model.matrix.y0[proxy.unobs==1,]),log=TRUE)
            ll.w.unobs.0[proxy.unobs==0] <- plogis(proxy.params %*% t(proxy.model.matrix.y0[proxy.unobs==0,]),log=TRUE, lower.tail=FALSE)
        }

        ll.unobs.1 <- ll.y.unobs.1 + ll.w.unobs.1
        ll.unobs.0 <- ll.y.unobs.0 + ll.w.unobs.0
        ll.unobs <- sum(colLogSumExps(rbind(ll.unobs.1,ll.unobs.0)))
        ll <- ll.unobs + ll.obs
        return(-ll)
    }
    
    params <- colnames(model.matrix(outcome_formula,df))
    lower <- rep(-Inf, length(params))
    proxy.params <- colnames(model.matrix(proxy_formula, df))
    params <- c(params, paste0('proxy_',proxy.params))
    lower <- c(lower, rep(-Inf, length(proxy.params)))
    start <- rep(0.1,length(params))
    names(start) <- params
    
    fit <- optim(start, fn = nll, lower=lower, method='L-BFGS-B', hessian=TRUE, control=list(maxit=1e6))
    return(fit)
}

measerr_mle <- function(df, outcome_formula, outcome_family=gaussian(), proxy_formula, proxy_family=binomial(link='logit'), truth_formula, truth_family=binomial(link='logit')){

    measrr_mle_nll <- function(params){
        df.obs <- model.frame(outcome_formula, df)
        
        proxy.variable <- all.vars(proxy_formula)[1]
        proxy.model.matrix <- model.matrix(proxy_formula, df)

        response.var <- all.vars(outcome_formula)[1]
        y.obs <- with(df.obs,eval(parse(text=response.var)))
        
        outcome.model.matrix <- model.matrix(outcome_formula, df)

        param.idx <- 1
        n.outcome.model.covars <- dim(outcome.model.matrix)[2]
        outcome.params <- params[param.idx:n.outcome.model.covars]
        param.idx <- param.idx + n.outcome.model.covars

        ## likelihood for the fully observed data 
        if(outcome_family$family == "gaussian"){
            sigma.y <- params[param.idx]
            param.idx <- param.idx + 1
            ll.y.obs <- dnorm(y.obs, outcome.params %*% t(outcome.model.matrix),sd=sigma.y, log=TRUE)
        }
        
        df.obs <- model.frame(proxy_formula,df)
        n.proxy.model.covars <- dim(proxy.model.matrix)[2]
        proxy.params <- params[param.idx:(n.proxy.model.covars+param.idx-1)]
        param.idx <- param.idx + n.proxy.model.covars
        proxy.obs <- with(df.obs, eval(parse(text=proxy.variable)))

        if( (proxy_family$family=="binomial") & (proxy_family$link=='logit')){
            ll.w.obs <- vector(mode='numeric',length=dim(proxy.model.matrix)[1])
            ll.w.obs[proxy.obs==1] <- plogis(proxy.params %*% t(proxy.model.matrix[proxy.obs==1,]),log=TRUE)
            ll.w.obs[proxy.obs==0] <- plogis(proxy.params %*% t(proxy.model.matrix[proxy.obs==0,]),log=TRUE, lower.tail=FALSE)
        }

        df.obs <- model.frame(truth_formula, df)
        truth.variable <- all.vars(truth_formula)[1]
        truth.obs <- with(df.obs, eval(parse(text=truth.variable)))
        truth.model.matrix <- model.matrix(truth_formula,df)
        n.truth.model.covars <- dim(truth.model.matrix)[2]
        
        truth.params <- params[param.idx:(n.truth.model.covars + param.idx - 1)]

        if( (truth_family$family=="binomial") & (truth_family$link=='logit')){
            ll.x.obs <- vector(mode='numeric',length=dim(truth.model.matrix)[1])
            ll.x.obs[truth.obs==1] <- plogis(truth.params %*% t(truth.model.matrix[truth.obs==1,]),log=TRUE)
            ll.x.obs[truth.obs==0] <- plogis(truth.params %*% t(truth.model.matrix[truth.obs==0,]),log=TRUE, lower.tail=FALSE)
        }
        
        ll.obs <- sum(ll.y.obs + ll.w.obs + ll.x.obs)

        ## likelihood for the predicted data
        ## integrate out the "truth" variable. 
        
        if(truth_family$family=='binomial'){
            df.unobs <- df[is.na(eval(parse(text=truth.variable)))]
            df.unobs.x1 <- copy(df.unobs)
            df.unobs.x1[,'x'] <- 1
            df.unobs.x0 <- copy(df.unobs)
            df.unobs.x0[,'x'] <- 0
            outcome.unobs <- with(df.unobs, eval(parse(text=response.var)))
            
            outcome.model.matrix.x0 <- model.matrix(outcome_formula, df.unobs.x0)
            outcome.model.matrix.x1 <- model.matrix(outcome_formula, df.unobs.x1)
            if(outcome_family$family=="gaussian"){
            ll.y.x0 <- dnorm(outcome.unobs, outcome.params %*% t(outcome.model.matrix.x0), sd=sigma.y, log=TRUE)
            ll.y.x1 <- dnorm(outcome.unobs, outcome.params %*% t(outcome.model.matrix.x1), sd=sigma.y, log=TRUE)
            }

            if( (proxy_family$family=='binomial') & (proxy_family$link=='logit')){

                proxy.model.matrix.x0 <- model.matrix(proxy_formula, df.unobs.x0)
                proxy.model.matrix.x1 <- model.matrix(proxy_formula, df.unobs.x1)
                proxy.unobs <- df.unobs[[proxy.variable]]
                ll.w.x0 <- vector(mode='numeric', length=dim(df.unobs)[1])
                ll.w.x1 <- vector(mode='numeric', length=dim(df.unobs)[1])

                ll.w.x0[proxy.unobs==1] <- plogis(proxy.params %*% t(proxy.model.matrix.x0[proxy.unobs==1,]), log=TRUE)
                ll.w.x1[proxy.unobs==1] <- plogis(proxy.params %*% t(proxy.model.matrix.x1[proxy.unobs==1,]), log=TRUE)

                ll.w.x0[proxy.unobs==0] <- plogis(proxy.params %*% t(proxy.model.matrix.x0[proxy.unobs==0,]), log=TRUE,lower.tail=FALSE)
                ll.w.x1[proxy.unobs==0] <- plogis(proxy.params %*% t(proxy.model.matrix.x1[proxy.unobs==0,]), log=TRUE,lower.tail=FALSE)
            }

            if(truth_family$link=='logit'){
                truth.model.matrix <- model.matrix(truth_formula, df.unobs.x0)
                ll.x.x0 <- plogis(truth.params %*% t(truth.model.matrix), log=TRUE)
                ll.x.x1 <- plogis(truth.params %*% t(truth.model.matrix), log=TRUE, lower.tail=FALSE)
            }
        }

        ll.x0 <- ll.y.x0 + ll.w.x0 + ll.x.x0
        ll.x1 <- ll.y.x1 + ll.w.x1 + ll.x.x1
        ll.unobs <- sum(colLogSumExps(rbind(ll.x0, ll.x1)))
        return(-(ll.unobs + ll.obs))
    }
    
    outcome.params <- colnames(model.matrix(outcome_formula,df))
    lower <- rep(-Inf, length(outcome.params))

    if(outcome_family$family=='gaussian'){
        params <- c(outcome.params, 'sigma_y')
        lower <- c(lower, 0.00001)
    } else {
        params <- outcome.params
    }
    
    proxy.params <- colnames(model.matrix(proxy_formula, df))
    params <- c(params, paste0('proxy_',proxy.params))
    lower <- c(lower, rep(-Inf, length(proxy.params)))

    truth.params <- colnames(model.matrix(truth_formula, df))
    params <- c(params, paste0('truth_', truth.params))
    lower <- c(lower, rep(-Inf, length(truth.params)))
    start <- rep(0.1,length(params))
    names(start) <- params
    
    fit <- optim(start, fn = measrr_mle_nll, lower=lower, method='L-BFGS-B', hessian=TRUE, control=list(maxit=1e6))

    return(fit)
}