# R-code computing the lower estimate of the triangulation probability as described in
# Triangulation Properties of the Target Area in Wireless Sensor Networks 
# by Xiaoyun Li, David K. Hunter and Sergei Zuyev
# Author: Sergei Zuyev
# Last modified: 2008-08-18
#
# Typical use: source this file in R-session:
# source("triang-estim.R")
# To get estimate for a sequence of values from 0.1 to 1 by, say, 0.2
# of the density lambda of sensors, run
# est(seq(0.1,1,by=0.2))
# For a single value lambda=2, this is simply
# est(2)
# errors of the numeric evaluation of the multiple integral is also reported.
#
# For debugging purposes, two functions testGplus and testGminus are provided.
# They estimate the areas of G+ and G- by running N Monte-Carlo simulations.
# By default N=10000, so expect error of order 0.01. These should be compared
# with values given by functions Gplus and Gminus for different values of r0,r1 and phi1.
# E.g. compare
# testGminus(0.5,1,2)
# with
# Gminus(0.5,1,2)
#
# Enjoy! SZ

R1=function(r0,phi)
  { # polar equation of b((-r0,0),2)
    return(sqrt(4-r0^2*sin(phi)^2)-r0*cos(phi))
  }

zeta=function(r0)
  { # angle of intersection of b((-r0,0),2) with b(0,r0)
    if(r0>2/sqrt(3))
      stop("r0>2/sqrt(3)")
    ifelse(r0<=1,return(0),return(2*acos(1/r0)))
  }

I1=function(r0,phi)
  { # integral of 1/2 R1(phi)^2 d phi
    sf=sin(phi)
    cf=cos(phi)
    return(r0^2/2 * sf*cf + 2*phi - 2*asin(r0*sf/2) - sf*r0/2 * sqrt(4 - r0^2 + r0^2*cf^2))
  }

Gplus=function(r0,phi1)
  { # area of G+
    return(I1(r0,phi1)-I1(r0,zeta(r0)) - r0^2*(phi1-zeta(r0))/2)
  }


R2=function(r0,phi,r1,phi1)
  { # polar equation of b((r1,phi1),2)
    return(sqrt(4-r1^2*sin(phi-phi1)^2)+r1*cos(phi-phi1))
  }

I2=function(r1,phi)
  { # integral of 1/2 R2(phi)^2 d phi
    sf=sin(phi)
    cf=cos(phi)
    return(r1^2/2 * sf*cf + 2*phi + 2*asin(r1*sf/2) + sf*r1/2 * sqrt(4 - r1^2 + r1^2*cf^2))
  }

zeta1=function(r0,r1,phi1)
  { # angle of intersection of b((r1,phi1),2) with b(0,r0)
    # or phi1+pi if b(0,r0) \subset  b((r1,phi1),2) 
    if(r0>2/sqrt(3))
      stop("r0>2/sqrt(3)")
    ifelse(r0<=2-r1,return(phi1+pi),return(2*pi+phi1-acos((r0^2+r1^2-4)/(2*r0*r1)) ))
  }


zeta2=function(r0,r1,phi1)
  { # angle of and radius vector of intersection of b((-r0,0),2) with b((r1,phi1),2)
    if(r0>2/sqrt(3))
      stop("r0>2/sqrt(3)")
    if(r0==0 & r1==0)
      return(list(angle=phi1+3*pi/2,radius2=4))
    xa=-r0
    ya=0
    xb=r1*cos(phi1)
    yb=r1*sin(phi1)
    d2=(xb-xa)^2+(yb-ya)^2
    t=sqrt((4/d2-0.25))
    x=(xa+xb)/2+t*(yb-ya)
    y=(ya+yb)/2-t*(xb-xa)
    angle=ifelse(x==0,3*pi/2,atan(y/x))
    if(angle<0)
      angle=angle+2*pi
    radius2=x^2+y^2
    return(list(x=x,y=y,angle=angle,radius2=radius2))
  }


