# Write to matrix
robot_marker = "X"

# copy a full matrix row
function mat_copyrow(out,ro,in,ri){
    out[ro] = {}
    var icr = 1
    while(icr <= size(in[ri])) {
        out[ro][icr]=in[ri][icr]
        icr = icr + 1
    }
}

function getvec(t,row){
    return math.vec2.new(t[row][1],t[row][2])
}

function init_test_map(len){
    map = {}
    var indexR = 1
    while(indexR <= len) {
        map[indexR] = {}
        var indexC = 1
        while(indexC <= len) {
            map[indexR][indexC] = 1.0
            indexC = indexC + 1
        }
        indexR = indexR + 1
    }
    # DEBUG\TEST: puts an obstacle right in the middle
    map[5][5] = 0.0
    map[6][5] = 0.0
    map[4][5] = 0.0

    log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.")
}

function init_map(len){
    map = {}
    var indexR = 1
    while(indexR <= len) {
        map[indexR] = {}
        var indexC = 1
        while(indexC <= len) {
            map[indexR][indexC] = 1.0
            indexC = indexC + 1
        }
        indexR = indexR + 1
    }
    log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
}

function add_obstacle(pos, off, inc_trust) {
    var xi = math.round(pos.x)
    var yi = math.round(pos.y)

    if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
        # log("Add obstacle in cell: ", xi, " ", yi)
        var old=map[xi][yi]
        if(old-inc_trust > 0.0)
          map[xi][yi] = old-inc_trust
        else
          map[xi][yi] = 0.0
    }
}

function remove_obstacle(pos, off, dec_trust) {
    var xi = math.round(pos.x)
    var yi = math.round(pos.y)

    if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
        # log("Remove obstacle in cell: ", xi, " ", yi)
        var old=map[xi][yi]
        if(old + dec_trust < 1.0)  #x,y
          map[xi][yi] = old+dec_trust
        else
          map[xi][yi] = 1.0
    }
}


function get_occupied_cells(cur_cell){
  var i = 1
  var occupied_cells = {}
  occupied_cells[0] = cur_cell
  occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
  occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
  occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
  occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
  return occupied_cells
}

function is_in(dictionary, x, y){
  var i = 0
  while(i < size(dictionary)){
    if(dictionary[i].x == x and dictionary[i].y == y){
      return 1
    }
    i = i + 1
  }
  return 0
}

function print_pos(t) {
  var ir=1
  while(ir <= size(t)) {
      log(ir, ": ", t[ir][1], " ", t[ir][2])
      ir = ir + 1
  }
}

function print_map(t) {
  var ir=size(t)
  log("Printing a ", size(t), " by ", size(t[1]), " map")
  while(ir > 0) {
      logst=string.concat("\t", string.tostring(ir), "\t:")
      ic = size(t[ir])
      while(ic > 0) {
        if(ir==cur_cell.x and ic==cur_cell.y)
          logst = string.concat(logst, " XXXXXXXX")
        else
          logst = string.concat(logst, " ", string.tostring(t[ir][ic]))
        ic = ic - 1
      }
      log(logst)
      ir = ir - 1
  }
  export_map(map)
}