class RepeatibilityTester():
    def __init__(self, armName, errorModelFile, outputMatFile='repeatability.mat', numPoses=10, numTries=1, speed=0.01):
        self.speed = speed
        self.arm = armName
        self.outputMatFile = outputMatFile

        self.errorModel = None
        if armName == 'L':
            self.errorModel = error_model.RavenErrorModel(leftModelFile=errorModelFile)
        else:
            self.errorModel = error_model.RavenErrorModel(rightModelFile=errorModelFile)

        self.ravenArm = RavenArm(armName, defaultPoseSpeed=speed)
        self.ravenPlanner = RavenPlanner(armName, errorModel= self.errorModel , addNoise=False)
        self.gripperPoseEstimator = GripperPoseEstimator(armName)
        
        receptaclePose = tfx.pose([-.060, .010, -.135], tfx.tb_angles(-90,90,0),frame='0_link')
        if armName == 'R':
            self.holdingPose = receptaclePose + [-.04, -.03, .03]
            self.homePose = raven_constants.HomePose.Right
        else:
            self.holdingPose = receptaclePose + [.04, -.03, .03]
            self.homePose = raven_constants.HomePose.Left
        
        self.homePoseConverted, dummy = self.errorModel.predictSinglePose(self.arm, self.homePose, self.homePose)
        
        
        self.grasp = DEFAULT_GRASP
        self.numPoses = numPoses
        self.numTries = numTries
        self.observedPose = None

        self.allErrors = np.zeros((0,6))
        
        self.data = {}
        self.data[POSE_KEY] = []
        self.data[MEAN_KEY] = []
        self.data[MEDIAN_KEY] = []
        self.data[RMS_KEY] = []
        self.data[MIN_KEY] = []
        self.data[MAX_KEY] = []
        self.data[COV_KEY] = []
        
        self.running = False
        self.trajNum = 0
        self.filename = 'test_%d.pkl'

        #p, d = self.sample()
        #IPython.embed()
        x_inc = .04
        y_inc = .08
        x_levels = 20
        y_levels = 40
        angles = [tfx.tb_angles(-90,90,0)]
        if armName == 'L':
            foamHome = tfx.pose([-.03,-.02,-.16],tfx.tb_angles(-90,90,0), frame='0_link')
        else:
            foamHome = tfx.pose([-.08,-.02,-.16],tfx.tb_angles(-90,90,0), frame='0_link')
        self.foamPoseIndex = 0
        self.foamPoseGrid = self.getGrid(foamHome, x_inc, y_inc, x_levels, y_levels, angles)
        
        random.seed(4000)
        random.shuffle(self.foamPoseGrid)

        #print raven_constants.GripperTape.Topic + '_' + armName
        rospy.Subscriber(PHASESPACE_POSE_TOPIC+ '_' + armName, PoseStamped, self.cameraPoseCallback)
    
    def cameraPoseCallback(self, msg):
        
        # convert to 0 link and post
        try:
            posePhasespace = tfx.pose(msg)
            self.observedPose = tfx.convertToFrame(posePhasespace, '0_link', wait=1.0)
        except:
            failCount = 1

    def getPhasespacePose(self):
        tries = 0
        while self.observedPose is None and tries < 200:
            rospy.sleep(0.01)
            tries = tries + 1
        ret = None
        if tries < 200:
            ret = self.observedPose.copy()
        self.observedPose = None
        return ret
    
    def startDataCollection(self):
        if not self.running:
            rospy.loginfo('Starting data collection...')
            self.running = True
            self.collectionFinished = False
            self.dataRecorder = data_recorder.DataRecorder(self.arm)
        
    def stopDataCollection(self, filename, sort=False, plot=False, targetPose=None):
        sync = True
        if self.running:
            rospy.loginfo('Stopping data collection')
            self.dataRecorder.stop_recording(sort)
            if plot:
                self.dataRecorder.plot(self.armName)
            if self.dataRecorder.write(filename, synchronized=sync, targetPose=targetPose):
                self.trajNum = self.trajNum+1
            self.running = False
            self.collectionFinished = True
            rospy.loginfo('Saved data')

    def getGrid(self, homePose, x_inc, y_inc, x_levels, y_levels, angles):
        grid = []
            
        for i in xrange(x_levels):
            for j in xrange(y_levels):
                for k in xrange(len(angles)):
                    # positive x
                    pose = homePose + [float(i) * (x_inc / float(x_levels)), float(j) * (-y_inc / y_levels), 0]
                    pose.tb_angles = angles[k]
                    grid += [pose.copy()]
                    # negative x
                    pose = homePose + [-float(i) * (x_inc / float(x_levels)), float(j) * (-y_inc / y_levels), 0]
                    pose.tb_angles = angles[k]
                    grid += [pose.copy()]
                    
        return grid
    
    def computeErrors(self, poseErrors):
        meanError = np.mean(np.abs(poseErrors), axis=0)
        medianError = np.median(np.abs(poseErrors), axis=0)
        rmsError = np.sqrt(np.mean(np.square(poseErrors), axis=0))
        minError = np.min(np.abs(poseErrors), axis=0)
        maxError = np.max(np.abs(poseErrors), axis=0)
        stdError = np.cov(np.abs(poseErrors).T)
        return PoseError(meanError, medianError, rmsError, minError, maxError, stdError)
    
    def storeErrors(self, repeatabilityErrors):
        
        for (targetPose, poseError) in repeatabilityErrors:
            self.data[POSE_KEY].append(targetPose)
            self.data[MEAN_KEY].append(poseError.meanError)
            self.data[MEDIAN_KEY].append(poseError.medianError)
            self.data[MIN_KEY].append(poseError.minError)
            self.data[MAX_KEY].append(poseError.maxError)
            self.data[RMS_KEY].append(poseError.rmsError)
            self.data[COV_KEY].append(poseError.stdError)
        
        scipy.io.savemat(self.outputMatFile, self.data)
        
    def sample(self):
        delta = tfx.pose([0,0,0], frame='0_link')
        pose = self.foamPoseGrid[self.foamPoseIndex]
        self.foamPoseIndex = self.foamPoseIndex+1

        return pose, delta
    
    def move(self, targetPose = None):
        poseErrors = np.zeros((0,6))
        deltaPose = tfx.pose([0,0,0])
        if targetPose == None:
            targetPose, deltaPose = self.sample()
        
        #cameraEndPose, dummy = self.sample()

        for j in range(self.numTries):
            if rospy.is_shutdown():
                return None
            try:
                # reset
                self.ravenArm.goToGripperPose(self.holdingPose)
                self.ravenArm.setGripper(self.grasp, duration=1.0)
                rospy.sleep(1)
                
                startPose = None
                while startPose is None:
                    if rospy.is_shutdown():
                        return None
                    startPose = self.getPhasespacePose()
                
                n_steps = 20
                robotStartPose = self.gripperPoseEstimator.getGripperPose(self.arm).copy()

                print robotStartPose
                # used for error model
                # cameraStartPose = tfx.pose([-0.107, 0.9899, 0.0930, -0.0455, -0.2510, 0.0636, -0.9659, -0.019, -0.9621, -0.1267, 0.2416, -0.0949, 0, 0, 0, 1.0000])
                # cameraEndPose   = tfx.pose([0.1263, 0.9422, 0.3103, 0.0063, -0.3207, 0.3348, -0.8860, -0.0304, -0.9387, 0.0124, 0.3445, -0.1530, 0, 0, 0, 1.0000])

                # startPose = tfx.pose([0.0008, 1.000, 0.0003, -0.0200, 0.0012, 0.0003, -1.0000, -0.0197, -1.000, 0.0008, -0.0012, -0.1051, 0, 0, 0, 1.0000])
                # endPose = tfx.pose([-0.0000, 1.0000, 0.0000, 0.0355, 0.0008, 0.0000, -1.0000, -0.0200, -1.000, -0.0000, -0.0008, -0.1580, 0, 0, 0, 1.0000])

                # cameraStartPose.frame = '0_link'
                # cameraEndPose.frame = '0_link'
                # startPose.frame = '0_link'
                # endPose.frame = '0_link'

                # good trajectory
                cameraStartPose = tfx.pose([-0.1075, 0.9868, 0.1208, -0.0468, -0.3099, 0.0822, -0.9472, -0.0201, -0.9447, -0.1393, 0.2970, -0.0968,  0, 0, 0, 1.0000])
                #cameraEndPose   = tfx.pose([-0.1163, 0.9858, 0.1207, -0.1170, -0.2292, 0.0916, -0.9691, -0.0582, -0.9664, -0.1404, 0.2153, -0.1489, 0, 0, 0, 1.0000])
                #cameraEndPose   = tfx.pose([0, 1, 0, -0.02982, 0, 0, -1, -0.07282, -1, 0, 0, -0.162, 0, 0, 0, 1.0000])

                #startPose = tfx.pose([-0.0003, 1.0000, -0.0001, -0.0200, 0.0003, -0.0001, -1.0000, -0.0200, -1.0000, -0.0003, -0.0003, -0.1050, 0, 0, 0, 1.0000])
                endPose   = tfx.pose([-0.0001, 1.0000, -0.0000, -0.0755, -0.0019, -0.0000, -1.0000, -0.0605, -1.0000, -0.0001, 0.0019, -0.1698, 0, 0, 0, 1.0000])

                cameraEndPose   = tfx.pose([-0.0825, -0.0603, -0.1487], [0.5, 0.5, -0.5, 0.5])
                startPose = tfx.pose([-0.0995, -0.0213, -0.1064], [0.4936, 0.5026, -0.5029, 0.5007])

                cameraStartPose.frame = '0_link'
                cameraEndPose.frame = '0_link'
                startPose.frame = '0_link'
                endPose.frame = '0_link'

                '''
                startPose = tfx.pose([-0.0210, 0.9996, -0.0188, -0.0204, 0.0127, -0.0191, -0.9997, -0.0209, -0.9997, -0.0208, 0.0131, -0.1053, 0, 0,0, 1])
                #startPose.frame = '0_link'

                #[-0.0521, 0.9986, 0.0037, 0.0082, -0.0099, 0.0032, -0.9999, -0.0886, -0.9986, -0.0521, 0.0098, -0.1693, 0,0,0, 1.000]
                cameraStartPose = tfx.pose([-0.0943, 0.9917, 0.0874, -0.0458, -0.2127, 0.0657, -0.9749, -0.0188, -0.9726, -0.1105, 0.2047, -0.0950, 0, 0, 0, 1])
                cameraStartPose.frame = '0_link'

                cameraEndPose = tfx.pose([-0.0559, 0.9882, 0.1424, -0.0357, -0.3918, 0.1094, -0.9135, -0.0839, -0.9183, -0.1069, 0.3811, -0.1343, 0,0,0,1])
                cameraEndPose.frame = '0_link'

                endPose = tfx.pose([0, 1.000, 0, 0.0025, 0, 0, -1.0, -0.0733, -1, 0, 0, -0.1510, 0,0,0,1])
                endPose.frame = '0_link'
                #IPython.embed()
                '''
                
                (correctedStartPose, deltaPoseTraj) = self.ravenPlanner.getFullTrajectoryFromPose(self.ravenArm.name, cameraEndPose,
                    startPose=startPose, n_steps=n_steps, approachDir=np.array([0,.1,.9]), speed=self.speed, correctTrajectory=True)

            except RuntimeError as e:
                rospy.loginfo(e)
                return 'IKFailure'  
            
            print 'Executing trajectory attempt', j
            #self.startDataCollection()
            #rospy.sleep(1)

            #rospy.sleep(2)
            self.ravenArm.executeDeltaPoseTrajectory(deltaPoseTraj, block=True, startPose=correctedStartPose)
        
            #rospy.sleep(5)
            #self.stopDataCollection(self.filename %(self.trajNum), plot=False, targetPose=cameraEndPose)

            rospy.sleep(2)
            actualPose = self.getPhasespacePose()
            errorDeltaPose = raven_util.deltaPose(actualPose, cameraEndPose)
            translationError = errorDeltaPose.translation.array
            angleError = np.array(transformations.euler_from_matrix(errorDeltaPose.matrix[:3,:3]))
            
            print
            print 'ERROR'
            print translationError
            print angleError
            print
            errorVector = np.c_[[translationError], [angleError]]
            poseErrors = np.r_[poseErrors, errorVector]
            rospy.sleep(1)

        return (targetPose.matrix, poseErrors)
        
        
    def run(self):
        self.ravenArm.start()
        
        rospy.loginfo('Press enter to start')
        raw_input()
        
        rospy.loginfo('Generating pose samples...')
        
        self.ravenArm.goToGripperPose(self.holdingPose)
        self.ravenArm.setGripper(self.grasp, duration=1.0)
        repeatabilityErrors = []
        targetPose = None
        
        for i in range(self.numPoses):
            poseErrorTuple = self.move(targetPose=targetPose)
            
            poseErrorSummary = self.computeErrors(poseErrorTuple[1])
            print "Pose errors"
            print poseErrorSummary

            repeatabilityErrors.append((poseErrorTuple[0], poseErrorSummary))
            self.allErrors = np.r_[self.allErrors, poseErrorTuple[1]]

            if rospy.is_shutdown():
                break

        print 'Done with move'

        # compute statistics of all tries
        poseErrorSummary = self.computeErrors(self.allErrors)
        repeatabilityErrors.append((tfx.pose([0,0,0]), poseErrorSummary))

        self.storeErrors(repeatabilityErrors)
        print 'Done Saving Errors'
        self.ravenArm.stop()