Gminus=function(r0,r1,phi1)
  { # area of G-
    if(r1>R1(r0,phi1))
      stop("r1>R1(r0,phi1)")
    z2=zeta2(r0,r1,phi1)
    if(z2$radius2<=r0^2)
      return(0)
    z0=zeta(r0)
    x0=r0*cos(z0)
    y0=-r0*sin(z0)
    if((x0-r1*cos(phi1))^2+(y0-r1*sin(phi1))^2>=4)
      return(0) # case like in testGminus(1.05,1.1,2):
                # intersection of b((-r0,0),2) with b(O,r0) is farther than 2 from (r1,phi1)
    if(z2$y>0)
      { # case like r0=0.5; r1=1; phi1=2
        z1=zeta1(r0,r1,phi1)
        return(I2(r1,2*pi-phi1)-I2(r1,z1-phi1)-r0^2*(2*pi-z1)/2)
      }
    else
      {
        z1=zeta1(r0,r1,phi1)
        int1=I2(r1,z2$angle-phi1)-I2(r1,z1-phi1)-r0^2*(z2$angle-z1)/2
        int2=I1(r0,2*pi-z0)-I1(r0,z2$angle)-r0^2*(2*pi-z0-z2$angle)/2
        return(int1+int2)
      }
  }

draw.circ=function(r,x0,y0,...)
  {
    tt=seq(0,2*pi,len=100)
    lines(r*cos(tt)+x0,r*sin(tt)+y0,...)
  }


testGplus=function(r0,phi1,N=10000,draw=TRUE)
  { # area of G+ by simulation
    phi=runif(N,0,pi)
    r=sqrt(4*runif(N))
    x=r*cos(phi)
    y=r*sin(phi)
    z=atan(y/(x-r0))
    z[x<r0]=pi+z[x<r0]
    hit=(x-r0)^2+y^2>r0^2 & z>zeta(r0) & z < phi1
    if(draw)
      {
        plot(c(-2,2),c(-2,2),ty="n",xlab="",ylab="")
        points(x,y,pch=20,col="grey")
        points(x[hit],y[hit],col="red",pch=20)
        draw.circ(2,0,0)
        draw.circ(r0,r0,0,col="blue")
        abline(h=0,col="yellow")
        points(0,0,col="blue",pch=19)
        points(r0,0,col="blue",pch=19)
        segments(r0,0,r0+r0*cos(zeta(r0)),r0*sin(zeta(r0)),col="green",lwd=2)
        segments(r0,0,r0+R1(r0,phi1)*cos(phi1),R1(r0,phi1)*sin(phi1),col="green",lwd=2)
      }
    return(sum(hit)/N*pi*2)
  }



