dyn.load("BivTrunc.so")

BivTrunc <- function(X,Y,xlim,ylim,mask,lambdax=NULL,lambday=NULL,tau=0,deg=1,datwght=NULL,verbose=F,grdsize=20,reductfactor=5)
{

# Scale X and Y to lie in (0,1)

   X2 = (X-xlim[1])/(xlim[2]-xlim[1])
   Y2 = (Y-ylim[1])/(ylim[2]-ylim[1])


# Here, mask is on the "finer" size

   resx2 = dim(mask)[[1]]
   resy2 = dim(mask)[[2]]

#  subseq = seq(3,res,by=5)
# reductfactor gives the value of resx2/resx and resy2/resy.
# It needs to be an odd integer.

   if(reductfactor %% 2 == 0)
   {
      stop("reductfactor needs to be an odd integer.")
   }
   if(resx2 %% reductfactor != 0)
   {
      print(resx2)
      stop("reductfactor needs to divide resx2")
   }
   if(resy2 %% reductfactor != 0)
   {
      stop("reductfactor needs to divide resy2")
   }

   subseqx = seq((reductfactor+1)/2,resx2,by=reductfactor)
   subseqy = seq((reductfactor+1)/2,resy2,by=reductfactor)


# mask2 is on the "reduced" size

   mask2 = mask[subseqx,subseqy]

   resx = length(subseqx)
   resy = length(subseqy)

   xseqbivest = xlim[1] + (0:(resx-1))/resx*(xlim[2]-xlim[1]) + (xlim[2]-xlim[1])/(2*resx)
   yseqbivest = ylim[1] + (0:(resy-1))/resy*(ylim[2]-ylim[1]) + (ylim[2]-ylim[1])/(2*resy)

   xseqcov = xlim[1] + (0:(grdsize-1))/grdsize*(xlim[2]-xlim[1]) + (xlim[2]-xlim[1])/(2*grdsize)
   yseqcov = ylim[1] + (0:(grdsize-1))/grdsize*(ylim[2]-ylim[1]) + (ylim[2]-ylim[1])/(2*grdsize)


# tau can be passed in as a fraction or as a count

   if(tau < 1)
   {
      tau = ceiling(tau*length(X))
   }

   if(is.null(datwght))
   {
      datwght = rep(1,length(X))
   }

   if(length(lambdax) == 1)
   {
      lambdaxorig = rep(lambdax,resx)
   }
   if(length(lambday) == 1)
   {
      lambdayorig = rep(lambday,resy)
   }

   if(length(lambdax) == resx)
   {
      lambdaxorig = lambdax
   }
   if(length(lambday) == resy)
   {
      lambdayorig = lambday
   }

   if(length(lambdax) == resx2)
   {
      lambdaxorig = lambdax[subseqx]
   }
   if(length(lambday) == resy2)
   {
      lambdayorig = lambday[subseqy]
   }

   bivest = matrix(0,nrow=resx,ncol=resy)
   grdcov = matrix(0,nrow=grdsize^2,grdsize^2)
   grdest = matrix(0,nrow=grdsize,grdsize)
   lvout = matrix(0,nrow=length(X),ncol=1)
   fits = matrix(0,nrow=length(X),ncol=1)
   fpr = matrix(0,nrow=resx,ncol=1)
   gpr = matrix(0,nrow=resy,ncol=1)
   maxiters1 = 20000
   maxiters2 = 20000
   info = 0
   theta = 0.0
   setheta = 0.0

   lambdaxused = rep(0,resx)
   lambdayused = rep(0,resy)

   if(is.null(lambdax))
   {
      lambdaxused = pmax(inflatelambda(resx,xlim,X,tau),2/resx)
   } else
   {
      lambdaxused = pmax(inflatelambda(resx,xlim,X,tau),lambdaxorig)
   }

   if(is.null(lambday))
   {
      lambdayused = pmax(inflatelambda(resy,ylim,Y,tau),2/resy)
   } else
   {
      lambdayused = pmax(inflatelambda(resy,ylim,Y,tau),lambdayorig)
   }

   lscv = 0.0
   likecv = 0.0
   likelihood = 0.0

   holdres <- .Fortran("bivtrunc",
                       as.double(cbind(X2,Y2)),
                       as.integer(length(X)),
                       as.integer(resx),
                       as.integer(resy),
                       as.integer(deg),
                       as.double(cbind(c(0,1),c(0,1))),
                       as.double(lambdaxused),
                       as.double(lambdayused),
                       as.logical(mask2),
                       bivest = as.double(bivest),
                       as.integer(maxiters1),
                       as.integer(maxiters2),
                       grdcov = as.double(grdcov),
                       grdest = as.double(grdest),
                       as.integer(grdsize),
                       lvout = as.double(lvout),
                       fits = as.double(fits),
                       as.logical(verbose),
                       info = as.integer(info),
                       lscv = as.double(lscv),
                       likecv = as.double(likecv),
                       likelihood = as.double(likelihood),
                       as.double(datwght),
                       as.logical(mask),
                       as.integer(resx2),
                       as.integer(resy2),
                       fpr = as.double(fpr),
                       gpr = as.double(gpr),
                       theta = as.double(theta),
                       setheta = as.double(setheta)
                 )

      scl = (ylim[2]-ylim[1])*(xlim[2]-xlim[1])

      lvout = holdres$lvout/scl
      fits = holdres$fits/scl
      bivest = exp(matrix(holdres$bivest,nrow=resx))
      theta = holdres$theta/scl
      setheta = holdres$setheta/scl
      lscv = holdres$lscv/scl/scl

      normcnst = scl

      bivest = bivest/normcnst
      grdcov = matrix(holdres$grdcov,nrow=grdsize^2)/normcnst/normcnst
      grdest = matrix(holdres$grdest,nrow=grdsize)/normcnst

      margx = apply(bivest,1,sum)*(ylim[2]-ylim[1])/resy
      margy = apply(bivest,2,sum)*(xlim[2]-xlim[1])/resx
      grdmargx = apply(grdest,1,sum)*(ylim[2]-ylim[1])/grdsize
      grdmargy = apply(grdest,2,sum)*(xlim[2]-xlim[1])/grdsize

      grdcov = grdcov + t(grdcov) - diag(diag(grdcov))

      list(bivest=bivest,margx=margx,margy=margy,xseqbivest=xseqbivest,
           yseqbivest=yseqbivest,
           fits=fits,lscv=lscv,likecv=holdres$likecv,
           likelihood=holdres$likelihood,grdest=grdest,grdcov=grdcov,xseqcov=xseqcov,yseqcov=yseqcov,
           lvout=lvout,xlim=xlim,
           ylim=ylim,grdmargx=grdmargx,grdmargy=grdmargy,datwght=datwght,X=X,Y=Y,
           lambdaxused=lambdaxused,lambdayused=lambdayused,lambdaxorig=lambdaxorig,lambdayorig=lambdayorig,
           fpr=holdres$fpr,gpr=holdres$gpr,theta=theta,setheta=setheta)
}



