#####
# RRT* Path Planing
#
# map table-based matrix
#####
include "plan/mapmatrix.bzz"

RRT_TIMEOUT = 500
RRT_RUNSTEP = 3
PROX_SENSOR_THRESHOLD_M = 8.0
GSCALE = {.x=1, .y=1}
map = nil
cur_cell = {}
nei_cell = {}
mapgoal = {}


#
# Work in progress....
function navigate() {
  BVMSTATE="NAVIGATE"
	if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
		storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
		cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
		m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
	}

	print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
	if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
		log("Sorry this is too far.")
		return
	}

	# create the map
	if(map==nil) {
		dyn_init_map(cur_goal)
		if(V_TYPE == 0) {
			homegps.lat = pose.position.latitude
			homegps.long = pose.position.longitude
		}
	}
	
	if(Path==nil) {			
		# add proximity sensor and neighbor obstacles to the map
		populateGrid(m_pos)
		# start the planner
		path_it = 1
		pathPlanner(cur_goal, m_pos)
	} else if(path_it <= size(Path)) {
		var cur_path_pt = convert_pt(getvec(Path, path_it))
		print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
		if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
			populateGrid(m_pos)
			if(check_path(Path, path_it, m_pos, 0)) {
        # stop
				goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0)
				Path = nil
				if(V_TYPE == 0)
					cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
				# re-start the planner
				path_it = 1
				pathPlanner(cur_goal, m_pos)
			} else {
				cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
				goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0)
			}
		} else
			path_it = path_it + 1
	} else {
		Path = nil
  		BVMSTATE="IDLE"
	}
}

# create a map with its length to fit the goal (rel. pos.) and the current position as the center
# TODO: test with khepera/argos
function dyn_init_map(m_navigation) {
    # create a map big enough for the goal
    init_map(math.round(2*math.vec2.length(m_navigation))+10)
    # center the robot on the grid
    cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
}

# start the RRT* to reach the goal (rel. pos.)
# TODO: test with khepera/argos
function pathPlanner(m_goal, m_pos, kh4) {
    # place the robot on the grid
    cur_cell = getcell(m_pos)
    # create the goal in the map grid
    mapgoal = getcell(math.vec2.add(m_goal, m_pos))

    #print_map(map)
    export_map(map)

    # Initialize RRTStar var
    log("--------Initialize RRTStar--------------")
    HEIGHT = size(map)
    WIDTH = size(map[1])
    RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
    goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0}
    #table_print(goalBoundary)
    numberOfPoints = 1
    arrayOfPoints = {}
    arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
    # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
    Q = {}
    Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0}
    goalReached = 0
    timeout = 0

    # search for a path
    old_statef = statef
    rrtstar()
}

# get the grid cell position (x,y) equivalent to the input position
# input should be relative to home position (planing start point)
function getcell(m_curpos) {
  var cell = {}
  # relative to center (start position)
  cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
  var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y)
  cell = math.vec2.add(cell, offset)
  cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
  
  if(cell.x > size(map))
    cell.x = size(map)
  if(cell.y > size(map[1]))
      cell.y = size(map[1])
  if(cell.x < 1)
    cell.x = 1
  if(cell.y < 1)
    cell.y = 1

  return cell
}

function populateGrid(m_pos) {
    # getproxobs(m_pos)
    getneiobs (m_pos)
    export_map(map)
}

# TODO: populate the map with neighors as blobs instead ?
function getneiobs (m_curpos) {
    cur_cell = getcell(m_curpos)
    # add all neighbors as obstacles in the grid
    neighbors.foreach(function(rid, data) {
            add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
            })
}

# old function tested with the khepera for reference
# DS 20/11
# function getneiobs (m_curpos) {
#   var foundobstacle = 0
#   cur_cell = getcell(m_curpos)
#   var old_nei = table_copy(nei_cell)

#   neighbors.foreach(function(rid, data) {
#           Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
#           Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
#           Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
#           nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
#           if(old_nei[rid]!=nil) {
#             if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
#               remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
#               remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
#               remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
#               remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
#               remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
#               add_obstacle(Ncell, 0, 1.0)
#               add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
#               add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
#               add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
#               add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
#               foundobstacle = 1
#             }
#           } else {
#             add_obstacle(Ncell, 0, 1.0)
#             add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
#             add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
#             add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
#             add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
#             foundobstacle = 1
#           }
#         })
#   return foundobstacle
# }

