# Librairies nécessaire : plotrix

source(paste(dirname(sys.frame(1)$ofile),"/intersectionCercles.R",sep=""))
library("plotrix")

##############################
# Retourne la position estimée pour les différentes distance
# gwX = une matrice des coordonnées en X de chaque gateway (attention à l'odre avec gwY) exemple :
#       gwX1,gwX2,gwX3
# gwY = une matrice des coordonnées en Y de chaque gateway (attention à l'odre avec gwX)
# distances = une matrice, exemple pour trois gateway avec 4 points à trouvées :
#
#  d1gw1,d2gw1,d3gw1,d4gw1
#  d1gw2,d2gw2,d3gw2,d4gw2
#  d1gw3,d2gw3,d3gw3,d4gw3
#
# stepByStep = si a TRUE alors affiche la multilatération pour chaque points
# precisionPlot = nombre d'unité en X et en Y lors de l'affichage de la multilatération pour chaque points
##############################
multilateration=function(gwX,gwY,distances,stepByStep=FALSE,precisionPlot=80){

  # Check parameters
  if(!is.vector(gwX)){
    stop("gwX is not a vector")
  }
  else if(!is.vector(gwY)){
    stop("gwY is not a vector")
  }
  else if(!is.matrix(distances)){
    stop("distances is not a matrix")
  }
  else if(length(gwX)!=length(gwY)){
    stop("gwX and gwY haven't the same length")
  }
  else if(NROW(distances)!= length(gwX)){
    stop("Number of rows of distances have not the same length of the gwX and gwY")
  }
  
  # Init convenience variables
  nbGw=length(gwX);
  nbDistances=NCOL(distances)
  
  # Get the middle point of a segment
  getMiddleOfSegment=function(x1,y1,x2,y2){
    x=(x1+x2)/2;
    y=(y1+y2)/2;
    return(c(x,y));
  }
  
  # Get line equation from two of his points y=ax+b
  getLineEquation=function(x1,y1,x2,y2){
    eq=NULL;
    if(x1!=x2){
      a=(y1-y2)/(x1-x2);
      b=y1-a*x1;
      eq=c(a,b);
    }
    return(eq);
  }
  
  # Get line equation of 2 circles intersections points
  getIntersectionLineEquation=function(circlesIntersections){
    if(NROW(circlesIntersections)>1){
      lineEquation=getLineEquation(
        circlesIntersections[1,1],circlesIntersections[1,2],
        circlesIntersections[2,1],circlesIntersections[2,2]);
      if(is.null(lineEquation)){
        return(circlesIntersections[1,1]);
      }
      return(lineEquation);
    }
    return(NULL);
  }
  
  # Build solution for each distances
  sol=NULL;
  for(i in 1:nbDistances){
    linearLines=NULL;
    xLines=NULL;
    currentSol=NULL;
    circlesIntersections=NULL;
    
    # Build lines equation for linearLines and xLines
    for(j in 1:(nbGw-1)){
      mainGw=c(gwX[j],gwY[j]);
      for(k in (j+1):nbGw){
        slaveGw=c(gwX[k],gwY[k])
        currentCirclesIntersections=getIntersection(mainGw[1],mainGw[2],distances[j,i],slaveGw[1],slaveGw[2],distances[k,i]);
        circlesIntersections=rbind(circlesIntersections,currentCirclesIntersections);
        lineEquation=getIntersectionLineEquation(currentCirclesIntersections)
        if(length(lineEquation)==1){
          xLines=rbind(xLines,lineEquation)
        }
        else{
          linearLines=rbind(linearLines,lineEquation)
        }
      }
    }

    # Build lines intersections
    intersections=NULL;
    if(NROW(linearLines)>0||length(xLines)>0){
      # Get intersections with xLines
      sapply(xLines,function(x){
        if(NROW(linearLines)>0){
          apply(linearLines,1,function(eq){
            intersections<<-rbind(intersections,c(x,eq[1]*x+eq[2]))
          });
        }
       
      });
      
     # Get linearLines intersections
      if(NROW(linearLines)>1){
        for(j in 1:(NROW(linearLines)-1)){
          mainLL=c(linearLines[j,1],linearLines[j,2]);
          for(k in (j+1):(NROW(linearLines))){
            slaveLL=c(linearLines[k,1],linearLines[k,2]);
            toSolve=matrix(c(-mainLL[1],-slaveLL[1],1,1), ncol=2,nrow=2);
            tryCatch({
              solution=solve(toSolve,c(mainLL[2],slaveLL[2]))
              intersections=rbind(intersections,solution)
            },error=function(error){});
           
          }
        }
      }
    }
    
    # Build solution, if we have intersections
    if(NROW(intersections)>0){
      currentSol=c(mean(intersections[,1]),mean(intersections[,2]));
      sol=cbind(sol,currentSol);
    }
    # If we don't have intersections (middle of segment)
    else{
      if(NROW(linearLines)>0||length(xLines)>0){
        currentSol=getMiddleOfSegment(
          circlesIntersections[1,1],circlesIntersections[1,2],
          circlesIntersections[2,1],circlesIntersections[2,2]
          );
       # sol=cbind(sol,currentSol);
      }
    }

    # If we plot current solution
    if(stepByStep){
      # Plot gateways
      plot(gwX,gwY,asp=1,xlim=c(-precisionPlot,precisionPlot),ylim=c(-precisionPlot,precisionPlot),pch=17, col="orange")
      # Plot circles
      for(j in 1:nbGw){
        draw.circle(gwX[j],gwY[j],distances[j,i],lwd=0.5);
      }
      # Plot circles intersections
      if(!is.null(circlesIntersections)){
        apply(circlesIntersections,1,function(row){
          lines(row[1],row[2],type="p",col="blue",pch=20)
        });
      }
      if(!is.null(intersections)){
        apply(intersections,1,function(row){
          lines(row[1],row[2],type="p",col="black",pch=20)
        });
      }
      if(!is.null(linearLines)){
        apply(linearLines,1,function(row){
          abline(row[2],row[1]);
        });
      }
      
      if(!is.null(xLines)){
        sapply(xLines,function(x){
          abline(v=x);
        });
      }
      if(!is.null(sol)){
        lines(sol[1],sol[2],type="p",col="red", pch=16)
      }
     
      readline(prompt = "Press enter for next step...")
    }
    
  }
  return(sol);
}

