from director.consoleapp import ConsoleApp
from director import robotsystem
from director import visualization as vis
from director import objectmodel as om
from director import ikplanner
from director import ikconstraintencoder as ce

import numpy as np
import pprint
import json


def getRobotState():
    return robotStateJointController.q.copy()


def buildConstraints():
    '''
    For testing, build some constraints and return them in a list.
    '''

    startPose = getRobotState()

    startPoseName = 'plan_start'
    endPoseName = 'plan_end'

    ikPlanner.addPose(startPose, startPoseName)
    ikPlanner.addPose(startPose, endPoseName)

    constraints = []
    constraints.extend(ikPlanner.createFixedFootConstraints(startPoseName))
    constraints.append(ikPlanner.createMovingBaseSafeLimitsConstraint())
    constraints.append(ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
    constraints.append(ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
    constraints.append(ikPlanner.createLockedRightArmPostureConstraint(startPoseName))

    targetFrame = ikPlanner.getLinkFrameAtPose(ikPlanner.getHandLink('left'), startPose)

    p, o = ikPlanner.createPositionOrientationGraspConstraints('left', targetFrame)
    p.tspan = [1.0, 1.0]
    o.tspan = [1.0, 1.0]
    constraints.extend([p, o])

    return constraints



def testPlanConstraints():

    ikPlanner.planningMode = 'dummy'

    constraints = buildConstraints()
    poses = ce.getPlanPoses(constraints, ikPlanner)


    poseJsonStr = json.dumps(poses, indent=4)
    constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)

    print poseJsonStr
    print constraintsJsonStr


    constraints = ce.decodeConstraints(constraintsJsonStr)
    pprint.pprint(constraints)



app = ConsoleApp()
app.setupGlobals(globals())

view = app.createView()
robotsystem.create(view, globals())


testPlanConstraints()

#app.start()

@@ -227,16 +227,87 @@
 er as ce
+%0Afrom director import ikconstraints%0Afrom director import transformUtils
 %0A%0Aimport
@@ -1427,16 +1427,651 @@
 aints%0A%0A%0A
+def reconstructConstraints(constraints):%0A    '''%0A    Convert dicts (decoded from json) back to the original%0A    constraint classes using the 'class' information in the dict%0A    '''%0A    objs = %5B%5D%0A%0A    for c in constraints:%0A        objClass = getattr(ikconstraints, c%5B'class'%5D)%0A        del c%5B'class'%5D%0A        obj = objClass()%0A        objs.append(obj)%0A%0A        for attr, value in c.iteritems():%0A            if isinstance(value, dict) and 'position' in value and 'quaternion' in value:%0A                value = transformUtils.transformFromPose(value%5B'position'%5D, value%5B'quaternion'%5D)%0A            setattr(obj, attr, value)%0A%0A    return objs%0A%0A
 %0Adef tes
@@ -2388,16 +2388,70 @@
 sonStr%0A%0A
+    print '--------------decoding--------------------'
 %0A    con
@@ -2538,58 +2538,249 @@
 s)%0A%0A
-%0A%0Aapp = ConsoleApp()%0Aapp.setupGlobals(global
+    print '--------------reconstructing--------------'%0A    constraints = reconstructConstraints(constraints)%0A%0A    print '--------------matlab commands---------------'%0A    for c in constraints:%0A        c.printCommand
 s()
-)
 %0A%0A
+%0A%0Aapp = ConsoleApp()%0A
 view
@@ -2803,41 +2803,278 @@
 w()%0A
-robotsystem.create(view, globals(
+%0A%0Afactory = robotsystem.RobotSystemFactory()%0Aoptions = factory.getDisabledOptions()%0Afactory.setDependentOptions(options, usePlannerPublisher=True)%0ArobotSystem = factory.construct(view=view, options=options)%0A%0A%0Aapp.setupGlobals(globals())%0Aglobals().update(dict(robotSystem
 ))%0A%0A
@@ -3100,18 +3100,4 @@
 s()%0A
-%0A#app.start()%0A

add more testing to testPlanConstraints.py