class Simulator(object):
    def __init__(
        self,
        percentObsDensity=20,
        endTime=40,
        nonRandomWorld=False,
        circleRadius=0.7,
        worldScale=1.0,
        autoInitialize=True,
        verbose=True,
    ):
        self.verbose = verbose
        self.startSimTime = time.time()
        self.collisionThreshold = 0.4
        self.randomSeed = 5
        self.Sensor_rayLength = 8

        self.percentObsDensity = percentObsDensity
        self.defaultControllerTime = 1000
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale

        # create the visualizer object
        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()

        if autoInitialize:
            self.initialize()

        self.XVelocity_drawing = 0.0
        self.YVelocity_drawing = 0.0

    def initializeOptions(self):
        self.options = dict()

        self.options["World"] = dict()
        self.options["World"]["obstaclesInnerFraction"] = 0.98
        self.options["World"]["randomSeed"] = 40
        self.options["World"]["percentObsDensity"] = 0.0
        self.options["World"]["nonRandomWorld"] = True
        self.options["World"]["circleRadius"] = 1.0
        self.options["World"]["scale"] = 1

        self.options["Sensor"] = dict()
        self.options["Sensor"]["rayLength"] = 10
        self.options["Sensor"]["numRays"] = 50

        self.options["Car"] = dict()
        self.options["Car"]["velocity"] = 4.0

        self.options["dt"] = 0.05

        self.options["runTime"] = dict()
        self.options["runTime"]["defaultControllerTime"] = 100

    def setDefaultOptions(self):

        defaultOptions = dict()

        defaultOptions["World"] = dict()
        defaultOptions["World"]["obstaclesInnerFraction"] = 0.98
        defaultOptions["World"]["randomSeed"] = 40
        defaultOptions["World"]["percentObsDensity"] = 30
        defaultOptions["World"]["nonRandomWorld"] = True
        defaultOptions["World"]["circleRadius"] = 1.75
        defaultOptions["World"]["scale"] = 2.5

        defaultOptions["Sensor"] = dict()
        defaultOptions["Sensor"]["rayLength"] = 10
        defaultOptions["Sensor"]["numRays"] = 51

        defaultOptions["Car"] = dict()
        defaultOptions["Car"]["velocity"] = 20

        defaultOptions["dt"] = 0.05

        defaultOptions["runTime"] = dict()
        defaultOptions["runTime"]["defaultControllerTime"] = 100

        for k in defaultOptions:
            self.options.setdefault(k, defaultOptions[k])

        for k in defaultOptions:
            if not isinstance(defaultOptions[k], dict):
                continue

            for j in defaultOptions[k]:
                self.options[k].setdefault(j, defaultOptions[k][j])

    def initializeColorMap(self):
        self.colorMap = dict()
        self.colorMap["default"] = [0, 1, 0]

    def initialize(self):

        self.dt = self.options["dt"]
        self.controllerTypeOrder = ["default"]

        self.setDefaultOptions()

        self.Sensor = SensorObj(
            rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
        )

        self.SensorApproximator = SensorApproximatorObj(
            numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"]
        )

        self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)

        self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])

        self.Controller.initializeVelocity(self.Car.v)

        self.ActionSet = ActionSetObj()

        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName("world"))
        self.world = World.buildLineSegmentTestWorld(
            percentObsDensity=self.options["World"]["percentObsDensity"],
            circleRadius=self.options["World"]["circleRadius"],
            nonRandom=self.options["World"]["nonRandomWorld"],
            scale=self.options["World"]["scale"],
            randomSeed=self.options["World"]["randomSeed"],
            obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
        )

        om.removeFromObjectModel(om.findObjectByName("robot"))
        self.robot, self.frame = World.buildRobot()

        self.frame = self.robot.getChildFrame()
        self.frame.setProperty("Scale", 3)
        # self.frame.setProperty('Visible', False)
        # self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]

        self.Car.setFrame(self.frame)
        print "Finished initialization"

    def runSingleSimulation(self, controllerType="default", simulationCutoff=None):

        # self.setRandomCollisionFreeInitialState()
        self.setInitialStateAtZero()

        currentCarState = np.copy(self.Car.state)
        nextCarState = np.copy(self.Car.state)
        self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])

        firstRaycast = self.Sensor.raycastAll(self.frame)
        firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)

        # self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
        # self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
        # self.Sensor.setLocator(self.LineSegmentLocator)

        nextRaycast = np.zeros(self.Sensor.numRays)

        # record the reward data
        runData = dict()
        startIdx = self.counter

        thisRunIndex = 0
        NMaxSteps = 100

        while self.counter < self.numTimesteps - 1:
            thisRunIndex += 1
            idx = self.counter
            currentTime = self.t[idx]
            self.stateOverTime[idx, :] = currentCarState
            x = self.stateOverTime[idx, 0]
            y = self.stateOverTime[idx, 1]

            self.setRobotFrameState(x, y, 0.0)
            # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
            currentRaycast = self.Sensor.raycastAll(self.frame)
            self.raycastData[idx, :] = currentRaycast
            S_current = (currentCarState, currentRaycast)

            if controllerType not in self.colorMap.keys():
                print
                raise ValueError("controller of type " + controllerType + " not supported")

            if controllerType in ["default", "defaultRandom"]:
                controlInput, controlInputIdx = self.Controller.computeControlInput(
                    currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False
                )

            self.controlInputData[idx] = controlInput

            nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
            print "NEXTCARSTATE is ", nextCarState

            x = nextCarState[0]
            y = nextCarState[1]

            self.setRobotFrameState(x, y, 0.0)
            nextRaycast = self.Sensor.raycastAll(self.frame)

            # Compute the next control input
            S_next = (nextCarState, nextRaycast)

            if controllerType in ["default", "defaultRandom"]:
                nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
                    nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False
                )

            # bookkeeping
            currentCarState = nextCarState
            currentRaycast = nextRaycast
            self.counter += 1

            # break if we are in collision
            if self.checkInCollision(nextRaycast):
                if self.verbose:
                    print "Had a collision, terminating simulation"
                break

            if thisRunIndex > NMaxSteps:
                print "was safe during N steps"
                break

            if self.counter >= simulationCutoff:
                break

        # fill in the last state by hand
        self.stateOverTime[self.counter, :] = currentCarState
        self.raycastData[self.counter, :] = currentRaycast

        # this just makes sure we don't get stuck in an infinite loop.
        if startIdx == self.counter:
            self.counter += 1

        return runData

    def setNumpyRandomSeed(self, seed=1):
        np.random.seed(seed)

    def runBatchSimulation(self, endTime=None, dt=0.05):

        self.controllerTypeOrder = ["default"]
        self.counter = 0
        self.simulationData = []

        self.initializeStatusBar()

        self.idxDict = dict()
        numRunsCounter = 0

        self.idxDict["default"] = self.counter
        loopStartIdx = self.counter
        simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps)

        while (self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1:
            self.printStatusBar()
            startIdx = self.counter
            runData = self.runSingleSimulation(controllerType="default", simulationCutoff=simCutoff)
            runData["startIdx"] = startIdx
            runData["controllerType"] = "default"
            runData["duration"] = self.counter - runData["startIdx"]
            runData["endIdx"] = self.counter
            runData["runNumber"] = numRunsCounter
            numRunsCounter += 1
            self.simulationData.append(runData)

        # BOOKKEEPING
        # truncate stateOverTime, raycastData, controlInputs to be the correct size
        self.numTimesteps = self.counter + 1
        self.stateOverTime = self.stateOverTime[0 : self.counter + 1, :]
        self.raycastData = self.raycastData[0 : self.counter + 1, :]
        self.controlInputData = self.controlInputData[0 : self.counter + 1]
        self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime

    def initializeStatusBar(self):
        self.numTicks = 10
        self.nextTickComplete = 1.0 / float(self.numTicks)
        self.nextTickIdx = 1
        print "Simulation percentage complete: (", self.numTicks, " # is complete)"

    def printStatusBar(self):
        fractionDone = float(self.counter) / float(self.numTimesteps)
        if fractionDone > self.nextTickComplete:

            self.nextTickIdx += 1
            self.nextTickComplete += 1.0 / self.numTicks

            timeSoFar = time.time() - self.startSimTime
            estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
            estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0

            print "#" * self.nextTickIdx, "-" * (
                self.numTicks - self.nextTickIdx
            ), "estimated", estimatedTimeLeft_minutes, "minutes left"

    def setCollisionFreeInitialState(self):
        tol = 5

        while True:

            x = 0.0
            y = -5.0
            theta = 0  # + np.random.uniform(0,2*np.pi,1)[0] * 0.01

            self.Car.setCarState(x, y, 0.0, 0.0)
            self.setRobotFrameState(x, y, theta)

            print "In loop"

            if not self.checkInCollision():
                break

        return x, y, theta

    def setInitialStateAtZero(self):

        x = 0.0
        y = 0.0
        theta = 0.0

        self.Car.setCarState(x, y, 0.0, 0.0)
        self.setRobotFrameState(x, y, theta)

        return x, y, theta

    def setRandomCollisionFreeInitialState(self):
        tol = 5

        while True:

            x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0]
            y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0]
            theta = np.random.uniform(0, 2 * np.pi, 1)[0]

            self.Car.setCarState(x, y, theta)
            self.setRobotFrameState(x, y, theta)

            if not self.checkInCollision():
                break

        return x, y, theta

    def setupPlayback(self):

        self.timer = TimerCallback(targetFps=30)
        self.timer.callback = self.tick

        playButtonFps = 1.0 / self.dt
        print "playButtonFPS", playButtonFps
        self.playTimer = TimerCallback(targetFps=playButtonFps)
        self.playTimer.callback = self.playTimerCallback
        self.sliderMovedByPlayTimer = False

        panel = QtGui.QWidget()
        l = QtGui.QHBoxLayout(panel)

        self.max_velocity = 20.0

        sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged)
        sliderXVelocity.setMaximum(self.max_velocity)
        sliderXVelocity.setMinimum(-self.max_velocity)

        l.addWidget(sliderXVelocity)

        sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged)
        sliderYVelocity.setMaximum(self.max_velocity)
        sliderYVelocity.setMinimum(-self.max_velocity)
        l.addWidget(sliderYVelocity)

        firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
        self.drawFirstIntersections(self.frame, firstRaycast)

        randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal")
        randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton)
        l.addWidget(randomGlobalGoalButton)

        randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles")
        randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton)
        l.addWidget(randomObstaclesButton)

        buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World")
        buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles)
        l.addWidget(buildWorldFromRandomObstaclesButton)

        findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)")
        findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton)
        l.addWidget(findLocalGoalButton)

        drawActionSetButton = QtGui.QPushButton("Draw Action Set")
        drawActionSetButton.connect("clicked()", self.onDrawActionSetButton)
        l.addWidget(drawActionSetButton)

        runSimButton = QtGui.QPushButton("Simulate")
        runSimButton.connect("clicked()", self.onRunSimButton)
        l.addWidget(runSimButton)

        playButton = QtGui.QPushButton("Play/Pause")
        playButton.connect("clicked()", self.onPlayButton)

        slider = QtGui.QSlider(QtCore.Qt.Horizontal)
        slider.connect("valueChanged(int)", self.onSliderChanged)
        self.sliderMax = self.numTimesteps
        slider.setMaximum(self.sliderMax)
        self.slider = slider

        # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider4.setMaximum(self.sliderMax)
        # l.addWidget(slider4)

        # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider5.setMaximum(self.sliderMax)
        # l.addWidget(slider5)

        # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider5.setMaximum(self.sliderMax)
        # l.addWidget(slider5)

        # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider6.setMaximum(self.sliderMax)
        # l.addWidget(slider6)

        # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider7.setMaximum(self.sliderMax)
        # l.addWidget(slider7)

        l.addWidget(playButton)
        l.addWidget(slider)

        w = QtGui.QWidget()
        l = QtGui.QVBoxLayout(w)
        l.addWidget(self.view)
        self.view.orientationMarkerWidget().Off()
        l.addWidget(panel)
        w.showMaximized()

        self.frame.connectFrameModified(self.updateDrawIntersection)
        self.updateDrawIntersection(self.frame)

        applogic.resetCamera(viewDirection=[0.2, 0, -1])
        self.view.showMaximized()
        self.view.raise_()
        panel = screengrabberpanel.ScreenGrabberPanel(self.view)
        panel.widget.show()

        cameracontrolpanel.CameraControlPanel(self.view).widget.show()

        elapsed = time.time() - self.startSimTime
        simRate = self.counter / elapsed
        print "Total run time", elapsed
        print "Ticks (Hz)", simRate
        print "Number of steps taken", self.counter
        self.onRandomObstaclesButton()
        self.app.start()

    def drawFirstIntersections(self, frame, firstRaycast):
        origin = np.array(frame.transform.GetPosition())
        d = DebugData()

        firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)

        for i in xrange(self.Sensor.numRays):
            endpoint = firstRaycastLocations[i, :]

            if firstRaycast[i] >= self.Sensor.rayLength - 0.1:
                d.addLine(origin, endpoint, color=[0, 1, 0])
            else:
                d.addLine(origin, endpoint, color=[1, 0, 0])

        vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")

        self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
        self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
        self.locator = self.LineSegmentLocator
        self.Sensor.setLocator(self.LineSegmentLocator)

    def updateDrawIntersection(self, frame, locator="None"):
        if locator == "None":
            locator = self.locator

        origin = np.array(frame.transform.GetPosition())
        # print "origin is now at", origin
        d = DebugData()

        sliderIdx = self.slider.value

        controllerType = self.getControllerTypeFromCounter(sliderIdx)
        colorMaxRange = self.colorMap[controllerType]

        for i in xrange(self.Sensor.numRays):
            ray = self.Sensor.rays[:, i]
            rayTransformed = np.array(frame.transform.TransformNormal(ray))
            # print "rayTransformed is", rayTransformed
            intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength)

            if intersection is not None:
                d.addLine(origin, intersection, color=[1, 0, 0])
            else:
                d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)

        vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")

        # camera = self.view.camera()
        # camera.SetFocalPoint(frame.transform.GetPosition())
        # camera.SetPosition(frame.transform.TransformPoint((-30,0,10)))

    def getControllerTypeFromCounter(self, counter):
        name = self.controllerTypeOrder[0]

        for controllerType in self.controllerTypeOrder[1:]:
            if counter >= self.idxDict[controllerType]:
                name = controllerType

        return name

    def setRobotFrameState(self, x, y, theta):
        t = vtk.vtkTransform()
        t.Translate(x, y, 0.0)
        t.RotateZ(np.degrees(theta))
        self.robot.getChildFrame().copyFrame(t)

    # returns true if we are in collision
    def checkInCollision(self, raycastDistance=None):
        if raycastDistance is None:
            self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2])
            raycastDistance = self.Sensor.raycastAll(self.frame)

        if np.min(raycastDistance) < self.collisionThreshold:
            return True
        else:
            return False

        # if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold:
        #     return True
        # else:
        #     return False

    def tick(self):
        # print timer.elapsed
        # simulate(t.elapsed)
        x = np.sin(time.time())
        y = np.cos(time.time())
        self.setRobotFrameState(x, y, 0.0)
        if (time.time() - self.playTime) > self.endTime:
            self.playTimer.stop()

    def tick2(self):
        newtime = time.time() - self.playTime
        print time.time() - self.playTime
        x = np.sin(newtime)
        y = np.cos(newtime)
        self.setRobotFrameState(x, y, 0.0)

    # just increment the slider, stop the timer if we get to the end
    def playTimerCallback(self):
        self.sliderMovedByPlayTimer = True
        currentIdx = self.slider.value
        nextIdx = currentIdx + 1
        self.slider.setSliderPosition(nextIdx)
        if currentIdx >= self.sliderMax:
            print "reached end of tape, stopping playTime"
            self.playTimer.stop()

    def onSliderChanged(self, value):
        if not self.sliderMovedByPlayTimer:
            self.playTimer.stop()
        numSteps = len(self.stateOverTime)
        idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax)))
        idx = min(idx, numSteps - 1)
        x, y, xdot, ydot = self.stateOverTime[idx]
        self.setRobotFrameState(x, y, 0)
        self.sliderMovedByPlayTimer = False

    def onXVelocityChanged(self, value):

        self.XVelocity_drawing = value
        self.onDrawActionSetButton()
        print "x velocity changed to ", value

    def onYVelocityChanged(self, value):
        print value

        self.YVelocity_drawing = -value
        self.onDrawActionSetButton()
        print "y velocity changed to ", -value

    def onShowSensorsButton(self):
        print "I pressed the show sensors button"
        self.setInitialStateAtZero()
        firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
        self.drawFirstIntersections(self.frame, firstRaycast)

    def onRandomObstaclesButton(self):
        print "random obstacles button pressed"
        self.setInitialStateAtZero()
        self.world = World.buildLineSegmentTestWorld(
            percentObsDensity=8.0,
            circleRadius=self.options["World"]["circleRadius"],
            nonRandom=False,
            scale=self.options["World"]["scale"],
            randomSeed=self.options["World"]["randomSeed"],
            obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
        )

        self.locator = World.buildCellLocator(self.world.visObj.polyData)
        self.Sensor.setLocator(self.locator)
        self.updateDrawIntersection(self.frame, locator=self.locator)

    def onRandomGlobalGoalButton(self):
        print "random global goal button pressed"
        self.globalGoal = World.buildGlobalGoal()

    def onBuildWorldFromRandomObstacles(self):
        distances = self.Sensor.raycastAll(self.frame)
        firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances)

        self.polygon_initial_distances = distances
        self.polygon_initial_raycastLocations = firstRaycastLocations

        self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
        self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
        self.locator = self.LineSegmentLocator
        self.Sensor.setLocator(self.LineSegmentLocator)
        self.updateDrawIntersection(self.frame)

    def onFindLocalGoalButton(self):
        print "find local goal button pressed"

        local_goal = self.findLocalGoal()
        print local_goal, "is my local goal"

        self.localGoal = World.placeLocalGoal(local_goal)

    def findLocalGoal(self):
        biggest_gap_width = 0
        biggest_gap_start_index = 0

        current_gap_width = 0
        current_gap_start_index = 0

        for index, value in enumerate(self.polygon_initial_distances):

            # if still in a gap
            if value == self.Sensor.rayLength:
                current_gap_width += 1

            # else terminate counting for this gap
            else:
                if current_gap_width > biggest_gap_width:
                    biggest_gap_width = current_gap_width
                    biggest_gap_start_index = current_gap_start_index

                current_gap_width = 0
                current_gap_start_index = index + 1

        if current_gap_width > biggest_gap_width:
            biggest_gap_width = current_gap_width
            biggest_gap_start_index = current_gap_start_index

        middle_index_of_gap = biggest_gap_start_index + biggest_gap_width / 2

        return self.polygon_initial_raycastLocations[middle_index_of_gap, :]

    def onDrawActionSetButton(self):
        print "drawing action set"
        # self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
        self.ActionSet.computeAllPositions(self.XVelocity_drawing, self.YVelocity_drawing)
        # self.ActionSet.drawActionSetFinal()
        self.ActionSet.drawActionSetFull()

    def onRunSimButton(self):
        self.runBatchSimulation()
        self.saveToFile("latest")

    def onPlayButton(self):

        if self.playTimer.isActive():
            self.onPauseButton()
            return

        print "play"
        self.playTimer.start()
        self.playTime = time.time()

    def onPauseButton(self):
        print "pause"
        self.playTimer.stop()

    def saveToFile(self, filename):

        # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime

        filename = "data/" + filename + ".out"
        my_shelf = shelve.open(filename, "n")

        my_shelf["options"] = self.options

        my_shelf["simulationData"] = self.simulationData
        my_shelf["stateOverTime"] = self.stateOverTime
        my_shelf["raycastData"] = self.raycastData
        my_shelf["controlInputData"] = self.controlInputData
        my_shelf["numTimesteps"] = self.numTimesteps
        my_shelf["idxDict"] = self.idxDict
        my_shelf["counter"] = self.counter
        my_shelf.close()

    def run(self, launchApp=True):
        self.counter = 1

        # for use in playback
        self.dt = self.options["dt"]
        self.endTime = self.defaultControllerTime  # used to be the sum of the other times as well

        self.t = np.arange(0.0, self.endTime, self.dt)
        maxNumTimesteps = np.size(self.t)

        self.stateOverTime = np.zeros((maxNumTimesteps + 1, 4))
        self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
        self.controlInputData = np.zeros((maxNumTimesteps + 1, 2))
        self.numTimesteps = maxNumTimesteps

        # self.runBatchSimulation()

        if launchApp:
            self.setupPlayback()

    @staticmethod
    def loadFromFile(filename):
        filename = "data/" + filename + ".out"
        sim = Simulator(autoInitialize=False, verbose=False)

        my_shelf = shelve.open(filename)
        sim.options = my_shelf["options"]
        sim.initialize()

        sim.simulationData = my_shelf["simulationData"]
        sim.stateOverTime = np.array(my_shelf["stateOverTime"])
        sim.raycastData = np.array(my_shelf["raycastData"])
        sim.controlInputData = np.array(my_shelf["controlInputData"])
        sim.numTimesteps = my_shelf["numTimesteps"]
        sim.idxDict = my_shelf["idxDict"]
        sim.counter = my_shelf["counter"]

        my_shelf.close()

        return sim
