#!/usr/bin/python

from numpy import random, pi, zeros

import robot, particles, map, pose
import time  # sleep

import std_sensors, std_noise

import sys
sys.path.append('..')
import mapping.map_class
sys.modules['map_class'] = mapping.map_class  # deal with the fact we pickled a module in another dir
import mapping.pickler

# blocks status + map = walls 
#def run( start_x, start_y, start_theta, ipc_channel = None, shared_data = {}, map_data = None ):
def run( bot_loc, blocks, map_properties, course_map, ipc_channel, bot_state ):
  
  start_pose = pose.Pose(bot_loc["x"],bot_loc["y"],bot_loc["theta"])
  #start_pose = pose.Pose(start_x,start_y,start_theta)
  ideal = robot.SimRobot(start_pose, std_sensors.default)

  themap = map.Map.from_map_class(course_map)

  if not ipc_channel:
    print "Using stub IPC"
    ipc_channel = Fake_IPC(start_pose, themap, delay = 1.0)

  print "Start: ", start_pose
  print

  localizer = DumbLocalizer(start_pose)
  #localizer = particles.ParticleLocalizer(std_sensors.default, std_noise.noise_params, map_data, pcount=1000)

  while True:
    msg = ipc_channel.get()
    turn, move = msg['turn'], msg['move']
    print "Message: Turn: %+0.2f, Move: %0.2f" % (turn, move)
    ideal.move(turn, move)
    localizer.move(turn, move)
    print "Ideal: ", ideal.pose
    localizer.update(msg['sensors'])
    guess = localizer.guess()
    print "Guess: ", guess
    print
    bot_loc['x'] = guess.x
    bot_loc['y'] = guess.y
    bot_loc['theta'] = guess.theta

#################################
class Fake_IPC(object):
  def __init__(self, start_pose, map_data, delay = 0.0):
    self.delay = delay
    self.map = map_data
    self.simbot = robot.SimRobot(pose = start_pose, sensors = std_sensors.default, 
                            noise_params = std_noise.noise_params)

  def get(self):
    print "Robot: ", self.simbot.pose
    turn = random.random() * pi - pi/2
    move = random.random() 
    self.simbot.move(turn,move)
    measured = self.simbot.sense(self.map)
    # %todo: x, y, theta -> dx, dy, dtheta
    msg = {'turn': turn, 'move': move, 'sensors': measured}
    time.sleep(self.delay)
    return msg

#################################

class DumbLocalizer(object):
  def __init__(self, start_pose):
    self.r = robot.SimRobot(start_pose)
  def move(self, turn, move):
    self.r.move(turn, move)
  def update(self, sensors):
    pass
  def guess(self):
    return self.r.pose

#################################

if __name__ == '__main__':

  #m = map.Map('maps/test3.map')
  map_obj = mapping.pickler.unpickle_map('../mapping/map.pkl')

  bot_loc = {'x': 6.0, 'y': 2.6, 'theta':pi/2}
  blocks = {}
  map_props = {}
  bot_state = {}

  run(bot_loc, blocks, map_props, map_obj, None, bot_state)
  #def run( bot_loc, blocks, map_properties, course_map, ipc_channel, bot_state ):


@@ -1128,20 +1128,22 @@
  = msg%5B'
-turn
+dTheta
 '%5D, msg%5B
@@ -1143,20 +1143,51 @@
 %5D, msg%5B'
-move
+dXY'%5D%0A    sensors = msg%5B'sensorData
 '%5D%0A    p
@@ -1239,24 +1239,55 @@
 turn, move)%0A
+    print %22Sensors: %22, sensors%0A
     ideal.mo
@@ -1389,22 +1389,15 @@
 ate(
-msg%5B'
 sensors
-'%5D
 )%0A  
@@ -1893,19 +1893,20 @@
  print %22
-Rob
+SimB
 ot: %22, s
@@ -1987,16 +1987,19 @@
 andom() 
+* 2
 %0A    sel
@@ -2121,20 +2121,22 @@
 msg = %7B'
-turn
+dTheta
 ': turn,
@@ -2137,20 +2137,19 @@
  turn, '
-move
+dXY
 ': move,
@@ -2156,17 +2156,20 @@
  'sensor
-s
+Data
 ': measu
@@ -2171,16 +2171,35 @@
 measured
+, 'timestamp': None
 %7D%0A    ti

use correct IPC element names