import logging
import numpy

import kgprim.motions as motions
import kgprim.ct.frommotions as frommotions
import kgprim.ct.repr.mxrepr as mxrepr
import motiondsl.motiondsl as motdsl

logger = logging.getLogger(__name__)

class RobotKinematics:
    '''The composition of the constant poses and the joint poses of a robot.

    This class is a simple aggregation of the geometry model and the joint-poses
    model. By merging the two, this class have access to the full robot
    kinematics.

    Thanks to gr.motions.ConnectedFramesInspector, an arbitrary relative pose
    between two frames on the robot can be obtained.
    '''
    def __init__(self, geometry, jointPoses):
        self.robotGeometry = geometry
        self.jointPoses = jointPoses
        self.baseFrame = geometry.framesModel.linkFrames[ geometry.connectivityModel.base ]

        allPoses = geometry.posesModel.mergeModel( jointPoses.jointPosesModel )

        self.framesConnectivity = motions.ConnectedFramesInspector(allPoses)



def base_H_ee(kinematics, framename):
    if framename not in kinematics.robotGeometry.framesModel.framesByName:
        logger.error("Could not find frame '{0}' in model '{1}'".format(framename, kinematics.robotGeometry.robotName))
        return None

    ee = kinematics.robotGeometry.framesModel.framesByName[ framename ]
    if not kinematics.framesConnectivity.hasRelativePose(ee, kinematics.baseFrame):
        logger.error("Frame '{0}' and the base frame do not seem to be connected".format(framename))
        return None

    poseSpec = kinematics.framesConnectivity.getPoseSpec(ee, kinematics.baseFrame)
    cotr = frommotions.toCoordinateTransform(poseSpec)
    H = mxrepr.hCoordinatesSymbolic(cotr)
    q = numpy.zeros( len(H.variables) )
    H = H.setVariablesValue( valueslist=q )
    return H


def serializeToMotionDSLModel(robotKinematics, ostream):
    header ='''
Model {modelname}

Convention = currentFrame

'''.format(modelname=robotKinematics.robotGeometry.robotName)

    ostream.write(header)
    for jp in robotKinematics.jointPoses.poseSpecByJoint.values():
        text = motdsl.poseSpecToMotionDSLSnippet( jp )
        ostream.write(text)
        ostream.write('\n')
    for cp in robotKinematics.robotGeometry.byPose.values() :
        text = motdsl.poseSpecToMotionDSLSnippet( cp )
        ostream.write(text)
        ostream.write('\n')


[((9, 9, 9, 36), 'logging.getLogger', 'logging.getLogger', ({(9, 27, 9, 35): '__name__'}, {}), '(__name__)', False, 'import logging\n'), ((43, 11, 43, 54), 'kgprim.ct.frommotions.toCoordinateTransform', 'frommotions.toCoordinateTransform', ({(43, 45, 43, 53): 'poseSpec'}, {}), '(poseSpec)', True, 'import kgprim.ct.frommotions as frommotions\n'), ((44, 8, 44, 41), 'kgprim.ct.repr.mxrepr.hCoordinatesSymbolic', 'mxrepr.hCoordinatesSymbolic', ({(44, 36, 44, 40): 'cotr'}, {}), '(cotr)', True, 'import kgprim.ct.repr.mxrepr as mxrepr\n'), ((28, 34, 28, 76), 'kgprim.motions.ConnectedFramesInspector', 'motions.ConnectedFramesInspector', ({(28, 67, 28, 75): 'allPoses'}, {}), '(allPoses)', True, 'import kgprim.motions as motions\n'), ((60, 15, 60, 54), 'motiondsl.motiondsl.poseSpecToMotionDSLSnippet', 'motdsl.poseSpecToMotionDSLSnippet', ({(60, 50, 60, 52): 'jp'}, {}), '(jp)', True, 'import motiondsl.motiondsl as motdsl\n'), ((64, 15, 64, 54), 'motiondsl.motiondsl.poseSpecToMotionDSLSnippet', 'motdsl.poseSpecToMotionDSLSnippet', ({(64, 50, 64, 52): 'cp'}, {}), '(cp)', True, 'import motiondsl.motiondsl as motdsl\n')]