class Simulator(object):


    def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False,
                 circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True):
        self.verbose = verbose
        self.startSimTime = time.time()
        self.collisionThreshold = 0.4
        self.randomSeed = 5

        self.percentObsDensity = percentObsDensity
        self.defaultControllerTime = 1000
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale

        # create the visualizer object
        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()
        
        if autoInitialize:
            self.initialize()

        self.XVelocity_drawing = 0.0
        self.YVelocity_drawing = 0.0

    def initializeOptions(self):
        self.options = dict()

        self.options['World'] = dict()
        self.options['World']['obstaclesInnerFraction'] = 0.98
        self.options['World']['randomSeed'] = 40
        self.options['World']['percentObsDensity'] = 0.0
        self.options['World']['nonRandomWorld'] = True
        self.options['World']['circleRadius'] = 1.0
        self.options['World']['scale'] = 1


        self.options['Car'] = dict()
        self.options['Car']['velocity'] = 4.0

        self.options['dt'] = 0.05

        self.options['runTime'] = dict()
        self.options['runTime']['defaultControllerTime'] = 100


    def setDefaultOptions(self):

        defaultOptions = dict()


        defaultOptions['World'] = dict()
        defaultOptions['World']['obstaclesInnerFraction'] = 0.98
        defaultOptions['World']['randomSeed'] = 40
        defaultOptions['World']['percentObsDensity'] = 30
        defaultOptions['World']['nonRandomWorld'] = True
        defaultOptions['World']['circleRadius'] = 1.75
        defaultOptions['World']['scale'] = 2.5


        defaultOptions['Car'] = dict()
        defaultOptions['Car']['velocity'] = 20

        defaultOptions['dt'] = 0.05


        defaultOptions['runTime'] = dict()
        defaultOptions['runTime']['defaultControllerTime'] = 100


        for k in defaultOptions:
            self.options.setdefault(k, defaultOptions[k])


        for k in defaultOptions:
            if not isinstance(defaultOptions[k], dict):
                continue

            for j in defaultOptions[k]:
                self.options[k].setdefault(j, defaultOptions[k][j])


    def initializeColorMap(self):
        self.colorMap = dict()
        self.colorMap['default'] = [0,1,0]

    def initialize(self):

        self.dt = self.options['dt']
        self.controllerTypeOrder = ['default']

        self.setDefaultOptions()


        self.Controller = ControllerObj()

        self.Car = CarPlant(controller=self.Controller,
                            velocity=self.options['Car']['velocity'])

        self.Controller.initializeVelocity(self.Car.v)

        self.ActionSet = ActionSetObj()



        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName('world'))
        self.world = World.buildLineSegmentTestWorld(percentObsDensity=self.options['World']['percentObsDensity'],
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=self.options['World']['nonRandomWorld'],
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])

        om.removeFromObjectModel(om.findObjectByName('robot'))
        self.robot, self.frame = World.buildRobot()
        

        self.frame = self.robot.getChildFrame()
        self.frame.setProperty('Scale', 3)
        #self.frame.setProperty('Visible', False)
        #self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options['runTime']['defaultControllerTime']

        self.Car.setFrame(self.frame)
        print "Finished initialization"


    def runSingleSimulation(self, controllerType='default', simulationCutoff=None):


        #self.setRandomCollisionFreeInitialState()
        self.setInitialStateAtZero()

        currentCarState = np.copy(self.Car.state)
        nextCarState = np.copy(self.Car.state)
        self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
        
        # DELETED FIRST RAYCAST AND RAYCAST LOCATIONS



        # record the reward data
        runData = dict()
        startIdx = self.counter

        thisRunIndex = 0
        NMaxSteps = 100

        while (self.counter < self.numTimesteps - 1):
            thisRunIndex += 1
            idx = self.counter
            currentTime = self.t[idx]
            self.stateOverTime[idx,:] = currentCarState
            x = self.stateOverTime[idx,0]
            y = self.stateOverTime[idx,1]
            
            self.setRobotFrameState(x,y,0.0)
            # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])



            if controllerType in ["default"]:
                controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState,
                                                                            currentTime, self.frame,
                                                                            randomize=False)

            self.controlInputData[idx] = controlInput

            nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
            print "NEXTCARSTATE is ", nextCarState

        
            x = nextCarState[0]
            y = nextCarState[1]
            
            self.setRobotFrameState(x,y,0.0)


            if controllerType in ["default", "defaultRandom"]:
                nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState,
                                                                            currentTime, self.frame,
                                                                            randomize=False)


            #bookkeeping
            self.counter+=1

            # break if we are in collision


            if self.counter >= simulationCutoff:
                break


        # fill in the last state by hand
        self.stateOverTime[self.counter,:] = currentCarState


        # this just makes sure we don't get stuck in an infinite loop.
        if startIdx == self.counter:
            self.counter += 1

        return runData

    def setNumpyRandomSeed(self, seed=1):
        np.random.seed(seed)

    def runBatchSimulation(self, endTime=None, dt=0.05):

        
        self.controllerTypeOrder = ['default']
        self.counter = 0
        self.simulationData = []
    
        self.initializeStatusBar()

        self.idxDict = dict()
        numRunsCounter = 0


        self.idxDict['default'] = self.counter
        loopStartIdx = self.counter
        simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps)
        
        while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1):
            self.printStatusBar()
            startIdx = self.counter
            runData = self.runSingleSimulation(controllerType='default',
                                               simulationCutoff=simCutoff)
            runData['startIdx'] = startIdx
            runData['controllerType'] = "default"
            runData['duration'] = self.counter - runData['startIdx']
            runData['endIdx'] = self.counter
            runData['runNumber'] = numRunsCounter
            numRunsCounter+=1
            self.simulationData.append(runData)

        # BOOKKEEPING
        # truncate stateOverTime, raycastData, controlInputs to be the correct size
        self.numTimesteps = self.counter + 1
        self.stateOverTime = self.stateOverTime[0:self.counter+1, :]
        self.controlInputData = self.controlInputData[0:self.counter+1]
        self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime




    def initializeStatusBar(self):
        self.numTicks = 10
        self.nextTickComplete = 1.0 / float(self.numTicks)
        self.nextTickIdx = 1
        print "Simulation percentage complete: (", self.numTicks, " # is complete)"
    
    def printStatusBar(self):
        fractionDone = float(self.counter) / float(self.numTimesteps)
        if fractionDone > self.nextTickComplete:

            self.nextTickIdx += 1
            self.nextTickComplete += 1.0 / self.numTicks

            timeSoFar = time.time() - self.startSimTime 
            estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
            estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0

            print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left"


    def setCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = 0.0
            y =   -5.0
            theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01
            
            self.Car.setCarState(x,y,0.0,0.0)
            self.setRobotFrameState(x,y,theta)

            print "In loop"

            if not self.checkInCollision():
                break
                
        return x,y,theta


    
    def setInitialStateAtZero(self):
        
        x = 0.0
        y = 0.0
        theta = 0.0
        
        self.Car.setCarState(x,y,0.0,0.0)
        self.setRobotFrameState(x,y,theta)

        return x,y,theta



    def setRandomCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0]
            y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0]
            theta = np.random.uniform(0,2*np.pi,1)[0]
            
            self.Car.setCarState(x,y,theta)
            self.setRobotFrameState(x,y,theta)

            if not self.checkInCollision():
                break

        return x,y,theta

    def setupPlayback(self):

        self.timer = TimerCallback(targetFps=30)
        self.timer.callback = self.tick

        playButtonFps = 1.0/self.dt
        print "playButtonFPS", playButtonFps
        self.playTimer = TimerCallback(targetFps=playButtonFps)
        self.playTimer.callback = self.playTimerCallback
        self.sliderMovedByPlayTimer = False

        panel = QtGui.QWidget()
        l = QtGui.QHBoxLayout(panel)

        self.max_velocity = 20.0

        sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
        sliderXVelocity.setMaximum(self.max_velocity)
        sliderXVelocity.setMinimum(-self.max_velocity)

        l.addWidget(sliderXVelocity)

        sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
        sliderYVelocity.setMaximum(self.max_velocity)
        sliderYVelocity.setMinimum(-self.max_velocity)
        l.addWidget(sliderYVelocity)


        randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
        randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
        l.addWidget(randomGlobalGoalButton)

        randomObstaclesButton = QtGui.QPushButton('Initialize Random Obstacles')
        randomObstaclesButton.connect('clicked()', self.onRandomObstaclesButton)
        l.addWidget(randomObstaclesButton)

        drawActionSetButton = QtGui.QPushButton('Draw Action Set')
        drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
        l.addWidget(drawActionSetButton)

        runSimButton = QtGui.QPushButton('Simulate')
        runSimButton.connect('clicked()', self.onRunSimButton)
        l.addWidget(runSimButton)

        playButton = QtGui.QPushButton('Play/Pause')
        playButton.connect('clicked()', self.onPlayButton)

        slider = QtGui.QSlider(QtCore.Qt.Horizontal)
        slider.connect('valueChanged(int)', self.onSliderChanged)
        self.sliderMax = self.numTimesteps
        slider.setMaximum(self.sliderMax)
        self.slider = slider

        

        # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider4.setMaximum(self.sliderMax)
        # l.addWidget(slider4)

        # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider5.setMaximum(self.sliderMax)
        # l.addWidget(slider5)

        # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider5.setMaximum(self.sliderMax)
        # l.addWidget(slider5)

        # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider6.setMaximum(self.sliderMax)
        # l.addWidget(slider6)

        # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal)
        # slider7.setMaximum(self.sliderMax)
        # l.addWidget(slider7)



        l.addWidget(playButton)
        l.addWidget(slider)

        w = QtGui.QWidget()
        l = QtGui.QVBoxLayout(w)
        l.addWidget(self.view)
        self.view.orientationMarkerWidget().Off()
        l.addWidget(panel)
        w.showMaximized()

        
        # We need to connectFrameModified with updateDrawActionSet
        # self.frame.connectFrameModified(self.updateDrawIntersection)
        # self.updateDrawIntersection(self.frame)
        
        applogic.resetCamera(viewDirection=[0.2,0,-1])
        self.view.showMaximized()
        self.view.raise_()
        panel = screengrabberpanel.ScreenGrabberPanel(self.view)
        panel.widget.show()

        cameracontrolpanel.CameraControlPanel(self.view).widget.show()

        elapsed = time.time() - self.startSimTime
        simRate = self.counter/elapsed
        print "Total run time", elapsed
        print "Ticks (Hz)", simRate
        print "Number of steps taken", self.counter
        self.onRandomObstaclesButton()
        self.app.start()
        
       


    def getControllerTypeFromCounter(self, counter):
        name = self.controllerTypeOrder[0]

        for controllerType in self.controllerTypeOrder[1:]:
            if counter >= self.idxDict[controllerType]:
                name = controllerType

        return name


    def setRobotFrameState(self, x, y, theta):
        t = vtk.vtkTransform()
        t.Translate(x,y,0.0)
        t.RotateZ(np.degrees(theta))
        self.robot.getChildFrame().copyFrame(t)

    # returns true if we are in collision
    def checkInCollision(self, raycastDistance=None):
        
        # We need to re-evaluate this
        return False

        # if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold:
        #     return True
        # else:
        #     return False

    def tick(self):
        #print timer.elapsed
        #simulate(t.elapsed)
        x = np.sin(time.time())
        y = np.cos(time.time())
        self.setRobotFrameState(x,y,0.0)
        if (time.time() - self.playTime) > self.endTime:
            self.playTimer.stop()

    def tick2(self):
        newtime = time.time() - self.playTime
        print time.time() - self.playTime
        x = np.sin(newtime)
        y = np.cos(newtime)
        self.setRobotFrameState(x,y,0.0)

    # just increment the slider, stop the timer if we get to the end
    def playTimerCallback(self):
        self.sliderMovedByPlayTimer = True
        currentIdx = self.slider.value
        nextIdx = currentIdx + 1
        self.slider.setSliderPosition(nextIdx)
        if currentIdx >= self.sliderMax:
            print "reached end of tape, stopping playTime"
            self.playTimer.stop()

    def onSliderChanged(self, value):
        if not self.sliderMovedByPlayTimer:
            self.playTimer.stop()
        numSteps = len(self.stateOverTime)
        idx = int(np.floor(numSteps*(1.0*value/self.sliderMax)))
        idx = min(idx, numSteps-1)
        x,y, xdot, ydot = self.stateOverTime[idx]
        self.setRobotFrameState(x,y,0)
        self.sliderMovedByPlayTimer = False

    def onXVelocityChanged(self, value):

        self.XVelocity_drawing = value
        self.onDrawActionSetButton()
        print "x velocity changed to ", value

    def onYVelocityChanged(self, value):
        print value

        self.YVelocity_drawing = -value
        self.onDrawActionSetButton()
        print "y velocity changed to ", -value


    def onRandomObstaclesButton(self):
        print "random obstacles button pressed"
        self.setInitialStateAtZero()
        self.world = World.buildLineSegmentTestWorld(percentObsDensity=8.0,
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=False,
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])
        
        self.locator = World.buildCellLocator(self.world.visObj.polyData)

    def onRandomGlobalGoalButton(self):
        print "random global goal button pressed"
        self.globalGoal = World.buildGlobalGoal()
   

    def onDrawActionSetButton(self):
        print "drawing action set"
        #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
        self.ActionSet.computeAllPositions(self.XVelocity_drawing,self.YVelocity_drawing)
        #self.ActionSet.drawActionSetFinal()
        self.ActionSet.drawActionSetFull()

    def onRunSimButton(self):
        self.runBatchSimulation()
        self.saveToFile("latest")
        
    def onPlayButton(self):

        if self.playTimer.isActive():
            self.onPauseButton()
            return

        print 'play'
        self.playTimer.start()
        self.playTime = time.time()

    def onPauseButton(self):
        print 'pause'
        self.playTimer.stop()

    def saveToFile(self, filename):

        # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime

        filename = 'data/' + filename + ".out"
        my_shelf = shelve.open(filename,'n')

        my_shelf['options'] = self.options

        my_shelf['simulationData'] = self.simulationData
        my_shelf['stateOverTime'] = self.stateOverTime
        my_shelf['controlInputData'] = self.controlInputData
        my_shelf['numTimesteps'] = self.numTimesteps
        my_shelf['idxDict'] = self.idxDict
        my_shelf['counter'] = self.counter
        my_shelf.close()

    def run(self, launchApp=True):
        self.counter = 1
        
        # for use in playback
        self.dt = self.options['dt']
        self.endTime = self.defaultControllerTime # used to be the sum of the other times as well

        self.t = np.arange(0.0, self.endTime, self.dt)
        maxNumTimesteps = np.size(self.t)

        self.stateOverTime = np.zeros((maxNumTimesteps+1, 4))
        self.controlInputData = np.zeros((maxNumTimesteps+1,2))
        self.numTimesteps = maxNumTimesteps

        self.runBatchSimulation()


        if launchApp:
            self.setupPlayback() 


    @staticmethod
    def loadFromFile(filename):
        filename = 'data/' + filename + ".out"
        sim = Simulator(autoInitialize=False, verbose=False)

        my_shelf = shelve.open(filename)
        sim.options = my_shelf['options']
        sim.initialize()

        sim.simulationData = my_shelf['simulationData']
        sim.stateOverTime = np.array(my_shelf['stateOverTime'])
        sim.raycastData = np.array( my_shelf['raycastData'])
        sim.controlInputData = np.array(my_shelf['controlInputData'])
        sim.numTimesteps = my_shelf['numTimesteps']
        sim.idxDict = my_shelf['idxDict']
        sim.counter = my_shelf['counter']

        my_shelf.close()

        return sim