class GridMove():
    def __init__(self, armName, rand=False, x=.03, y=.05, z=.004, testAngles=False, maxPoses=1000, speed=0.01):
        self.ravenArm = RavenArm(armName, defaultPoseSpeed=speed)
        self.arm = armName
        if armName == 'R':
            self.homePose = tfx.pose([-.12,-.02,-.135],tfx.tb_angles(-90,90,0))
        else:
            self.homePose = tfx.pose([-.03,-.02,-.135],tfx.tb_angles(-90,90,0))
        self.random = rand
        print self.homePose
        
        self.grasp = 1.2
        joints_dict = kinematics.invArmKin(self.arm, self.homePose, self.grasp)
        print joints_dict
        
        self.x_levels = 5
        self.y_levels = 5
        self.z_levels = 7
        
        self.x = x
        self.y = y
        self.z = z
        
        self.maxPoses = maxPoses
        
        # generate all angle combinations in order to sample the rotations as well
        self.angles = []
        
        self.angles.append(tfx.tb_angles(-90,90,0))
        
        if False:
            self.angles.append(tfx.tb_angles(-90,80,0))
            self.angles.append(tfx.tb_angles(-90,100,0))
            
            self.angles.append(tfx.tb_angles(-80,90,0))
            self.angles.append(tfx.tb_angles(-80,80,0))
            self.angles.append(tfx.tb_angles(-80,100,0))
            
            self.angles.append(tfx.tb_angles(-100,90,0))
            self.angles.append(tfx.tb_angles(-100,80,0))
            self.angles.append(tfx.tb_angles(-100,100,0))
        
        self.grid = []
        self.grid += [self.homePose + [float(i)*(x/self.x_levels),0,0] for i in xrange(self.x_levels)]
        self.grid += [self.homePose + [x, float(i)*(-y/self.y_levels),0] for i in xrange(self.x_levels)]
        self.grid += [self.homePose + [-float(i)*(x/self.x_levels),-y,0] for i in xrange(self.x_levels)]
        self.grid += [self.homePose + [0, -float(i)*(-y/self.y_levels),0] for i in xrange(self.x_levels)]
        
        """
        self.grid = []
        self.grid += [tfx.pose([x/self.x_levels,0,0]) for _ in xrange(self.x_levels)]
        self.grid += [tfx.pose([0,-y/self.y_levels,0]) for _ in xrange(self.y_levels)]
        self.grid += [tfx.pose([-x/self.x_levels,0,0]) for _ in xrange(self.x_levels)]
        self.grid += [tfx.pose([0,y/self.y_levels,0]) for _ in xrange(self.y_levels)]
        """
        
        """
        # start in front right
        self.grid = [tfx.pose([x,0,-z]),
                     tfx.pose([0,-y,z]),
                     tfx.pose([-x,0,-z]),
                     tfx.pose([0,y,z])]
        """
        
    def getGrid(self, homePose):
        grid = []
        
        for i in xrange(self.x_levels):
            for j in xrange(len(self.angles)):
                pose = homePose + [float(i)*(self.x/self.x_levels),0,0]
                pose.tb_angles = self.angles[j]
                grid += [pose]
        
        for i in xrange(self.x_levels):
            for j in xrange(len(self.angles)):
                pose = homePose + [self.x, float(i)*(-self.y/self.y_levels),0]
                pose.tb_angles = self.angles[j]
                grid += [pose]
                
        for i in xrange(self.x_levels):
            for j in xrange(len(self.angles)):
                pose = homePose + [self.x-float(i)*(self.x/self.x_levels),-self.y,0]
                pose.tb_angles = self.angles[j]
                grid += [pose]
                
        for i in xrange(self.x_levels):
            for j in xrange(len(self.angles)):
                pose = homePose + [0, -self.y+float(i)*(self.y/self.y_levels),0]
                pose.tb_angles = self.angles[j]
                grid += [pose]
        
        return grid
        
    def moveByGrid(self, homePose, maxPoses=1000):
        grid = []
        for _ in xrange(self.z_levels):
            homePose.position.z -= self.z
            grid += self.getGrid(homePose)
        
        print 'moveByGrid'
        print "GRID", len(grid)
        
        if not self.random:
            print 'seeded'
            random.seed(2000)
        random.shuffle(grid)
        
        homePose = grid[0]
        self.ravenArm.goToGripperPose(self.homePose)
        self.ravenArm.setGripper(self.grasp, duration=1.0)
        print 'ROSBAG RECORD WHEN GRIPPER REACHES POSE'
        #rospy.sleep(10.)
        
        i = 0
        for pose in grid:
            print pose
            rospy.sleep(2)
            if rospy.is_shutdown():
                return
            self.ravenArm.goToGripperPose(pose)
            if i > maxPoses:
                break
            i = i+1
        
    def run(self):
        self.ravenArm.start()
        
        rospy.loginfo('Press enter to start')
        raw_input()
        
        homePose = self.homePose
        self.moveByGrid(homePose, self.maxPoses)
                
        if rospy.is_shutdown():
            return
                
        self.ravenArm.stop()