testGminus=function(r0,r1,phi1,N=10000,draw=TRUE)
  { # area of G- by simulation
    phi=runif(N,pi,2*pi)
    r=sqrt(4*runif(N))
    x=r*cos(phi)
    y=r*sin(phi)
    z=atan(y/(x-r0))
    z[x<r0]=pi+z[x<r0]
    z[y<0 & x>=r0]=2*pi+z[y<0 & x>=r0]
    hit=(x-r0-r1*cos(phi1))^2+(y-r1*sin(phi1))^2<=4 & (x-r0)^2+y^2>r0^2 & z>phi1+pi
    if(draw)
      {
        plot(c(-2,2),c(-2,2),ty="n",xlab="",ylab="")
        points(x,y,pch=20,col="grey")
        points(x[hit],y[hit],col="red",pch=20)
        draw.circ(2,0,0)
        draw.circ(2,r0+r1*cos(phi1),r1*sin(phi1),col="blue")
        draw.circ(r0,r0,0,col="blue")
        abline(h=0,col="yellow")
        points(0,0,col="blue",pch=19)
        points(r0,0,col="blue",pch=19)
        segments(r0,0,r0+R1(r0,phi1)*cos(phi1),R1(r0,phi1)*sin(phi1),col="green",lwd=2)
        points(r0+r1*cos(phi1),r1*sin(phi1),col="blue",pch=19)
        segments(r0,0,r0+r0*cos(2*pi-zeta(r0)),r0*sin(2*pi-zeta(r0)),col="green",lwd=2)
        text(r0+r0*cos(2*pi-zeta(r0)),r0*sin(2*pi-zeta(r0)),labels="2pi-z",pos=4)
        segments(r0,0,r0+r0*cos(zeta1(r0,r1,phi1)),r0*sin(zeta1(r0,r1,phi1)),col="green",lwd=2)
        text(r0+r0*cos(zeta1(r0,r1,phi1)),r0*sin(zeta1(r0,r1,phi1)),labels="z1",pos=2)
        z2=zeta2(r0,r1,phi1)
        segments(r0,0,r0+sqrt(z2$radius2)*cos(z2$angle),sqrt(z2$radius2)*sin(z2$angle),col="green",lwd=2)
        text(r0+sqrt(z2$radius2)*cos(z2$angle),sqrt(z2$radius2)*sin(z2$angle),labels="z2",pos=4)
      }
    return(sum(hit)/N*pi*2)
  }

iiint=function(f,z1,z2,y1,y2,x1,x2)
  { # triple integral int_x1^x2 dx int_y1(x)^y2(x) dy int_z1(x,y)^z2(x,y) f(x,y,z) dz
    if(is.numeric(z1))
      {
        zz1=z1
        z1=function(u,v) zz1
      }
    if(is.numeric(z2))
      {
        zz2=z2
        z2=function(u,v) zz2
      }
    if(!(is.function(z1) & length(formals(z1))==2))
      stop("z1 should be either a constant or a function of 2 arguments!")
    if(!(is.function(z2) & length(formals(z2))==2))
      stop("z2 should be either a constant or a function of 2 arguments!")
    if(is.numeric(y1))
      {
        yy1=y1
        y1=function(t) yy1
      }
    if(is.numeric(y2))
      {
        yy2=y2
        y2=function(t) yy2
      }
    if(!(is.function(y1) & length(formals(y1))==1))
      stop("y1 should be either a constant or a function of 1 argument!")
    if(!(is.function(y2) & length(formals(y2))==1))
      stop("y2 should be either a constant or a function of 1 argument!")
    if(!(is.numeric(x1)))
       stop("x1 should be numeric!")
    if(!(is.numeric(x2)))
       stop("x2 should be numeric!")
    i1=function(x,y)
      {
        fxy=function(u) f(x,y,u)
        return(integrate(Vectorize(fxy),z1(x,y),z2(x,y))$value)
      }
    i2=function(x)
      {
        fx=function(u) i1(x,u)
        return(integrate(Vectorize(fx),y1(x),y2(x))$value)
      }
    res=integrate(Vectorize(i2),x1,x2)
    return(list(value=res$value,abs.error=res$abs.error,message=res$message))
  }




est=function(lam,details=FALSE)
  { # The main function providing the estimate
    R0=function(r0,phi1) r0
    res=vector()
    for(l in lam)
      {
        f=function(r0,phi1,r1)
          {
            r0*exp(-l*pi*r0^2-l*Gplus(r0,phi1))*(1-exp(-l*Gminus(r0,r1,phi1)))
          }
        a1=iiint(f,R0,R1,0,pi,0,1)
        a2=iiint(f,R0,R1,zeta,pi,1,2/sqrt(3))
        if(details)
          cat("lambda=",l,"a1=",a1$value,a1$message,"a2=",a2$value,a2$message,"\n")
        res=rbind(res,c(l,2*pi*l^2*(a1$value+a2$value),2*pi*l^2*(a1$abs.error+a2$abs.error)))
      }
    colnames(res)=c("lambda","tri.prob.est","abs.error")
    return(res)
  }