class Simulator(object):


    def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False,
                 circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True):
        self.verbose = verbose
        self.startSimTime = time.time()
        self.collisionThreshold = 1.0
        self.randomSeed = 5
        self.Sensor_rayLength = 8

        self.percentObsDensity = percentObsDensity
        self.defaultControllerTime = 1000
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale

        # create the visualizer object
        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()

        self.initial_XVelocity = 0.0
        self.initial_YVelocity = 0.0

        self.running_sim = True

        self.currentIdx = 0;
        
        if autoInitialize:
            self.initialize()

    def initializeOptions(self):
        self.options = dict()

        self.options['World'] = dict()
        self.options['World']['obstaclesInnerFraction'] = 0.98
        self.options['World']['randomSeed'] = 40
        self.options['World']['percentObsDensity'] = 25.0
        self.options['World']['nonRandomWorld'] = True
        self.options['World']['circleRadius'] = 0.6
        self.circleRadius = self.options['World']['circleRadius']
        self.options['World']['scale'] = 2.0

        self.options['Sensor'] = dict()
        self.options['Sensor']['rayLength'] = 10
        self.options['Sensor']['numRays'] = 40


        self.options['Car'] = dict()
        self.options['Car']['velocity'] = 20

        self.options['dt'] = 0.05

        self.options['runTime'] = dict()
        self.options['runTime']['defaultControllerTime'] = 100


    def initializeColorMap(self):
        self.colorMap = dict()
        self.colorMap['default'] = [0,1,0]

    def initialize(self):

        self.dt = self.options['dt']
        self.controllerTypeOrder = ['default']

        self.Controller = ControllerObj()

        self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
                                numRays=self.options['Sensor']['numRays'])

        self.Car = CarPlant(controller=self.Controller,
                            velocity=self.options['Car']['velocity'])

        self.Controller.initializeVelocity(self.Car.v)

        self.ActionSet = ActionSetObj()

        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName('world'))
        self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'],
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=self.options['World']['nonRandomWorld'],
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])

        om.removeFromObjectModel(om.findObjectByName('robot'))
        self.robot, self.frame = World.buildRobot()
        self.locator = World.buildCellLocator(self.world.visObj.polyData)
        
        #will need to set locator for purple trajectories
        #self.Sensor.setLocator(self.locator)
        self.frame = self.robot.getChildFrame()
        self.frame.setProperty('Scale', 3)
        #self.frame.setProperty('Visible', False)
        #self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options['runTime']['defaultControllerTime']

        self.Car.setFrame(self.frame)
        print "Finished initialization"


    def terminalEuclideanCostForTrajectories(self):
        # access the trajectories
        x_traj = self.ActionSet.p_x_trajectories
        y_traj = self.ActionSet.p_y_trajectories


        final_x_positions = x_traj[:,-1]
        final_y_positions = y_traj[:,-1]

        #access the global goal
        global_goal_x = self.globalGoal.global_goal_x
        global_goal_y = self.globalGoal.global_goal_y

        euclideans_vector = []

        # def getKey(item):
        #     return item[1]

        for i in xrange(self.ActionSet.num_x_bins):
            for j in xrange(self.ActionSet.num_y_bins):
                distance = np.sqrt((final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2)
                euclideans_vector.append(distance)


        #sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey)

        return np.array(euclideans_vector)

    def CheckIfCollisionFreeTrajectoryIndex(self, traj_index):
        # accessing the right trajectory
        x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]]
        y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]]


        #check if anything is too close to the center of the circles
        #print self.world.list_of_circles
        for i in range(len(x_trajectory_to_check)):
            point = ( x_trajectory_to_check[i], y_trajectory_to_check[i]  )
            if not self.CheckIfCollisionFreePoint(point):
                #print "trajectory wasn't free"
                return False 


        return True

    def CheckIfCollisionFreePoint(self, point):

        for circle_center in self.world.list_of_circles:
            #print "circle centers first"
            #print circle_center[0], circle_center[1]
            #print self.circleRadius
            #print point[0], point[1]
            #print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2
            if  ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2):
                #print "I found a point in a circle"
                return False

        if point[0] > self.world.Xmax:
            #print "I would have gone past top wall"
            return False
        if point[0] < self.world.Xmin:
            #print "I would have gone below bottom wall"
            return False
        if point[1] > self.world.Ymax:
            #print "I would have gone to the right of the side wall"
            return False    
        if point[1] < self.world.Ymin:
            #print "I would have gone to the left of the side wall"
            return False

        return True

    def computeProbabilitiesOfCollisionAllTrajectories(self, currentRaycastIntersectionLocations, speed_allowed_matrix):
        probability_vector = []
        indices_vector = []
        for x_index in xrange(self.ActionSet.num_x_bins):
            for y_index in xrange(self.ActionSet.num_y_bins):
                if not speed_allowed_matrix[x_index, y_index]:
                    probability_vector.append(1.0)

                else:
                    probability_of_collision = self.computeProbabilityOfCollisionOneTrajectory(x_index, y_index, currentRaycastIntersectionLocations)
                    probability_vector.append(probability_of_collision)

                indices_vector.append([x_index, y_index])

        return np.array(probability_vector), indices_vector
        

    def computeProbabilityOfCollisionOneTrajectory(self, x_index, y_index, currentRaycastIntersectionLocations):
        probability_no_collision = 1
        for time_step_index, time_step_value in enumerate(self.ActionSet.t_vector):
            for obstacle_center in currentRaycastIntersectionLocations:

                probability_of_collision_step_k_obstacle_j = self.computeProbabilityOfCollisionOneStepOneObstacle(x_index, y_index, time_step_index, time_step_value, obstacle_center)

                probability_no_collision_step_k_obstacle_j = 1 - probability_of_collision_step_k_obstacle_j

                probability_no_collision = probability_no_collision * probability_no_collision_step_k_obstacle_j

        return float(1.0 - probability_no_collision)

    def computeProbabilityOfCollisionOneStepOneObstacle(self, x_index, y_index, time_step_index, time_step_value, obstacle_center):
        volume = 4.18
        Sigma_sensor = np.zeros((3,3))
        np.fill_diagonal(Sigma_sensor, self.sigma_sensor)

        variance_x = (2.5 + abs(self.current_initial_velocity_x*0.1))*time_step_value+0.01
        variance_y = (2.5 + abs(self.current_initial_velocity_y*0.1))*time_step_value+0.01
        variance_z = (1.5)*time_step_value+0.01

        Sigma_robot = np.zeros((3,3))
        np.fill_diagonal(Sigma_sensor, [variance_x, variance_y, variance_z])


        Sigma_C = Sigma_robot + Sigma_sensor

        denominator = np.sqrt(np.linalg.det(2*np.pi*Sigma_C))

        x = self.ActionSet.p_x_trajectories[x_index, time_step_index]
        y = self.ActionSet.p_y_trajectories[y_index, time_step_index]
        z = 0

        x_robot = np.array([x, y, z])
        obstacle_center = np.array(obstacle_center)

        exponent = - 0.5 * np.matrix((x_robot - obstacle_center)) * np.matrix(np.linalg.inv(Sigma_C)) * np.matrix((x_robot - obstacle_center)).T

        return volume / denominator * np.exp(exponent)

    def computeJerkVector(self,controlInput):
        jerk_vector = []
        for x_index in xrange(self.ActionSet.num_x_bins):
            for y_index in xrange(self.ActionSet.num_y_bins):
                last_x_accel = controlInput[0]
                last_y_accel = controlInput[1]

                this_x_accel = self.ActionSet.a_x[x_index]
                this_y_accel = self.ActionSet.a_y[y_index]

                jerk = np.sqrt( (last_x_accel - this_x_accel)**2 + (last_y_accel - this_y_accel)**2 )
                jerk_vector.append(jerk)

        return np.array(jerk_vector) 


    def identifySpeedAllowedTrajectories(self):
        speed_allowed_matrix = np.ones((self.ActionSet.num_x_bins, self.ActionSet.num_y_bins))

        # for x_index in xrange(self.ActionSet.num_x_bins):
        #     for y_index in xrange(self.ActionSet.num_y_bins):

        #         this_x_accel = self.ActionSet.a_x[x_index]
        #         this_y_accel = self.ActionSet.a_y[y_index]

        #         if np.sqrt( (self.current_initial_velocity_x - this_x_accel)**2 + (self.current_initial_velocity_y - this_y_accel)**2) < self.speed_max:
        #             speed_allowed_matrix[x_index,y_index] = 1

        return speed_allowed_matrix


    def runSingleSimulation(self, controllerType='default', simulationCutoff=None):


        self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity)
        self.setRobotFrameState(0.0,0.0,0.0)


        currentCarState = np.copy(self.Car.state)
        nextCarState = np.copy(self.Car.state)
        self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])

        self.Sensor.setLocator(self.locator)

        firstRaycast = self.Sensor.raycastAll(self.frame)
        firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)

        nextRaycast = np.zeros(self.Sensor.numRays)

        # record the reward data
        runData = dict()
        startIdx = self.counter

        controlInput = [0,0]


        while (self.counter < self.numTimesteps - 1):
            idx = self.counter
            currentTime = self.t[idx]
            self.stateOverTime[idx,:] = currentCarState
            x = self.stateOverTime[idx,0]
            y = self.stateOverTime[idx,1]
            self.setRobotFrameState(x,y,0.0)
            # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])

            currentRaycastIntersectionLocations = self.Sensor.raycastLocationsOnlyOfIntersections(self.frame)

            if self.checkInCollision(currentCarState[0], currentCarState[1], currentRaycastIntersectionLocations):
                break
            

            if controllerType not in self.colorMap.keys():
                print
                raise ValueError("controller of type " + controllerType + " not supported")

            # compute all trajectories from action set
            self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1], currentCarState[2],currentCarState[3])
            self.current_initial_velocity_x = currentCarState[2]
            self.current_initial_velocity_y = currentCarState[3]

            speed_allowed_matrix = self.identifySpeedAllowedTrajectories()

            probability_vector, indices_list = self.computeProbabilitiesOfCollisionAllTrajectories(currentRaycastIntersectionLocations, speed_allowed_matrix)

            euclideans_vector = self.terminalEuclideanCostForTrajectories()

            jerk_vector = self.computeJerkVector(controlInput)

            # k_collision_cost = 10
            # k_terminal_cost = 1
            # k_jerk = 0.1
            # sum_vector = probability_vector*k_collision_cost + euclideans_vector/np.max(euclideans_vector)*k_terminal_cost + k_jerk*jerk_vector/np.max(jerk_vector)
            # min_action_index_in_vector = np.argmin(sum_vector)
            # x_index_to_use = indices_list[min_action_index_in_vector][0]
            # y_index_to_use = indices_list[min_action_index_in_vector][1]

            

            # convert to probability no collision
            probability_vector = np.ones(len(probability_vector)) - probability_vector
            
            # convert distances to amount of progress
            current_distance = np.sqrt((currentCarState[0] - self.globalGoal.global_goal_x)**2 + (currentCarState[1] - self.globalGoal.global_goal_y)**2)
            #print "euclideans vector", euclideans_vector

            euclidean_progress_vector = np.ones(len(euclideans_vector))*current_distance - euclideans_vector
            #print "euclidean progress vector", euclidean_progress_vector
            reward_vector = euclidean_progress_vector - 0.1*jerk_vector/np.max(jerk_vector)
            #print "reward_vector", reward_vector
            
            expected_reward = np.multiply(probability_vector, reward_vector)
            max_action_index_in_vector = np.argmax(expected_reward)
            x_index_to_use = indices_list[max_action_index_in_vector][0]
            y_index_to_use = indices_list[max_action_index_in_vector][1]

        
            self.actionIndicesOverTime[idx,:] = [x_index_to_use, y_index_to_use] 

            x_accel = self.ActionSet.a_x[x_index_to_use]
            y_accel = self.ActionSet.a_y[y_index_to_use]

            controlInput = [x_accel, y_accel]

            self.controlInputData[idx] = controlInput

            nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)

        
            x = nextCarState[0]
            y = nextCarState[1]
            theta = nextCarState[2]
            self.setRobotFrameState(x,y,theta)
            

            #bookkeeping
            currentCarState = nextCarState
            
            self.counter+=1

            # break if we are in collision

            if self.counter >= simulationCutoff:
                break


        # fill in the last state by hand
        self.stateOverTime[self.counter,:] = currentCarState
        self.actionIndicesOverTime[self.counter,:] = [x_index_to_use, y_index_to_use]


        # this just makes sure we don't get stuck in an infinite loop.
        if startIdx == self.counter:
            self.counter += 1

        return runData

    def setNumpyRandomSeed(self, seed=1):
        np.random.seed(seed)

    def runBatchSimulation(self, endTime=None, dt=0.05):

        # for use in playback
        self.dt = self.options['dt']

        self.endTime = self.defaultControllerTime # used to be the sum of the other times as well

        self.t = np.arange(0.0, self.endTime, dt)
        maxNumTimesteps = np.size(self.t)
        self.stateOverTime = np.zeros((maxNumTimesteps+1, 4))
        self.actionIndicesOverTime = np.zeros((maxNumTimesteps+1, 2))
        self.controlInputData = np.zeros((maxNumTimesteps+1,2))
        self.numTimesteps = maxNumTimesteps

        self.controllerTypeOrder = ['default']
        self.counter = 0
        self.simulationData = []
    
        self.initializeStatusBar()

        self.idxDict = dict()
        numRunsCounter = 0


        self.idxDict['default'] = self.counter
        loopStartIdx = self.counter
        simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps)
        
        while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1):
            self.printStatusBar()
            startIdx = self.counter
            runData = self.runSingleSimulation(controllerType='default',
                                               simulationCutoff=simCutoff)
            runData['startIdx'] = startIdx
            runData['controllerType'] = "default"
            runData['duration'] = self.counter - runData['startIdx']
            runData['endIdx'] = self.counter
            runData['runNumber'] = numRunsCounter
            numRunsCounter+=1
            self.simulationData.append(runData)

        # BOOKKEEPING
        # truncate stateOverTime, raycastData, controlInputs to be the correct size
        self.numTimesteps = self.counter + 1
        self.stateOverTime = self.stateOverTime[0:self.counter+1, :]
        self.actionIndicesOverTime = self.actionIndicesOverTime[0:self.counter+1, :]
        self.controlInputData = self.controlInputData[0:self.counter+1]
        self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime

    def initializeStatusBar(self):
        self.numTicks = 10
        self.nextTickComplete = 1.0 / float(self.numTicks)
        self.nextTickIdx = 1
        print "Simulation percentage complete: (", self.numTicks, " # is complete)"
    
    def printStatusBar(self):
        fractionDone = float(self.counter) / float(self.numTimesteps)
        if fractionDone > self.nextTickComplete:

            self.nextTickIdx += 1
            self.nextTickComplete += 1.0 / self.numTicks

            timeSoFar = time.time() - self.startSimTime 
            estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
            estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0

            print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left"


    def setCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = 0.0
            y =   -5.0
            theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01
            
            self.Car.setCarState(x,y,theta)
            self.setRobotFrameState(x,y,theta)

            print "In loop"

            if not self.checkInCollision():
                break
                
        return x,y,theta


    def setRandomCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0]
            y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0]
            #theta = np.random.uniform(0,2*np.pi,1)[0]
            theta = 0 #always forward

            self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity)
            self.setRobotFrameState(x,y,theta)

            if not self.checkInCollision():
                break

        self.firstRandomCollisionFreeInitialState_x = x
        self.firstRandomCollisionFreeInitialState_y = y

        return x,y,0,0

    def setupPlayback(self):

        self.timer = TimerCallback(targetFps=30)
        self.timer.callback = self.tick

        playButtonFps = 1.0/self.dt
        print "playButtonFPS", playButtonFps
        self.playTimer = TimerCallback(targetFps=playButtonFps)
        self.playTimer.callback = self.playTimerCallback
        self.sliderMovedByPlayTimer = False

        panel = QtGui.QWidget()
        l = QtGui.QHBoxLayout(panel)

        self.max_velocity = 20.0

        sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
        sliderXVelocity.setMaximum(self.max_velocity)
        sliderXVelocity.setMinimum(-self.max_velocity)

        l.addWidget(sliderXVelocity)

        sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
        sliderYVelocity.setMaximum(self.max_velocity)
        sliderYVelocity.setMinimum(-self.max_velocity)
        l.addWidget(sliderYVelocity)

        randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
        randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
        l.addWidget(randomGlobalGoalButton)

        drawActionSetButton = QtGui.QPushButton('Draw Action Set')
        drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
        l.addWidget(drawActionSetButton)

        runSimButton = QtGui.QPushButton('Simulate')
        runSimButton.connect('clicked()', self.onRunSimButton)
        l.addWidget(runSimButton)

        playButton = QtGui.QPushButton('Play/Pause')
        playButton.connect('clicked()', self.onPlayButton)

        slider = QtGui.QSlider(QtCore.Qt.Horizontal)
        slider.connect('valueChanged(int)', self.onSliderChanged)
        self.sliderMax = self.numTimesteps
        slider.setMaximum(self.sliderMax)
        self.slider = slider

        l.addWidget(playButton)
        l.addWidget(slider)

        toggleSensorNoiseButton = QtGui.QPushButton('Toggle Sensor Noise')
        toggleSensorNoiseButton.connect('clicked()', self.onToggleSensorNoise)
        l.addWidget(toggleSensorNoiseButton)

        showObstaclesButton = QtGui.QPushButton('Show Obsatacles')
        showObstaclesButton.connect('clicked()', self.onShowObstacles)
        l.addWidget(showObstaclesButton)

        w = QtGui.QWidget()
        l = QtGui.QVBoxLayout(w)
        l.addWidget(self.view)
        self.view.orientationMarkerWidget().Off()
        l.addWidget(panel)
        w.showMaximized()

        # need to connect frames with DrawActionSet
        self.frame.connectFrameModified(self.updateDrawActionSet)
        self.updateDrawActionSet(self.frame)
        self.frame.connectFrameModified(self.updateDrawIntersection)
        self.updateDrawIntersection(self.frame)
        
        applogic.resetCamera(viewDirection=[0.2,0,-1])
        self.view.showMaximized()
        self.view.raise_()
        panel = screengrabberpanel.ScreenGrabberPanel(self.view)
        panel.widget.show()

        cameracontrolpanel.CameraControlPanel(self.view).widget.show()

        elapsed = time.time() - self.startSimTime
        simRate = self.counter/elapsed
        print "Total run time", elapsed
        print "Ticks (Hz)", simRate
        print "Number of steps taken", self.counter
        self.app.start()

    def run(self, launchApp=True):
        self.sphere_toggle = False
        self.sigma_sensor = 0.5
        self.speed_max = 10.0


        self.running_sim = True
        self.onRandomGlobalGoalButton()
        self.counter = 1
        self.runBatchSimulation()
        self.running_sim = False

        if launchApp:
            self.setupPlayback()

    def onToggleSensorNoise(self):
        self.sphere_toggle = not self.sphere_toggle

    def onShowObstacles(self):
        print "time to show obstacles"
        A = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'],
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=self.options['World']['nonRandomWorld'],
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'], alpha=1.0)


    def drawFirstIntersections(self, frame, firstRaycast):
        origin = np.array(frame.transform.GetPosition())
        d = DebugData()

        firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)

        for i in xrange(self.Sensor.numRays):
            endpoint = firstRaycastLocations[i,:]

            if firstRaycast[i] >= self.Sensor.rayLength - 0.1:
                d.addLine(origin, endpoint, color=[0,1,0])
            else:
                d.addLine(origin, endpoint, color=[1,0,0])

        vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')

        self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
        self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
        self.locator = self.LineSegmentLocator
        self.Sensor.setLocator(self.LineSegmentLocator)


    def updateDrawIntersection(self, frame, locator="None"):
        if locator=="None":
            locator = self.locator

        if self.sphere_toggle:
            sphere_radius=self.sigma_sensor
        else:
            sphere_radius=0.0


        origin = np.array(frame.transform.GetPosition())
        #print "origin is now at", origin
        d = DebugData()
        d_sphere=DebugData()

        sliderIdx = self.slider.value

        controllerType = self.getControllerTypeFromCounter(sliderIdx)
        colorMaxRange = self.colorMap[controllerType]

        for i in xrange(self.Sensor.numRays):
            ray = self.Sensor.rays[:,i]
            rayTransformed = np.array(frame.transform.TransformNormal(ray))
            #print "rayTransformed is", rayTransformed
            intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed*self.Sensor.rayLength)

            if intersection is not None:
                d.addLine(origin, intersection, color=[1,0,0])
                d_sphere.addSphere(intersection, radius=sphere_radius, color=[1,0,0])

            else:
                d.addLine(origin, origin+rayTransformed*self.Sensor.rayLength, color=colorMaxRange)
                d_sphere.addSphere(origin, radius=0.0, color=[0,1,0])

        vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
        vis.updatePolyData(d_sphere.getPolyData(), 'spheres', colorByName='RGB255', alpha=0.3)



    def getControllerTypeFromCounter(self, counter):
        name = self.controllerTypeOrder[0]

        for controllerType in self.controllerTypeOrder[1:]:
            if counter >= self.idxDict[controllerType]:
                name = controllerType

        return name


    def setRobotFrameState(self, x, y, theta):
        t = vtk.vtkTransform()
        t.Translate(x,y,0.0)
        t.RotateZ(np.degrees(theta))
        self.robot.getChildFrame().copyFrame(t)

    # returns true if we are in collision
    def checkInCollision(self, x, y, raycastLocations):
        return False

        # carState = [x,y,0]
        # for raycastLocation in raycastLocations:
        #     if np.linalg.norm(carState - raycastLocation) < 0.3:
        #         return True

        # return False

    def updateDrawActionSet(self, frame):

        origin = np.array(frame.transform.GetPosition())

        if not self.running_sim:
            self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3])
            #self.ActionSet.drawActionSetFinal()
            self.ActionSet.drawActionSetFull()
            self.drawChosenFunnel()

    def drawChosenFunnel(self):
        velocity_initial_x = self.stateOverTime[self.currentIdx,2]
        velocity_initial_y = self.stateOverTime[self.currentIdx,3]

        variance_x = 2.5 + abs(velocity_initial_x*0.1)
        variance_y = 2.5 + abs(velocity_initial_y*0.1)
        variance_z = 1.5

        x_index = self.actionIndicesOverTime[self.currentIdx,0]
        y_index = self.actionIndicesOverTime[self.currentIdx,1]

        for i in xrange(0,10):
            time = 0.5/10.0*i
            x_center = self.ActionSet.p_x_trajectories[x_index, i]
            y_center = self.ActionSet.p_y_trajectories[y_index, i]
            z_center = 0.0
            World.buildEllipse(i, [x_center,y_center,z_center], variance_x*time, variance_y*time, variance_z*time, alpha=0.3)


    def tick(self):
        #print timer.elapsed
        #simulate(t.elapsed)
        x = np.sin(time.time())
        y = np.cos(time.time())
        self.setRobotFrameState(x,y,0.0)
        if (time.time() - self.playTime) > self.endTime:
            self.playTimer.stop()

    def tick2(self):
        newtime = time.time() - self.playTime
        print time.time() - self.playTime
        x = np.sin(newtime)
        y = np.cos(newtime)
        self.setRobotFrameState(x,y,0.0)

    # just increment the slider, stop the timer if we get to the end
    def playTimerCallback(self):
        self.sliderMovedByPlayTimer = True
        currentIdx = self.slider.value
        nextIdx = currentIdx + 1
        self.slider.setSliderPosition(nextIdx)
        if currentIdx >= self.sliderMax:
            print "reached end of tape, stopping playTime"
            self.playTimer.stop()

    def onXVelocityChanged(self, value):
        self.initial_XVelocity = value
        print "initial x velocity changed to ", value

    def onYVelocityChanged(self, value):
        self.initial_YVelocity = -value
        print "initial y velocity changed to ", -value

    def onSliderChanged(self, value):
        if not self.sliderMovedByPlayTimer:
            self.playTimer.stop()
        numSteps = len(self.stateOverTime)
        idx = int(np.floor(numSteps*(1.0*value/self.sliderMax)))
        idx = min(idx, numSteps-1)
        self.currentIdx = idx
        x,y, xdot, ydot = self.stateOverTime[idx]
        self.setRobotFrameState(x,y,0)
        self.sliderMovedByPlayTimer = False

    def onRandomGlobalGoalButton(self):
        print "random global goal button pressed"
        self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale'])

    def onDrawActionSetButton(self):
        print "drawing action set"
        #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
        self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity)
        #self.ActionSet.drawActionSetFinal()
        self.ActionSet.drawActionSetFull()

    def onRunSimButton(self):
        self.running_sim = True
        self.runBatchSimulation()
        self.running_sim = False
        #self.saveToFile("latest")

    def onPlayButton(self):

        if self.playTimer.isActive():
            self.onPauseButton()
            return

        print 'play'
        self.playTimer.start()
        self.playTime = time.time()

    def onPauseButton(self):
        print 'pause'
        self.playTimer.stop()

    def saveToFile(self, filename):

        # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime

        filename = 'data/' + filename + ".out"
        my_shelf = shelve.open(filename,'n')

        my_shelf['options'] = self.options

        my_shelf['simulationData'] = self.simulationData
        my_shelf['stateOverTime'] = self.stateOverTime
        my_shelf['controlInputData'] = self.controlInputData
        my_shelf['numTimesteps'] = self.numTimesteps
        my_shelf['idxDict'] = self.idxDict
        my_shelf['counter'] = self.counter
        my_shelf.close()


    @staticmethod
    def loadFromFile(filename):
        filename = 'data/' + filename + ".out"
        sim = Simulator(autoInitialize=False, verbose=False)

        my_shelf = shelve.open(filename)
        sim.options = my_shelf['options']
        sim.initialize()

        sim.simulationData = my_shelf['simulationData']
        sim.stateOverTime = np.array(my_shelf['stateOverTime'])
        sim.controlInputData = np.array(my_shelf['controlInputData'])
        sim.numTimesteps = my_shelf['numTimesteps']
        sim.idxDict = my_shelf['idxDict']
        sim.counter = my_shelf['counter']

        my_shelf.close()

        return sim
