"""
Lab7a demonstrates a particle filter using the light cubes
and orientation-only sensor information.

In the particle viewer window:
   the WASD keys move the robot
  'e' forces an evaluation step
  'r' forces a resampling
  'v' displays the weight statistics
  'z' re-randomizes the particles.
"""

from cozmo_fsm import *
from cozmo.util import degrees, Pose


class Lab7a(StateMachineProgram):
    def __init__(self):
        landmarks = {
            cube1 : Pose( 55, 160, 0, angle_z=degrees(90)),
            cube2 : Pose(160,  55, 0, angle_z=degrees( 0)),
            cube3 : Pose(160, -55, 0, angle_z=degrees( 0))
        }
        pf = ParticleFilter(robot,
                            landmarks = landmarks,
                            sensor_model = CubeOrientSensorModel(robot))
        super().__init__(particle_filter=pf, particle_viewer=True)