# populate a line in front of the sensor using plateform independant proximity sensing
# TODO: adapt to M100 stereo
function getproxobs (m_curpos) {
  var updated_obstacle = 0
  cur_cell = getcell(m_curpos)

  reduce(proximity,
    function(key, value, acc) {
      if (value.angle != -1) {  # down sensor of M100
        if(value.value < PROX_SENSOR_THRESHOLD_M) {
          obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw)))
          per = math.vec2.sub(obs,cur_cell)
          obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
          obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
          obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
          obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
          # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs)))
          # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
          # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
          if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
            add_obstacle(obs, 0, 0.25)
            add_obstacle(obsr, 0, 0.25)
            add_obstacle(obsl, 0, 0.25)
            add_obstacle(obsr2, 0, 0.25)
            add_obstacle(obsl2, 0, 0.25)
            # add_obstacle(obs2, 0, 0.25)
            # add_obstacle(obsr2, 0, 0.25)
            # add_obstacle(obsl2, 0, 0.25)
            updated_obstacle = 1
          }
        } else {
          var line_length = PROX_SENSOR_THRESHOLD_M;
          while(line_length > 0) {
            obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw)))
            per = math.vec2.sub(obs,cur_cell)
            obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
            obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
            obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
            obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
            remove_obstacle(obs, 0, 0.05)
            remove_obstacle(obsr, 0, 0.05)
            remove_obstacle(obsl, 0, 0.05)
            line_length = line_length - 1;
          }
        }
      }
      return acc
  }, math.vec2.new(0, 0))

  return updated_obstacle
}			

# check if any obstacle blocks the way
# TODO: remove the kh4 bool for retro-compatilibty
function check_path(m_path, goal_l, m_curpos, kh4) {
  var pathisblocked = 0
  var nb = goal_l
  cur_cell = getcell(m_curpos)
  var Cvec = cur_cell
  while(nb <= size(m_path) and nb <= goal_l+1) {
    var Nvec = getvec(m_path, nb)
    if(kh4 == 0) {
        Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
        Nvec = getcell(Nvec)
    }
    if(doesItIntersect(Cvec, Nvec)) {
      log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
      pathisblocked = 1
    }
    Cvec = Nvec
    nb = nb + 1
  }

  return pathisblocked
}

##
# main search loop as a state
##
function rrtstar() {
    # update state machine variables
    BVMSTATE = "PATHPLAN"


    step_timeout = 0
    while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {

        # Point generation only as grid cell centers
        pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
        #log("Point generated: ", pt.x, " ", pt.y)

        var pointList = findPointsInRadius(pt,Q,RADIUS);

        # Find connection that provides the least cost to come
        nbCount = 0;
        minCounted = 999;
        minCounter = 0;

        if(size(pointList) != 0) {
            #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) 
            ipt=1
            while(ipt <= size(pointList)) {
                pointNumber = {}
                mat_copyrow(pointNumber,1,pointList,ipt)

                # Follow the line to see if it intersects anything
                intersects = doesItIntersect(pt,getvec(pointNumber,1));
                #log("intersects1: ", intersects)

                # If there is no intersection we need consider its connection
                nbCount = nbCount + 1;
                if(intersects != 1) {
                    #log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
                    var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5]

                    if(distance < minCounted) {
                        minCounted = distance;
                        minCounter = nbCount;
                    }
                }
                ipt = ipt + 1
            }
            if(minCounter > 0) {
                numberOfPoints = numberOfPoints + 1;
                arrayOfPoints[numberOfPoints] = {}
                arrayOfPoints[numberOfPoints][1]=pt.x
                arrayOfPoints[numberOfPoints][2]=pt.y

                Q[numberOfPoints] = {}
                Q[numberOfPoints][1] = pt.x
                Q[numberOfPoints][2] = pt.y
                Q[numberOfPoints][3] = pointList[minCounter][4]
                Q[numberOfPoints][4] = numberOfPoints
                Q[numberOfPoints][5] = minCounted

                #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)

                # Now check to see if any of the other points can be redirected
                nbCount = 0;
                ipt = 1
                while(ipt <= size(pointList)) {
                    pointNumber = {}
                    mat_copyrow(pointNumber,1,pointList,ipt)
                    #log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])

                    # Follow the line to see if it intersects anything
                    intersects = doesItIntersect(pt,getvec(pointNumber,1));
                    #log("intersects2: ", intersects)

                    # If there is no intersection we need consider its connection
                    nbCount = nbCount + 1;
                    if(intersects != 1) {
                        # If the alternative path is shorter than change it
                        tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
                        #log("Q: ", size(Q), "x", size(Q[1]))
                        if(tmpdistance < Q[pointNumber[1][4]][5]) {
                            Q[pointNumber[1][4]][3] = numberOfPoints
                            Q[pointNumber[1][4]][5] = tmpdistance
                        }
                    }
                    ipt = ipt + 1
                }

                # Check to see if this new point is within the goal
                if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
                    goalReached = 1;
            }
        } else {
            # Associate with the closest point
            pointNum = findClosestPoint(pt,arrayOfPoints);

            # Follow the line to see if it intersects anything
            intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum));
            #log("intersects3 (", pointNum, "): ", intersects)

            # If there is no intersection we need to add to the tree
            if(intersects != 1) {
                numberOfPoints = numberOfPoints + 1;
                arrayOfPoints[numberOfPoints] = {}
                arrayOfPoints[numberOfPoints][1]=pt.x
                arrayOfPoints[numberOfPoints][2]=pt.y

                Q[numberOfPoints] = {}
                Q[numberOfPoints][1] = pt.x
                Q[numberOfPoints][2] = pt.y
                Q[numberOfPoints][3] = pointNum
                Q[numberOfPoints][4] = numberOfPoints
                Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))

                #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)

                # Check to see if this new point is within the goal
                if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
                    goalReached = 1;
            }
        }
        if(numberOfPoints % 100 == 0) {
            log(numberOfPoints, " points processed. Still looking for goal.");
        }
        timeout = timeout + 1
        step_timeout = step_timeout + 1
    }

    if(goalReached){
      log("Goal found(",numberOfPoints,")!")
      Path = convert_path(getPath(Q,numberOfPoints))
      # done. resume the state machine
      BVMSTATE = "NAVIGATE"
    } else if(timeout >= RRT_TIMEOUT) {
      log("FAILED TO FIND A PATH!!!")
      Path = nil
      # done. resume the state machine
      BVMSTATE = "NAVIGATE"
    }
}

