import os
import sys
import vtk
import math
import PythonQt
from PythonQt import QtCore, QtGui
import director.objectmodel as om
from director import drcargs
from director import robotstate
from director.timercallback import TimerCallback
from director.utime import getUtime
from director.simpletimer import MovingAverageComputer
import vtkDRCFiltersPython as drc
from director.debugVis import DebugData
import director.visualization as vis
from director import vtkNumpy as vnp
import numpy as np

import drc as lcmdrc
import bot_core as lcmbotcore
import multisense as lcmmultisense
import maps as lcmmaps
import lcmUtils


class MultisenseItem(om.ObjectModelItem):

    def __init__(self, model):

        om.ObjectModelItem.__init__(self, 'Multisense', om.Icons.Laser)

        self.model = model
        self.scalarBarWidget = None
        self.addProperty('Color By', 0,
                         attributes=om.PropertyAttributes(enumNames=['Solid Color', 'Intensity', 'Z Coordinate', 'Range', 'Spindle Angle', 'Azimuth', 'Camera RGB', 'Scan Delta']))
        self.addProperty('Show Scalar Bar', False)
        self.addProperty('Updates Enabled', True)
        self.addProperty('Min Range', model.reader.GetDistanceRange()[0],
                         attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
        self.addProperty('Max Range', model.reader.GetDistanceRange()[1],
                         attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
        self.addProperty('Edge Filter Angle', model.reader.GetEdgeAngleThreshold(),
                         attributes=om.PropertyAttributes(decimals=0, minimum=0.0, maximum=60.0, singleStep=1, hidden=False))
        self.addProperty('Number of Scan Lines', model.numberOfScanLines,
                         attributes=om.PropertyAttributes(decimals=0, minimum=0, maximum=100, singleStep=1, hidden=False))
        self.addProperty('Visible', model.visible)
        self.addProperty('Point Size', model.pointSize,
                         attributes=om.PropertyAttributes(decimals=0, minimum=1, maximum=20, singleStep=1, hidden=False))
        self.addProperty('Alpha', model.alpha,
                         attributes=om.PropertyAttributes(decimals=2, minimum=0, maximum=1.0, singleStep=0.1, hidden=False))
        self.addProperty('Min Height', model.reader.GetHeightRange()[0],
                         attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
        self.addProperty('Max Height', model.reader.GetHeightRange()[1],
                         attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))

        #self.addProperty('Color', QtGui.QColor(255,255,255))
        #self.addProperty('Scanline Color', QtGui.QColor(255,0,0))

    def _onPropertyChanged(self, propertySet, propertyName):
        om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Updates Enabled':
            if self.getProperty('Updates Enabled'):
                self.model.start()
            else:
                self.model.stop()

        elif propertyName == 'Edge Filter Angle':
            self.model.reader.SetEdgeAngleThreshold(self.getProperty('Edge Filter Angle'))
            self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName == 'Alpha':
            self.model.setAlpha(self.getProperty(propertyName))

        elif propertyName == 'Visible':
            self.model.setVisible(self.getProperty(propertyName))

        elif propertyName == 'Point Size':
            self.model.setPointSize(self.getProperty(propertyName))

        elif propertyName == 'Number of Scan Lines':
            self.model.numberOfScanLines = self.getProperty(propertyName)
            self.model.initScanLines()

        elif propertyName in ('Min Range', 'Max Range'):
            self.model.reader.SetDistanceRange(self.getProperty('Min Range'), self.getProperty('Max Range'))
            self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName in ('Min Height', 'Max Height'):
            self.model.reader.SetHeightRange(self.getProperty('Min Height'), self.getProperty('Max Height'))
            self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName == 'Color By':
            self._updateColorBy()

        elif propertyName == 'Show Scalar Bar':
            self._updateScalarBar()

        self.model.polyDataObj._renderAllViews()


    def _updateColorBy(self):

        arrayMap = {
          0 : 'Solid Color',
          1 : 'intensity',
          2 : 'z',
          3 : 'distance',
          4 : 'spindle_angle',
          5 : 'azimuth',
          6 : 'rgb',
          7 : 'scan_delta'
          }

        colorBy = self.getProperty('Color By')
        arrayName = arrayMap.get(colorBy)

        if arrayName == 'rgb' and arrayName not in self.model.polyDataObj.getArrayNames():
            self.model.colorizeCallback()
            self.model.polyDataObj._updateColorByProperty()
        self.model.polyDataObj.setProperty('Color By', arrayName)
        self._updateScalarBar()

    def hasDataSet(self, dataSet):
        return self.model.polyDataObj.hasDataSet(dataSet)

    def _updateScalarBar(self):
        self.model.polyDataObj.setProperty('Show Scalar Bar', self.getProperty('Show Scalar Bar'))


class LidarItem(om.ObjectModelItem):

    def __init__(self, model):

        om.ObjectModelItem.__init__(self, model.sensorName, om.Icons.EyeOff)

        self.model = model
        self.scalarBarWidget = None
        self.addProperty('Color By', 0,
                         attributes=om.PropertyAttributes(enumNames=['Solid Color', 'Intensity', 'Z Coordinate', 'Range', 'Spindle Angle', 'Azimuth', 'Camera RGB', 'Scan Delta']))
        self.addProperty('Show Scalar Bar', False)
        self.addProperty('Updates Enabled', True)
        self.addProperty('Min Range', model.reader.GetDistanceRange()[0],
                         attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
        self.addProperty('Max Range', model.reader.GetDistanceRange()[1],
                         attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
        self.addProperty('Edge Filter Angle', model.reader.GetEdgeAngleThreshold(),
                         attributes=om.PropertyAttributes(decimals=0, minimum=0.0, maximum=60.0, singleStep=1, hidden=False))
        self.addProperty('Number of Scan Lines', model.numberOfScanLines,
                         attributes=om.PropertyAttributes(decimals=0, minimum=0, maximum=5000, singleStep=1, hidden=False))
        self.addProperty('Visible', model.visible)
        self.addProperty('Point Size', model.pointSize,
                         attributes=om.PropertyAttributes(decimals=0, minimum=-1, maximum=20, singleStep=1, hidden=False))
        self.addProperty('Alpha', model.alpha,
                         attributes=om.PropertyAttributes(decimals=2, minimum=0, maximum=1.0, singleStep=0.1, hidden=False))
        self.addProperty('Min Height', model.reader.GetHeightRange()[0],
                         attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
        self.addProperty('Max Height', model.reader.GetHeightRange()[1],
                         attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))

        #self.addProperty('Color', QtGui.QColor(255,255,255))
        #self.addProperty('Scanline Color', QtGui.QColor(255,0,0))

    def _onPropertyChanged(self, propertySet, propertyName):
        om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Updates Enabled':
            if self.getProperty('Updates Enabled'):
                self.model.start()
            else:
                self.model.stop()

        elif propertyName == 'Edge Filter Angle':
            self.model.reader.SetEdgeAngleThreshold(self.getProperty('Edge Filter Angle'))
            #    self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName == 'Alpha':
            self.model.setAlpha(self.getProperty(propertyName))

        elif propertyName == 'Visible':
            self.model.setVisible(self.getProperty(propertyName))

        elif propertyName == 'Point Size':
            self.model.setPointSize(self.getProperty(propertyName))

        elif propertyName == 'Number of Scan Lines':
            self.model.numberOfScanLines = self.getProperty(propertyName)
            self.model.initScanLines()

        elif propertyName in ('Min Range', 'Max Range'):
            self.model.reader.SetDistanceRange(self.getProperty('Min Range'), self.getProperty('Max Range'))
            #    self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName in ('Min Height', 'Max Height'):
            self.model.reader.SetHeightRange(self.getProperty('Min Height'), self.getProperty('Max Height'))
            #    self.model.showRevolution(self.model.displayedRevolution)

        elif propertyName == 'Color By':
            self._updateColorBy()

        elif propertyName == 'Show Scalar Bar':
            self._updateScalarBar()

        self.model.polyDataObj._renderAllViews()


    def _updateColorBy(self):

        arrayMap = {
          0 : 'Solid Color',
          1 : 'intensity',
          2 : 'z',
          3 : 'distance',
          4 : 'spindle_angle',
          5 : 'azimuth',
          6 : 'rgb',
          7 : 'scan_delta'
          }

        colorBy = self.getProperty('Color By')
        arrayName = arrayMap.get(colorBy)

        self.model.setColorBy(arrayName)
        self._updateScalarBar()

    def hasDataSet(self, dataSet):
        return self.model.polyDataObj.hasDataSet(dataSet)

    def _updateScalarBar(self):
        self.model.polyDataObj.setProperty('Show Scalar Bar', self.getProperty('Show Scalar Bar'))


class SpindleAxisDebug(vis.PolyDataItem):

    def __init__(self, frameProvider):
        vis.PolyDataItem.__init__(self, 'spindle axis', vtk.vtkPolyData(), view=None)
        self.frameProvider = frameProvider
        self.timer = TimerCallback()
        self.timer.callback = self.update
        self.setProperty('Color', QtGui.QColor(0, 255, 0))
        self.setProperty('Visible', False)

    def _onPropertyChanged(self, propertySet, propertyName):
        vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Visible':
            if self.getProperty(propertyName):
                self.timer.start()
            else:
                self.timer.stop()

    def onRemoveFromObjectModel(self):
        vis.PolyDataItem.onRemoveFromObjectModel(self)
        self.timer.stop()

    def update(self):

        t = self.frameProvider.getFrame('MULTISENSE_SCAN')

        p1 = [0.0, 0.0, 0.0]
        p2 = [2.0, 0.0, 0.0]

        p1 = t.TransformPoint(p1)
        p2 = t.TransformPoint(p2)

        d = DebugData()
        d.addSphere(p1, radius=0.01, color=[0,1,0])
        d.addLine(p1, p2, color=[0,1,0])
        self.setPolyData(d.getPolyData())



class MultiSenseSource(TimerCallback):

    def __init__(self, view):
        TimerCallback.__init__(self)
        self.view = view
        self.reader = None
        self.displayedRevolution = -1
        self.lastScanLine = 0
        self.numberOfScanLines = 1
        self.nextScanLineId = 0
        self.scanLines = []
        self.pointSize = 1
        self.alpha = 0.5
        self.visible = True
        self.initScanLines()

        self.revPolyData = vtk.vtkPolyData()
        self.polyDataObj = vis.PolyDataItem('Multisense Sweep', self.revPolyData, view)
        self.polyDataObj.actor.SetPickable(1)

        self.setPointSize(self.pointSize)
        self.setAlpha(self.alpha)
        self.targetFps = 60
        self.showRevolutionCallback = None
        self.colorizeCallback = None

    def initScanLines(self):

        for scanLine in self.scanLines:
            scanLine.removeFromAllViews()

        self.scanLines = []
        self.nextScanLineId = 0
        self.lastScanLine = max(self.lastScanLine - self.numberOfScanLines, 0)

        for i in xrange(self.numberOfScanLines):
            polyData = vtk.vtkPolyData()
            scanLine = vis.PolyDataItem('scan line %d' % i, polyData, self.view)
            scanLine.actor.SetPickable(0)
            scanLine.setSolidColor((1,0,0))
            self.scanLines.append(scanLine)

    def getScanToLocal(self):
        return None

    def showRevolution(self, revId):

        self.reader.GetDataForRevolution(revId, self.revPolyData)
        self.displayedRevolution = revId

        if self.showRevolutionCallback:
            self.showRevolutionCallback()
        if self.colorizeCallback and self.polyDataObj.getPropertyEnumValue('Color By') == 'rgb':
            self.colorizeCallback()

        self.polyDataObj._updateColorByProperty()
        self.polyDataObj._updateColorBy()

        if self.polyDataObj.getProperty('Visible'):
            self.view.render()


    def setPointSize(self, pointSize):
        for scanLine in self.scanLines:
            scanLine.setProperty('Point Size', pointSize + 2)
        self.polyDataObj.setProperty('Point Size', pointSize)

    def setAlpha(self, alpha):
        self.alpha = alpha
        for scanLine in self.scanLines:
            scanLine.setProperty('Alpha', alpha)
        self.polyDataObj.setProperty('Alpha', alpha)

    def setVisible(self, visible):
        self.visible = visible
        for scanLine in self.scanLines:
            scanLine.setProperty('Visible', visible)
        self.polyDataObj.setProperty('Visible', visible)

    def start(self):
        if self.reader is None:
            self.reader = drc.vtkMultisenseSource()
            self.reader.InitBotConfig(drcargs.args().config_file)
            self.reader.SetDistanceRange(0.25, 4.0)
            self.reader.SetHeightRange(-80.0, 80.0)
            self.reader.Start()

        self.setIntensityRange(400, 4000)

        TimerCallback.start(self)

    def updateScanLines(self):

        if not self.numberOfScanLines:
            return

        currentScanLine = self.reader.GetCurrentScanLine() - 1
        scanLinesToUpdate = currentScanLine - self.lastScanLine
        scanLinesToUpdate = min(scanLinesToUpdate, self.numberOfScanLines)

        if not scanLinesToUpdate:
            return

        #print 'current scanline:', currentScanLine
        #print 'scan lines to update:', scanLinesToUpdate
        #print 'updating actors:', self.nextScanLineId, (self.nextScanLineId + (scanLinesToUpdate-1)) % self.numberOfActors
        #print 'updating scan lines:', self.lastScanLine + 1, self.lastScanLine + 1 + (scanLinesToUpdate-1)

        for i in xrange(scanLinesToUpdate):
            scanLine = self.scanLines[(self.nextScanLineId + i) % self.numberOfScanLines]
            self.reader.GetDataForScanLine(self.lastScanLine + i + 1, scanLine.polyData)

        self.lastScanLine = currentScanLine
        self.nextScanLineId = (self.nextScanLineId + scanLinesToUpdate) % self.numberOfScanLines

        if self.scanLines[0].getProperty('Visible'):
            self.view.render()


    def updateRevolution(self):
        currentRevolution = self.reader.GetCurrentRevolution() - 1
        if currentRevolution == self.displayedRevolution:
            return

        self.showRevolution(currentRevolution)

    def getSpindleAxis(self):
        return self.getAxis('MULTISENSE_SCAN', [1.0, 0.0, 0.0])

    def getAxis(self, frameName, axisVector):
        t = self.getFrame(frameName)
        return np.array(t.TransformVector(axisVector))

    def getFrame(self, name, relativeTo='local'):
        t = vtk.vtkTransform()
        self.reader.GetTransform(name, relativeTo, self.reader.GetCurrentScanTime(), t)
        return t

    def tick(self):
        self.updateRevolution()
        self.updateScanLines()


    @staticmethod
    def setLidarRevolutionTime(secondsPerRevolution):

        assert secondsPerRevolution >= 1.0

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = -1
        m.gain = -1
        m.agc = -1
        m.rpm = 60.0 / (secondsPerRevolution)

        lcmUtils.publish('MULTISENSE_COMMAND', m)

    @staticmethod
    def setLidarRpm(rpm):

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = -1
        m.gain = -1
        m.agc = -1
        m.rpm = rpm

        lcmUtils.publish('MULTISENSE_COMMAND', m)

    @staticmethod
    def sendMultisenseCommand(fps, gain, exposure, agc, rpm, led_flash, led_duty):

        m = lcmmultisense.command_t()
        m.utime = getUtime()
        m.fps = fps
        m.gain = gain
        m.exposure_us = exposure
        m.agc = agc
        m.rpm = rpm
        m.leds_flash = led_flash
        m.leds_duty_cycle = led_duty

        lcmUtils.publish('MULTISENSE_COMMAND', m)

    @staticmethod
    def setNeckPitch(neckPitchDegrees):
        assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90

        jointGroups = drcargs.getDirectorConfig()['teleopJointGroups']
        jointGroupNeck = filter(lambda group: group['name'] == 'Neck', jointGroups)

        neckJoints = jointGroupNeck[0]['joints']
        if (len(neckJoints) == 1): # Atlas
            jointPositions = [math.radians(neckPitchDegrees)]
        elif (len(neckJoints) == 3): # Valkyrie, assuming joint 0 is lowerNeckPitch
            jointPositions = [math.radians(neckPitchDegrees), 0, 0]
        else:
            return

        # Assume first neck joint is the pitch joint
        m = lcmbotcore.joint_angles_t()
        m.utime = getUtime()
        m.num_joints = len(neckJoints)
        m.joint_name = neckJoints
        m.joint_position = jointPositions
        lcmUtils.publish('DESIRED_NECK_ANGLES', m)

    def setIntensityRange(self, lowerBound, upperBound):
        self.polyDataObj.setRangeMap('intensity', [lowerBound, upperBound])



class LidarSource(TimerCallback):

    def __init__(self, view, channelName, coordinateFrame, sensorName, intensityRange):
        TimerCallback.__init__(self)
        self.view = view
        self.channelName = channelName
        self.reader = None
        self.displayedRevolution = -1
        self.lastScanLine = 0
        self.numberOfScanLines = 1000
        self.nextScanLineId = 0
        self.scanLines = []
        self.pointSize = 1
        self.alpha = 0.5
        self.visible = True
        self.colorBy = 'Solid Color'
        self.initScanLines()
        self.sensorName = sensorName
        self.coordinateFrame = coordinateFrame

        self.revPolyData = vtk.vtkPolyData()
        self.polyDataObj = vis.PolyDataItem('Lidar Sweep', self.revPolyData, view)
        self.polyDataObj.actor.SetPickable(1)

        self.polyDataObj.setRangeMap('intensity', intensityRange)

        self.setPointSize(self.pointSize)
        self.setAlpha(self.alpha)
        self.targetFps = 60
        self.colorizeCallback = None

    def initScanLines(self):

        for scanLine in self.scanLines:
            scanLine.removeFromAllViews()

        self.scanLines = []
        self.nextScanLineId = 0
        self.lastScanLine = max(self.lastScanLine - self.numberOfScanLines, 0)

        for i in xrange(self.numberOfScanLines):
            polyData = vtk.vtkPolyData()
            scanLine = vis.PolyDataItem('scan line %d' % i, polyData, self.view)
            scanLine.actor.SetPickable(0)
            #scanLine.setSolidColor((0,1,0))
            self.scanLines.append(scanLine)

    def getScanToLocal(self):
        return None

    def setPointSize(self, pointSize):
        for scanLine in self.scanLines:
            scanLine.setProperty('Point Size', pointSize + 2)
        self.polyDataObj.setProperty('Point Size', pointSize)

    def setAlpha(self, alpha):
        self.alpha = alpha
        for scanLine in self.scanLines:
            scanLine.setProperty('Alpha', alpha)
        self.polyDataObj.setProperty('Alpha', alpha)

    def setVisible(self, visible):
        self.visible = visible
        for scanLine in self.scanLines:
            scanLine.setProperty('Visible', visible)
        self.polyDataObj.setProperty('Visible', visible)

    def setColorBy(self, colorBy):
        self.colorBy = colorBy
        for scanLine in self.scanLines:
            if colorBy and colorBy in scanLine.getArrayNames():
                scanLine.colorBy(self.colorBy)
            elif colorBy == "Solid Color":
                scanLine.setSolidColor((1,1,1))

    def start(self):
        if self.reader is None:
            self.reader = drc.vtkLidarSource()
            self.reader.subscribe(self.channelName)
            self.reader.setCoordinateFrame(self.coordinateFrame)
            self.reader.InitBotConfig(drcargs.args().config_file)
            self.reader.SetDistanceRange(0.25, 80.0)
            self.reader.SetHeightRange(-80.0, 80.0)
            self.reader.Start()

        TimerCallback.start(self)

    def updateScanLines(self):

        if not self.numberOfScanLines:
            return

        currentScanLine = self.reader.GetCurrentScanLine() - 1
        scanLinesToUpdate = currentScanLine - self.lastScanLine
        scanLinesToUpdate = min(scanLinesToUpdate, self.numberOfScanLines)

        if not scanLinesToUpdate:
            return

        #print 'current scanline:', currentScanLine
        #print 'scan lines to update:', scanLinesToUpdate
        #print 'updating actors:', self.nextScanLineId, (self.nextScanLineId + (scanLinesToUpdate-1)) % self.numberOfActors
        #print 'updating scan lines:', self.lastScanLine + 1, self.lastScanLine + 1 + (scanLinesToUpdate-1)

        for i in xrange(scanLinesToUpdate):
            scanLine = self.scanLines[(self.nextScanLineId + i) % self.numberOfScanLines]
            self.reader.GetDataForScanLine(self.lastScanLine + i + 1, scanLine.polyData)
            if self.colorBy and self.colorBy in scanLine.getArrayNames():
                scanLine.colorBy(self.colorBy)

        self.lastScanLine = currentScanLine
        self.nextScanLineId = (self.nextScanLineId + scanLinesToUpdate) % self.numberOfScanLines

        if self.scanLines[0].getProperty('Visible'):
            self.view.render()

    def getPolyData(self):
        self.revPolyData = vtk.vtkPolyData()
        self.reader.GetDataForHistory(self.numberOfScanLines, self.revPolyData)
        vis.updatePolyData( self.revPolyData , 'point cloud', colorByName=self.colorBy)

    def tick(self):
        self.updateScanLines()

    def setIntensityRange(self, lowerBound, upperBound):
        self.polyDataObj.setRangeMap('intensity', [lowerBound, upperBound])

class NeckDriver(object):

    def __init__(self, view, getNeckPitchFunction):
        self.active = False
        self.view = view
        self.delta = 0
        self.textObj = None
        self._getNeckPitchFunction = getNeckPitchFunction
        self.presetMap = {0:0, 1:60, 2:45, 3:30}

    def showText(self, neckPitch):
        if not self.view:
            return
        self.textObj = vis.updateText('Neck pitch: %d' % neckPitch, 'Neck Pitch Annotation', parent='spindle axis')

    def cleanupText(self):
        if self.textObj:
            om.removeFromObjectModel(self.textObj)
            self.textObj = None

    def activateNeckControl(self):
        if not self.active:
            self.active = True
            self.delta = 0

    def getNeckPitchDegrees(self):
        return math.degrees(self._getNeckPitchFunction())

    def applyNeckPitchPreset(self, presetId):
        if not self.active:
            return False
        self.setNeckPitch(self.presetMap[presetId])
        return True


    def applyNeckPitchDelta(self, delta):
        if delta == 0:
            return

        neckPitch = self.getNeckPitchDegrees() + delta
        neckPitch = np.clip(neckPitch, -90, 90)
        self.setNeckPitch(neckPitch)

    def setNeckPitch(self, neckPitch):
        MultiSenseSource.setNeckPitch(neckPitch)

    def deactivateNeckControl(self):
        if self.active:
            self.active = False
            self.applyNeckPitchDelta(self.delta)
            self.cleanupText()
            self.delta = 0

    def onWheelDelta(self, delta):
        if not self.active:
            return False

        self.delta += np.clip(delta, -1, 1)
        neckPitch = self.getNeckPitchDegrees() + self.delta
        self.showText(neckPitch)
        return True


class SpindleMonitor(object):
    def __init__(self, getSpindleAngleFunction):
        self.lastSpindleAngle = 0
        self.lastStateTime = 0
        self.spindleSpinRateAverager = MovingAverageComputer()
        self.spindleSpinRateAverager.timeWindow = 0.5
        self._getSpindleAngleFunction = getSpindleAngleFunction

    def onRobotStateChanged(self, newState):
        t, newAngle = self._getSpindleAngleFunction()
        elapsed = t - self.lastStateTime
        if (elapsed > 0.001 and elapsed < 100):
            # unwrap
            diff = newAngle - self.lastSpindleAngle
            if (abs(diff - 2*math.pi) < abs(diff)):
                diff = diff - 2*math.pi
            if (abs(diff + 2*math.pi) < abs(diff)):
                diff = diff + 2*math.pi
            velocity = diff / elapsed
            self.spindleSpinRateAverager.update(velocity)
            # if avg veloicty is bad panic
        self.lastStateTime = t
        self.lastSpindleAngle = newAngle

    def getAverageSpindleVelocity(self):
        return self.spindleSpinRateAverager.getAverage()


class MapServerSource(TimerCallback):

    def __init__(self, view, callbackFunc=None):
        TimerCallback.__init__(self)
        self.reader = None
        self.folder = None
        self.view = view
        self.displayedMapIds = {}
        self.polyDataObjects = {}
        self.targetFps = 10
        self.callbackFunc = callbackFunc
        self.colorizeCallback = None
        self.useMeshes = True

    def getNameForViewId(self, viewId):

        for typeName, typeValue in lcmmaps.data_request_t.__dict__.iteritems():
            if typeValue == viewId:
                return typeName

        return 'Map View ' + str(viewId)

    def updatePolyData(self, viewId, polyData):

        obj = self.polyDataObjects.get(viewId)
        if obj not in om.getObjects():
            obj = None
        if not obj:
            hiddenMapIds = [9999]
            visibleDefault = False if viewId in hiddenMapIds else True
            obj = vis.PolyDataItem(self.getNameForViewId(viewId), polyData, self.view)

            obj.setProperty('Visible', visibleDefault)
            if obj._isPointCloud():
                obj.setProperty('Color', [1, 1, 1])
                obj.setProperty('Alpha', 0.5)
            else:
                obj.setProperty('Color', [0, 0.68, 1])

            if viewId == lcmmaps.data_request_t.HEIGHT_MAP_SCENE:
                obj.setProperty('Surface Mode', 'Wireframe')

            folder = om.findObjectByName('Map Server')
            folder.addProperty('Min Range', self.reader.GetDistanceRange()[0],
                             attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
            folder.addProperty('Max Range', self.reader.GetDistanceRange()[1],
                             attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
            folder.addProperty('Edge Filter Angle', self.reader.GetEdgeAngleThreshold(),
                         attributes=om.PropertyAttributes(decimals=0, minimum=0.0, maximum=60.0, singleStep=1, hidden=False))
            folder.addProperty('Min Height', self.reader.GetHeightRange()[0],
                             attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
            folder.addProperty('Max Height', self.reader.GetHeightRange()[1],
                             attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
            om.addToObjectModel(obj, folder)
            om.expand(folder)
            self.folder = folder
            self.polyDataObjects[viewId] = obj
        else:
            obj.setPolyData(polyData)

        if self.colorizeCallback:
            self.colorizeCallback(obj)

    def showMap(self, viewId, mapId):
        polyData = vtk.vtkPolyData()

        if self.useMeshes:
            self.reader.GetMeshForMapId(viewId, mapId, polyData)
        else:
            self.reader.GetDataForMapId(viewId, mapId, polyData)

        self.updatePolyData(viewId, polyData)
        self.displayedMapIds[viewId] = mapId

        if self.callbackFunc:
            self.callbackFunc()

    def getSceneHeightData(self):
        return self.getDepthMapData(lcmmaps.data_request_t.HEIGHT_MAP_SCENE)

    def getDepthMapData(self, viewId):

        mapId = self.reader.GetCurrentMapId(viewId)
        if mapId < 0:
            return None, None

        depthImage = vtk.vtkImageData()
        transform = vtk.vtkTransform()
        self.reader.GetDataForMapId(viewId, mapId, depthImage, transform)

        dims = depthImage.GetDimensions()
        d = vnp.getNumpyFromVtk(depthImage, 'ImageScalars')
        d = d.reshape(dims[1], dims[0])
        t = np.array([[transform.GetMatrix().GetElement(r, c) for c in xrange(4)] for r in xrange(4)])

        return d, t

    def start(self):
        if self.reader is None:
            self.reader = drc.vtkMapServerSource()
            self.reader.Start()

        TimerCallback.start(self)

    def updateMap(self):
        if (self.folder):
            self.reader.SetDistanceRange(self.folder.getProperty('Min Range'), self.folder.getProperty('Max Range'))
            self.reader.SetEdgeAngleThreshold(self.folder.getProperty('Edge Filter Angle'))
            self.reader.SetHeightRange(self.folder.getProperty('Min Height'), self.folder.getProperty('Max Height'))
            
        viewIds = self.reader.GetViewIds()
        viewIds = vnp.numpy_support.vtk_to_numpy(viewIds) if viewIds.GetNumberOfTuples() else []
        for viewId in viewIds:
            mapId = self.reader.GetCurrentMapId(viewId)
            if viewId not in self.displayedMapIds or mapId != self.displayedMapIds[viewId]:
                self.showMap(viewId, mapId)

    def tick(self):
        self.updateMap()


def init(view):
    global _multisenseItem
    global multisenseDriver

    sensorsFolder = om.getOrCreateContainer('sensors')

    m = MultiSenseSource(view)
    m.start()
    multisenseDriver = m
    _multisenseItem = MultisenseItem(m)
    om.addToObjectModel(_multisenseItem, sensorsFolder)

    queue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
    queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
    lidarNames = queue.getLidarNames()
    for lidar in lidarNames:
        if queue.displayLidar(lidar):
            
            l = LidarSource(view, queue.getLidarChannelName(lidar), queue.getLidarCoordinateFrame(lidar), queue.getLidarFriendlyName(lidar), queue.getLidarIntensity(lidar))
            l.start()
            lidarDriver = l
            _lidarItem = LidarItem(l)
            om.addToObjectModel(_lidarItem, sensorsFolder)


    useMapServer = hasattr(drc, 'vtkMapServerSource')
    if useMapServer:
        mapServerSource = MapServerSource(view, callbackFunc=view.render)
        mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot)
        mapsServerContainer.source = mapServerSource
        om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
        mapServerSource.start()
    else:
        mapServerSource = None


    spindleDebug = SpindleAxisDebug(multisenseDriver)
    spindleDebug.addToView(view)
    om.addToObjectModel(spindleDebug, sensorsFolder)

    return multisenseDriver, mapServerSource