makebivarplot <- function(bivest,xlim,ylim,xvarname="X",yvarname="Y")
{
   res <- dim(bivest$bivest)[[2]]
   contour(seq(xlim[1],xlim[2],length=res),seq(ylim[1],ylim[2],length=res),bivest$bivest,col=2,xlab=xvarname,ylab=yvarname)
}


inflatelambda = function(res,lim,dat,mincnt)
{
  if(mincnt==0)
  {
    out = 0
  } else
  { 
     datseq = lim[1] + (lim[2]-lim[1])*((0:(res-1))/res) + (lim[2]-lim[1])/res/2
     minlam = rep(0,res)
     for(i in 1:res)
     {
        hold = sort(abs(dat - datseq[i]))
        minlam[i] = hold[mincnt]
     } 
     out = minlam/(lim[2]-lim[1])
  }
  out
}



inflatelambda2 = function(res,lim,dat,minrat,gooddat,minlam,datwght)
{
  out = rep(0,length(res))
  if(minrat==0)
  {
    out = minlam
  } else
  { 
     datseq = lim[1] + (lim[2]-lim[1])*((0:(res-1))/res) + (lim[2]-lim[1])/res/2
     for(i in 1:res)
     {
        hold = sort(abs(dat - datseq[i]),index.return=T)
        hold$x = hold$x/(lim[2]-lim[1])
        hold2 = gooddat[hold$ix]
        datwghtsrt = datwght[hold$ix]
        curpt = max(c((1:length(hold$x))[hold$x < minlam[i]],1))
        while(sum(hold2[1:curpt])/curpt < minrat & curpt < length(dat))
#       while(sum(datwghtsrt[1:curpt][hold2[1:curpt]])/sum(datwghtsrt[1:curpt]) < minrat & curpt < length(dat))
        {
           curpt = curpt + 1
        }
        print(curpt) 
        out[i] = max(hold$x[curpt],minlam[i])
     } 
  }
  out
}