##############################
# Multilateration avec optimisation de la taille des cercles (prendre le plus petit rayon possible)
# radiusStep = pas de réduction de la taille du rayon
##############################
optimizeRadius=function(gwX,gwY,distances,stepByStep=FALSE,precisionPlot=80, radiusStep=1,zeroForNull=FALSE){
  sol=NULL;
  for(i in 1:NCOL(distances)){
    factor=0
    curDistances=as.matrix(distances[,i])-factor
    # Try to find solution without optimisation (factor=0)
    estimated=multilateration(gwX,gwY,curDistances,stepByStep=stepByStep,precisionPlot=precisionPlot)
    # Repeat multilateration until radius = 0
    while(sum(curDistances<=0)!=NROW(gwX)){
      factor=factor+radiusStep
      curDistances=curDistances-factor # Reduce circles radius
      curDistances[curDistances<0]<-0 # Put zero in negative radius
      e=multilateration(gwX,gwY,curDistances,stepByStep=stepByStep,precisionPlot=precisionPlot) # Temporary solution
      if(!is.null(e)){ # A better solution is found
        estimated=e
      }
      else if(!is.null(estimated)){ # No other solution is possible
        break;
      }
    }
    if(zeroForNull && is.null(estimated)){
        sol=cbind(sol,c(0,0))
      
    }
    else{
      if(!is.null(estimated)){
        sol=cbind(sol,estimated)
      }
    }
  }
  return(sol)
}