class Simulator(object):


    def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False,
                 circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True):
        self.verbose = verbose
        self.startSimTime = time.time()
        self.collisionThreshold = 1.0
        self.randomSeed = 5

        self.percentObsDensity = percentObsDensity
        self.defaultControllerTime = 1000
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale

        # create the visualizer object
        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()

        self.initial_XVelocity = 0.0
        self.initial_YVelocity = 0.0

        self.running_sim = True

        self.currentIdx = 0;
        
        if autoInitialize:
            self.initialize()

    def initializeOptions(self):
        self.options = dict()

        self.options['World'] = dict()
        self.options['World']['obstaclesInnerFraction'] = 0.98
        self.options['World']['randomSeed'] = 40
        self.options['World']['percentObsDensity'] = 15.0
        self.options['World']['nonRandomWorld'] = True
        self.options['World']['circleRadius'] = 0.6
        self.circleRadius = self.options['World']['circleRadius']
        self.options['World']['scale'] = 2.0


        self.options['Car'] = dict()
        self.options['Car']['velocity'] = 20

        self.options['dt'] = 0.05

        self.options['runTime'] = dict()
        self.options['runTime']['defaultControllerTime'] = 100


    def setDefaultOptions(self):

        defaultOptions = dict()


        defaultOptions['World'] = dict()
        defaultOptions['World']['obstaclesInnerFraction'] = 0.98
        defaultOptions['World']['randomSeed'] = 40
        defaultOptions['World']['percentObsDensity'] = 30
        defaultOptions['World']['nonRandomWorld'] = True
        defaultOptions['World']['circleRadius'] = 1.75
        defaultOptions['World']['scale'] = 2.5


        defaultOptions['Car'] = dict()
        defaultOptions['Car']['velocity'] = 20

        defaultOptions['dt'] = 0.05


        defaultOptions['runTime'] = dict()
        defaultOptions['runTime']['defaultControllerTime'] = 100


        for k in defaultOptions:
            self.options.setdefault(k, defaultOptions[k])


        for k in defaultOptions:
            if not isinstance(defaultOptions[k], dict):
                continue

            for j in defaultOptions[k]:
                self.options[k].setdefault(j, defaultOptions[k][j])


    def initializeColorMap(self):
        self.colorMap = dict()
        self.colorMap['default'] = [0,1,0]

    def initialize(self):

        self.dt = self.options['dt']
        self.controllerTypeOrder = ['default']

        self.setDefaultOptions()

        self.Controller = ControllerObj()

        self.Car = CarPlant(controller=self.Controller,
                            velocity=self.options['Car']['velocity'])

        self.Controller.initializeVelocity(self.Car.v)

        self.ActionSet = ActionSetObj()

        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName('world'))
        self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'],
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=self.options['World']['nonRandomWorld'],
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])

        om.removeFromObjectModel(om.findObjectByName('robot'))
        self.robot, self.frame = World.buildRobot()
        self.locator = World.buildCellLocator(self.world.visObj.polyData)
        
        #will need to set locator for purple trajectories
        #self.Sensor.setLocator(self.locator)
        self.frame = self.robot.getChildFrame()
        self.frame.setProperty('Scale', 3)
        #self.frame.setProperty('Visible', False)
        #self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options['runTime']['defaultControllerTime']

        self.Car.setFrame(self.frame)
        print "Finished initialization"


    def rankTrajectories(self):
        # access the trajectories
        x_traj = self.ActionSet.p_x_trajectories
        y_traj = self.ActionSet.p_y_trajectories


        final_x_positions = x_traj[:,-1]
        final_y_positions = y_traj[:,-1]

        #access the global goal
        global_goal_x = self.globalGoal.global_goal_x
        global_goal_y = self.globalGoal.global_goal_y

        ranks_with_indices = []

        def getKey(item):
            return item[1]

        final_distances_squared = np.zeros((len(final_x_positions),len(final_y_positions)))
        for i in xrange(len(final_x_positions)):
            for j in xrange(len(final_y_positions)):
                distance = (final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2
                ranks_with_indices.append(((i,j), distance))


        sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey)

        return sorted_ranks_with_indices

    def CheckIfCollisionFreeTrajectoryIndex(self, traj_index):
        # accessing the right trajectory
        x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]]
        y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]]


        #check if anything is too close to the center of the circles
        #print self.world.list_of_circles
        for i in range(len(x_trajectory_to_check)):
            point = ( x_trajectory_to_check[i], y_trajectory_to_check[i]  )
            if not self.CheckIfCollisionFreePoint(point):
                #print "trajectory wasn't free"
                return False 


        return True

    def CheckIfCollisionFreePoint(self, point):

        for circle_center in self.world.list_of_circles:
            #print "circle centers first"
            #print circle_center[0], circle_center[1]
            #print self.circleRadius
            #print point[0], point[1]
            #print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2
            if  ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2):
                #print "I found a point in a circle"
                return False

        if point[0] > self.world.Xmax:
            #print "I would have gone past top wall"
            return False
        if point[0] < self.world.Xmin:
            #print "I would have gone below bottom wall"
            return False
        if point[1] > self.world.Ymax:
            #print "I would have gone to the right of the side wall"
            return False    
        if point[1] < self.world.Ymin:
            #print "I would have gone to the left of the side wall"
            return False

        return True




    def runSingleSimulation(self, controllerType='default', simulationCutoff=None):


        self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity)
        self.setRobotFrameState(0.0,0.0,0.0)


        currentCarState = np.copy(self.Car.state)
        nextCarState = np.copy(self.Car.state)
        self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])

        # record the reward data
        runData = dict()
        startIdx = self.counter


        while (self.counter < self.numTimesteps - 1):
            idx = self.counter
            currentTime = self.t[idx]
            self.stateOverTime[idx,:] = currentCarState
            x = self.stateOverTime[idx,0]
            y = self.stateOverTime[idx,1]
            self.setRobotFrameState(x,y,0.0)
            # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
            


            if controllerType not in self.colorMap.keys():
                print
                raise ValueError("controller of type " + controllerType + " not supported")

            # compute all trajectories from action set
            self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1],currentCarState[2],currentCarState[3])


            sorted_ranks_with_indices = self.rankTrajectories()
            #method
            #inputs: trajectories, global goal
            #outputs: sorted list of indexes by desirability global goal


            #method
            #input: trajectory
            #output: is collision free or not
            for traj in sorted_ranks_with_indices:
                if self.CheckIfCollisionFreeTrajectoryIndex(traj[0]):
                    traj_to_use_index = traj[0]
                    break
                traj_to_use_index = traj[0]

            x_index_to_use = traj_to_use_index[0]
            y_index_to_use = traj_to_use_index[1]

            x_accel = self.ActionSet.a_x[x_index_to_use]
            y_accel = self.ActionSet.a_y[y_index_to_use]

            controlInput = [x_accel, y_accel]


            self.controlInputData[idx] = controlInput

            nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)

        
            x = nextCarState[0]
            y = nextCarState[1]
            theta = nextCarState[2]
            self.setRobotFrameState(x,y,theta)
            

            #bookkeeping
            currentCarState = nextCarState
            
            self.counter+=1

            # break if we are in collision

            if self.counter >= simulationCutoff:
                break


        # fill in the last state by hand
        self.stateOverTime[self.counter,:] = currentCarState


        # this just makes sure we don't get stuck in an infinite loop.
        if startIdx == self.counter:
            self.counter += 1

        return runData

    def setNumpyRandomSeed(self, seed=1):
        np.random.seed(seed)

    def runBatchSimulation(self, endTime=None, dt=0.05):

        # for use in playback
        self.dt = self.options['dt']

        self.endTime = self.defaultControllerTime # used to be the sum of the other times as well

        self.t = np.arange(0.0, self.endTime, dt)
        maxNumTimesteps = np.size(self.t)
        self.stateOverTime = np.zeros((maxNumTimesteps+1, 4))
        self.controlInputData = np.zeros((maxNumTimesteps+1,2))
        self.numTimesteps = maxNumTimesteps

        self.controllerTypeOrder = ['default']
        self.counter = 0
        self.simulationData = []
    
        self.initializeStatusBar()

        self.idxDict = dict()
        numRunsCounter = 0


        self.idxDict['default'] = self.counter
        loopStartIdx = self.counter
        simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps)
        
        while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1):
            self.printStatusBar()
            startIdx = self.counter
            runData = self.runSingleSimulation(controllerType='default',
                                               simulationCutoff=simCutoff)
            runData['startIdx'] = startIdx
            runData['controllerType'] = "default"
            runData['duration'] = self.counter - runData['startIdx']
            runData['endIdx'] = self.counter
            runData['runNumber'] = numRunsCounter
            numRunsCounter+=1
            self.simulationData.append(runData)

        # BOOKKEEPING
        # truncate stateOverTime, raycastData, controlInputs to be the correct size
        self.numTimesteps = self.counter + 1
        self.stateOverTime = self.stateOverTime[0:self.counter+1, :]
        self.controlInputData = self.controlInputData[0:self.counter+1]
        self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime



    def initializeStatusBar(self):
        self.numTicks = 10
        self.nextTickComplete = 1.0 / float(self.numTicks)
        self.nextTickIdx = 1
        print "Simulation percentage complete: (", self.numTicks, " # is complete)"
    
    def printStatusBar(self):
        fractionDone = float(self.counter) / float(self.numTimesteps)
        if fractionDone > self.nextTickComplete:

            self.nextTickIdx += 1
            self.nextTickComplete += 1.0 / self.numTicks

            timeSoFar = time.time() - self.startSimTime 
            estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
            estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0

            print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left"


    def setCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = 0.0
            y =   -5.0
            theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01
            
            self.Car.setCarState(x,y,theta)
            self.setRobotFrameState(x,y,theta)

            print "In loop"

            if not self.checkInCollision():
                break
                
        return x,y,theta


    def setRandomCollisionFreeInitialState(self):
        tol = 5

        while True:
            
            x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0]
            y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0]
            #theta = np.random.uniform(0,2*np.pi,1)[0]
            theta = 0 #always forward

            self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity)
            self.setRobotFrameState(x,y,theta)

            if not self.checkInCollision():
                break

        self.firstRandomCollisionFreeInitialState_x = x
        self.firstRandomCollisionFreeInitialState_y = y

        return x,y,0,0

    def setupPlayback(self):

        self.timer = TimerCallback(targetFps=30)
        self.timer.callback = self.tick

        playButtonFps = 1.0/self.dt
        print "playButtonFPS", playButtonFps
        self.playTimer = TimerCallback(targetFps=playButtonFps)
        self.playTimer.callback = self.playTimerCallback
        self.sliderMovedByPlayTimer = False

        panel = QtGui.QWidget()
        l = QtGui.QHBoxLayout(panel)

        self.max_velocity = 20.0

        sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
        sliderXVelocity.setMaximum(self.max_velocity)
        sliderXVelocity.setMinimum(-self.max_velocity)

        l.addWidget(sliderXVelocity)

        sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
        sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
        sliderYVelocity.setMaximum(self.max_velocity)
        sliderYVelocity.setMinimum(-self.max_velocity)
        l.addWidget(sliderYVelocity)

        randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
        randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
        l.addWidget(randomGlobalGoalButton)

        drawActionSetButton = QtGui.QPushButton('Draw Action Set')
        drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
        l.addWidget(drawActionSetButton)

        runSimButton = QtGui.QPushButton('Simulate')
        runSimButton.connect('clicked()', self.onRunSimButton)
        l.addWidget(runSimButton)

        playButton = QtGui.QPushButton('Play/Pause')
        playButton.connect('clicked()', self.onPlayButton)

        slider = QtGui.QSlider(QtCore.Qt.Horizontal)
        slider.connect('valueChanged(int)', self.onSliderChanged)
        self.sliderMax = self.numTimesteps
        slider.setMaximum(self.sliderMax)
        self.slider = slider

        l.addWidget(playButton)
        l.addWidget(slider)

        w = QtGui.QWidget()
        l = QtGui.QVBoxLayout(w)
        l.addWidget(self.view)
        self.view.orientationMarkerWidget().Off()
        l.addWidget(panel)
        w.showMaximized()

        # need to connect frames with DrawActionSet
        self.frame.connectFrameModified(self.updateDrawActionSet)
        self.updateDrawActionSet(self.frame)
        # self.frame.connectFrameModified(self.updateDrawIntersection)
        # self.updateDrawIntersection(self.frame)
        
        applogic.resetCamera(viewDirection=[0.2,0,-1])
        self.view.showMaximized()
        self.view.raise_()
        panel = screengrabberpanel.ScreenGrabberPanel(self.view)
        panel.widget.show()

        cameracontrolpanel.CameraControlPanel(self.view).widget.show()

        elapsed = time.time() - self.startSimTime
        simRate = self.counter/elapsed
        print "Total run time", elapsed
        print "Ticks (Hz)", simRate
        print "Number of steps taken", self.counter
        self.app.start()

    def run(self, launchApp=True):
        self.running_sim = True
        self.onRandomGlobalGoalButton()
        self.counter = 1
        self.runBatchSimulation()
        self.running_sim = False
        print "my state over time is", self.stateOverTime

        if launchApp:
            self.setupPlayback()



    def getControllerTypeFromCounter(self, counter):
        name = self.controllerTypeOrder[0]

        for controllerType in self.controllerTypeOrder[1:]:
            if counter >= self.idxDict[controllerType]:
                name = controllerType

        return name


    def setRobotFrameState(self, x, y, theta):
        t = vtk.vtkTransform()
        t.Translate(x,y,0.0)
        t.RotateZ(np.degrees(theta))
        self.robot.getChildFrame().copyFrame(t)

    # returns true if we are in collision
    def checkInCollision(self):

        return False

    def updateDrawActionSet(self, frame):

        origin = np.array(frame.transform.GetPosition())

        if not self.running_sim:
            self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3])
            #self.ActionSet.drawActionSetFinal()
            self.ActionSet.drawActionSetFull()


    def tick(self):
        #print timer.elapsed
        #simulate(t.elapsed)
        x = np.sin(time.time())
        y = np.cos(time.time())
        self.setRobotFrameState(x,y,0.0)
        if (time.time() - self.playTime) > self.endTime:
            self.playTimer.stop()

    def tick2(self):
        newtime = time.time() - self.playTime
        print time.time() - self.playTime
        x = np.sin(newtime)
        y = np.cos(newtime)
        self.setRobotFrameState(x,y,0.0)

    # just increment the slider, stop the timer if we get to the end
    def playTimerCallback(self):
        self.sliderMovedByPlayTimer = True
        currentIdx = self.slider.value
        nextIdx = currentIdx + 1
        self.slider.setSliderPosition(nextIdx)
        if currentIdx >= self.sliderMax:
            print "reached end of tape, stopping playTime"
            self.playTimer.stop()

    def onXVelocityChanged(self, value):
        self.initial_XVelocity = value
        print "initial x velocity changed to ", value

    def onYVelocityChanged(self, value):
        self.initial_YVelocity = -value
        print "initial y velocity changed to ", -value

    def onSliderChanged(self, value):
        if not self.sliderMovedByPlayTimer:
            self.playTimer.stop()
        numSteps = len(self.stateOverTime)
        idx = int(np.floor(numSteps*(1.0*value/self.sliderMax)))
        idx = min(idx, numSteps-1)
        self.currentIdx = idx
        x,y, xdot, ydot = self.stateOverTime[idx]
        self.setRobotFrameState(x,y,0)
        self.sliderMovedByPlayTimer = False

    def onRandomGlobalGoalButton(self):
        print "random global goal button pressed"
        self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale'])

    def onDrawActionSetButton(self):
        print "drawing action set"
        #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
        self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity)
        #self.ActionSet.drawActionSetFinal()
        self.ActionSet.drawActionSetFull()

    def onRunSimButton(self):
        self.running_sim = True
        self.runBatchSimulation()
        print "my state over time is", self.stateOverTime
        self.running_sim = False
        #self.saveToFile("latest")

    def onPlayButton(self):

        if self.playTimer.isActive():
            self.onPauseButton()
            return

        print 'play'
        self.playTimer.start()
        self.playTime = time.time()

    def onPauseButton(self):
        print 'pause'
        self.playTimer.stop()

    def saveToFile(self, filename):

        # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime

        filename = 'data/' + filename + ".out"
        my_shelf = shelve.open(filename,'n')

        my_shelf['options'] = self.options

        my_shelf['simulationData'] = self.simulationData
        my_shelf['stateOverTime'] = self.stateOverTime
        my_shelf['controlInputData'] = self.controlInputData
        my_shelf['numTimesteps'] = self.numTimesteps
        my_shelf['idxDict'] = self.idxDict
        my_shelf['counter'] = self.counter
        my_shelf.close()


    @staticmethod
    def loadFromFile(filename):
        filename = 'data/' + filename + ".out"
        sim = Simulator(autoInitialize=False, verbose=False)

        my_shelf = shelve.open(filename)
        sim.options = my_shelf['options']
        sim.initialize()

        sim.simulationData = my_shelf['simulationData']
        sim.stateOverTime = np.array(my_shelf['stateOverTime'])
        sim.controlInputData = np.array(my_shelf['controlInputData'])
        sim.numTimesteps = my_shelf['numTimesteps']
        sim.idxDict = my_shelf['idxDict']
        sim.counter = my_shelf['counter']

        my_shelf.close()

        return sim