plotconditional = function(bivest,marg,val,varnames=NULL,semult=1.0,trans=NULL,transtext="",reverse=FALSE,normalize=T,add=F,lcol=1,ltype=1,ylimits=NULL,lwd=3,shft=0.0,xlimits=NULL,plotaxes=TRUE,errbars=TRUE,errcol=rgb(0.5,0.5,0.75))
{
   if(is.null(trans))
   {
      trans = function(x){x}
   }

   res = dim(bivest$bivest)[[1]] 
   grdsize = dim(bivest$grdest)[[1]]
   if(marg == 1) 
   {
      lims = bivest$xlim
      closest1 = round((val-bivest$ylim[1])/(bivest$ylim[2]-bivest$ylim[1])*(res-1)) + 1
      closest2 = round((val-bivest$ylim[1])/(bivest$ylim[2]-bivest$ylim[1])*(grdsize-1)) + 1
   } else
   {
      lims = bivest$ylim
      closest1 = round((val-bivest$xlim[1])/(bivest$xlim[2]-bivest$xlim[1])*(res-1)) + 1
      closest2 = round((val-bivest$xlim[1])/(bivest$xlim[2]-bivest$xlim[1])*(grdsize-1)) + 1
   }
   if(!is.null(xlimits))
   {
      lims = xlimits
   }

   if(is.null(varnames))
   {
      varnames = c("X","Y")
   } 
   if(!plotaxes)
   {
      varnames = c("","")
   }

   if(marg == 1) 
   {
      condest1 = bivest$bivest[,closest1]
   } else
   {
      condest1 = bivest$bivest[closest1,]
   }
   if(normalize)
   {  
      normconst = (sum(condest1)/res)*(lims[2]-lims[1])
      condest1 = condest1/normconst
   }
   grdmat = matrix(0,nrow=grdsize^2,ncol=grdsize^2)
   if(marg == 1)
   {
      for(x in 1:grdsize)
      {
         lo = (x-1)*grdsize + 1
         if(normalize)
         {
            grdmat[x,lo+closest2-1] = 1/normconst
         } else {
            grdmat[x,lo+closest2-1] = 1.0
         }
      }
   }
   if(marg == 2)
   {
      lo = (closest2-1)*grdsize
      for(y in 1:grdsize)
      {
         if(normalize)
         {
            grdmat[y,lo+y] = 1/normconst
         } else 
         {
            grdmat[y,lo+y] = 1.0
         }
      }
   }
   xlist = round((1:grdsize - 1)*(length(condest1)-1)/(grdsize-1))+1
   condest = condest1[xlist]
   condcov = diag(grdmat %*% bivest$grdcov %*% t(grdmat))
   scl = (lims[2]-lims[1])/100
   up = rep(0,grdsize)
   dn = rep(0,grdsize)
   for(i in 1:grdsize)
   {
      one = trans(condest[i] + semult*sqrt(condcov[i]))
      two = trans(condest[i] - semult*sqrt(condcov[i]))
      dn[i] = min(one,two)
      up[i] = max(one,two)
   }
   if(marg == 1)
   {
      ylabel = paste(transtext," Conditional Density at ",varnames[2]," = ",val,sep="")
   } else
   {
      ylabel = paste(transtext, " Conditional Density at ",varnames[1]," = ",val,sep="")
   }
   if(!plotaxes)
   {
      ylabel=""
   }
   if(add)
   {
         lines(seq(lims[1],lims[2],length=length(condest1)),trans(condest1)+shft,type="l",
            lwd=lwd,col=lcol,lty=ltype)
   } else
   {
      if(reverse)
      {
         if(is.null(ylimits))
         {
            plot(seq(lims[1],lims[2],length=length(condest1)),trans(condest1)+shft,type="l",xlab=varnames[marg],
               ylab=ylabel,lwd=lwd,col=lcol,ylim=c(min(dn[is.finite(dn)]),max(up))
               ,xlim=c(lims[2],lims[1]),lty=ltype,axes=plotaxes)
         } else
         {
            plot(seq(lims[1],lims[2],length=length(condest1)),trans(condest1)+shft,type="l",xlab=varnames[marg],
               ylab=ylabel,lwd=lwd,col=lcol,ylim=ylimits
               ,xlim=c(lims[2],lims[1]),lty=ltype,axes=plotaxes)
         }
      } else {
         if(is.null(ylimits))
         {
            plot(seq(lims[1],lims[2],length=length(condest1)),trans(condest1)+shft,type="l",xlab=varnames[marg],
               ylab=ylabel,lwd=lwd,col=lcol,ylim=c(min(dn[is.finite(dn)]),max(up)),lty=ltype)
         } else
         {
            plot(seq(lims[1],lims[2],length=length(condest1)),trans(condest1)+shft,type="l",xlab=varnames[marg],
               ylab=ylabel,lwd=lwd,col=lcol,ylim=ylimits,lty=ltype)
         }
      }
   }

   if(errbars)
   {
   dn[is.infinite(dn)] = -9999999

   up = up + shft
   dn = dn + shft
   for(i in 1:grdsize)
   {
      xpos = (xlist[i]-1)/(res-1)*(lims[2]-lims[1])+ lims[1]
      lines(c(xpos,xpos),c(dn[i],up[i]),lwd=lwd-1,col=errcol)
      lines(c(xpos-scl,xpos+scl),c(dn[i],dn[i]),lwd=lwd-1,col=errcol)
      lines(c(xpos-scl,xpos+scl),c(up[i],up[i]),lwd=lwd-1,col=errcol)
   }
   }
}