# Go through each points and find the distances between them and the
# target point
function findClosestPoint(point,aPt) {
    var distance = 999
    var pointNb = -1
    var ifcp=1
    while(ifcp <= size(aPt)) {
        var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))

        if(range < distance) {
            distance = range;
            pointNb = ifcp;
        }
        ifcp = ifcp + 1
    }
    return pointNb
}

# Find the closest point in the tree
function findPointsInRadius(point,q,r) {
    var ptList = {}
    var counted = 0;
    var iir = 1
    while(iir <= size(q)) {
        var tmpvv = getvec(q,iir)
        #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
        var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))

        if(distance < r) {
            counted = counted+1;
            mat_copyrow(ptList,counted,q,iir)
        }

        iir = iir + 1
    }
    return ptList
}

# check if the line between 2 point intersect an obstacle
function doesItIntersect(point,vector) {
    #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
    var dif = math.vec2.sub(point,vector)
    var distance = math.vec2.length(dif)
    if(distance == 0.0){
        # Find what block we're in right now
        var xi = math.round(point.x) #+1
        var yi = math.round(point.y) #+1
        if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
            if(map[xi][yi] > 0.5)
                return 1
            else
                return 0
        } else
            return 0
    }
    var vec = math.vec2.scale(dif,1.0/distance)
    var pathstep = size(map) - 2

    idii = 1.0
    while(idii <= pathstep) {
        var range = distance*idii/pathstep
        var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));

        # Find what block we're in right now
        var xi = math.round(pos_chk.x) #+1
        var yi = math.round(pos_chk.y) #+1
        #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")

        # TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross
        if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
        #if(xi!=cur_cell.x and yi!=cur_cell.y) {
          if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
              if(map[xi][yi] < 0.5) { # because obstacle use trust values
                  #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
                  return 1;
              }
          } else {
              #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
              return 1;
          }
        }
        idii = idii + 1.0
    }
    #log("No intersect!")
    return 0
}

# create a table with only the path's points in order
function getPath(Q,nb){
    var pathL={}
    var npt=0
    # get the path from the point list
    while(nb > 0) {
        npt=npt+1
        pathL[npt] = {}
        pathL[npt][1]=Q[nb][1]
        pathL[npt][2]=Q[nb][2]
        if( nb > 1) {
          if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening?
              log("ERROR - Recursive path !!!")
              return nil
          }
        }
        nb = Q[nb][3];
    }

    # Re-Order the list.
    var pathR={}
    var totpt = npt + 1
    while(npt > 0){
        pathR[totpt-npt] = {}
        var tmpgoal = getvec(pathL,npt)
        pathR[totpt-npt][1]=tmpgoal.x
        pathR[totpt-npt][2]=tmpgoal.y
        npt = npt - 1
    }
    #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
    return pathR
}