Monte Carlo -self localization algorithm

;Montecarlo-selflocalization algorithm

globals [TheColor myxcor myycor totalP sensor_color i1 i2 lastx lasty diffX diffY]

patches-own [p mycolor]
turtles-own []

to setup
  
clear-all

ask patches[set pcolor one-of[green yellow brown blue] set p 1 / count(patches) set totalP sum[p]of patches set mycolor pcolor]  

ask one-of patches

[
  sprout 1[
    
    set shape "circle"
    set color red
    set size 0.8
    
  ]

]
end 

to moveup
  
  ask turtles
  [
    set ycor ycor + 1
  ]
  ask patches
  [
    set p (move-right * [p] of patch-at 0 (- 1)) + (1 - move-right) * p
  ]
  sense
end 

to movedown
  
  ask turtles
  [
    set ycor ycor - 1
  ]
  ask patches
  [
    set p (move-right * [p] of patch-at 0 1) + (1 - move-right) * p
  ]
  sense
end 

to moveright
  
  ask turtles
  [
    set xcor xcor + 1
  ]
  ask patches
  [
    set p (move-right * [p] of patch-at (- 1) 0) + (1 - move-right) * p
  ]
  sense
end 

to moveleft
  
  ask turtles
  [
    set xcor xcor - 1
  ]
  ask patches
  [
    set p (move-right * [p] of patch-at 1 0) + (1 - move-right) * p
  ]
  sense
end 

to sense
  
  ;ask turtles with [color = white][set sensor_color [mycolor] of patch-ahead 0]  
  ask turtles [ask patch-here [set sensor_color mycolor]]
    
  ask patches
  [
    ifelse mycolor = sensor_color
    [
      set p p * sensor-right
    ][
      set p p * (1 -  sensor-right)
    ]
  ]
  set totalP sum[p]of patches
    
  ask patches
  [
    set p p / totalP
  ]
 
  ask patches
  [
    set pcolor scale-color green p 0 light
  ]
end 

to move
  
  
ask turtles
[
  set i1 one-of [0 1 -1]
  set i2 one-of [0 1 -1]
  
  set xcor xcor + i1
  set ycor ycor + i2
]
  
ask patches [set p (move-right * [p] of patch-at (- i1) (- i2) + (1 -  move-right) * p)]

  
  ask patches
  [
    set pcolor scale-color green p 0 light
  ]
end 

to go

  move
    
  sense 
end 

to go1
  if mouse-down?
  [
    sense1
    move1
    
  ]
end 

to s_color
  
  ask patches [set pcolor mycolor]
end 

to move1
  


ask patch mouse-xcor mouse-ycor [set diffX (lastx - mouse-xcor) set diffY (lasty - mouse-ycor)] 

ask patches [set p (move-right * [p] of patch-at (diffX) (diffY) + (1 -  move-right) * p)]

ask patch mouse-xcor mouse-ycor [set lastx mouse-xcor set lasty mouse-ycor]
end 

to sense1
  
  ;ask turtles with [color = white][set sensor_color [mycolor] of patch-ahead 0]  
  ask patch mouse-xcor mouse-ycor [set sensor_color mycolor]
    
  ask patches
  [
    ifelse mycolor = sensor_color
    [
      set p p * sensor-right
    ][
      set p p * (1 -  sensor-right)
    ]
  ]
  set totalP sum[p]of patches
    
  ask patches
  [
    set p p / totalP
  ]
 
  ask patches
  [
    set pcolor scale-color green p 0 light
  ]
end