plotmarginal = function(bivest,marg,varname=NULL,semult=1.0,trans=NULL,reverse=FALSE,transtext="",add=F,cex=1,shft=0,shft2=0,errcol="blue",linecol="red",ylabel=NULL)
{
   res = dim(bivest$bivest)[[1]]
   if(is.null(trans))
   {
      trans = function(x){x}
   }

   if(marg == 1) 
   {
      lims = bivest$xlim
      othscl = (bivest$ylim[2]-bivest$ylim[1])
      plotseq = selectest$xseqbivest
      plotseq2 = selectest$xseqcov
   } else
   {
      lims = bivest$ylim
      othscl = (bivest$xlim[2]-bivest$xlim[1])
      plotseq = selectest$yseqbivest
      plotseq2 = selectest$yseqcov
   }

   if(is.null(varname))
   {
      if(marg==1)
      {
         varname = "X"
      } else
      {
         varname = "Y"
      }
   }
  
   margest1 = apply(bivest$bivest,marg,sum) 
   margest1 = margest1 * othscl/res
   grdsize = dim(bivest$grdest)[[1]]
   grdmat = matrix(0,nrow=grdsize^2,ncol=grdsize^2)
   if(marg == 1)
   {
      for(x in 1:grdsize)
      {
         lo = (x-1)*grdsize + 1
         hi = x*grdsize
         grdmat[x,lo:hi] = othscl/grdsize
      }
   }
   if(marg == 2)
   {
      xseq = grdsize*(0:(grdsize-1))
      for(y in 1:grdsize)
      {
         grdmat[y,xseq+y] = othscl/grdsize
      }
   }

   if(add)
   {
      typ = 2
   } else
   {
      typ = 1
   }

#  margest = margest1[round((1:grdsize - 1)*(length(margest1)-1)/(grdsize-1))+1]
   margcov = diag(grdmat %*% bivest$grdcov %*% t(grdmat))
   scl = (lims[2]-lims[1])/100
   up = rep(0,grdsize)
   dn = rep(0,grdsize)
   margest = rep(0,grdsize)
   for(i in 1:grdsize)
   {
      margest[i] = margest1[which.min(abs(plotseq2[i]-plotseq))]
#     xpos = lims[1] + (i-1)/(grdsize-1)*(lims[2]-lims[1])
      xpos = plotseq2[i]
      one = trans(margest[i] + semult*sqrt(margcov[i]))
      two = trans(margest[i] - semult*sqrt(margcov[i]))
      dn[i] = min(one,two)+shft2[i]
      up[i] = max(one,two)+shft2[i]
   }
   if(is.null(ylabel))
   {
      ylabel = paste(transtext,"Marginal Density")
   }
   if(!add)
   {
      if(reverse)
      {
         plot(plotseq,trans(margest1)+shft,type="l",xlab=varname,ylab=ylabel,lwd=3,col=linecol,ylim=c(min(dn),max(up)) ,xlim=c(lims[2],lims[1]),
           cex.axis=cex,cex.lab=cex)
      } else {
         plot(plotseq,trans(margest1)+shft,type="l",xlab=varname,ylab=ylabel,lwd=3,col=linecol,ylim=c(min(dn),max(up)),cex.axis=cex, cex.lab=cex)
      }
   } else
   {
      if(reverse)
      {
         points(plotseq,trans(margest1)+shft,type="l",lwd=3,col=linecol,lty=2)
      } else {
         points(plotseq,trans(margest1)+shft,type="l",lwd=3,col=linecol,lty=2)
      }
   }


   if(!is.null(errcol))
   {
   for(i in 1:grdsize)
   {
#     xpos = lims[1] + (i-1)/(grdsize-1)*(lims[2]-lims[1])
      xpos = plotseq2[i]
      if(add)
      {
#        xpos = xpos + (lims[2]-lims[1])/grdsize/5.0
      }
      lines(c(xpos,xpos),c(dn[i],up[i]),lwd=2,col=errcol,lty=typ)
      lines(c(xpos-scl,xpos+scl),c(dn[i],dn[i]),lwd=2,col=errcol)
      lines(c(xpos-scl,xpos+scl),c(up[i],up[i]),lwd=2,col=errcol)
   }
   }
}