void TensegrityModel::addChildRotation(tgStructure& childStructure, const Yam& rotation) {
    if (!rotation) return;
    Yam reference = rotation["reference"];
    Yam axis = rotation["axis"];
    Yam angle = rotation["angle"];
    if (axis && angle) {
        double axisX = axis[0].as<double>();
        double axisY = axis[1].as<double>();
        double axisZ = axis[2].as<double>();
        btVector3 axisVector = btVector3(axisX, axisY, axisZ);
        double angleDegrees = angle.as<double>();
        double angleRadians = tgUtil::deg2rad(angleDegrees);
        btVector3 referenceVector;
        if (reference) {
            // rotate child around provided reference point
            double referenceX = reference[0].as<double>();
            double referenceY = reference[1].as<double>();
            double referenceZ = reference[2].as<double>();
            referenceVector = btVector3(referenceX, referenceY, referenceZ);
        }
        else {
            // rotate child around structure's centroid
            referenceVector = childStructure.getCentroid();
        }
        childStructure.addRotation(referenceVector, axisVector, angleRadians);
    }
}

void BigDoxieNoFeet::addRodsLeg(tgStructure& s){
    s.addPair(0,1,"rod");
    s.addPair(1,2,"rod");
    s.addPair(1,3,"rod");
    s.addPair(1,4,"rod");
    s.addPair(0,5,"rod");
}

void BigPuppySymmetricArching::addNodesVertebra(tgStructure& s, double r) {
    s.addNode(0,0,0); //Node 0
    s.addNode(r,0,r); //Node 1
    s.addNode(r,0,-r); //Node 2
    s.addNode(-r,0,-r); //Node 3
    s.addNode(-r,0,r); //Node 4
}

/// @todo should use a best fit transformation from one set of points to another
void TensegrityModel::rotateAndTranslate(tgStructure& childStructure2,
    std::vector<btVector3>& structure1RefNodes, std::vector<btVector3>& structure2RefNodes) {

    btVector3 structure1RefNodesCentroid = tgUtil::getCentroid(structure1RefNodes);
    btVector3 structure2RefNodesCentroid = tgUtil::getCentroid(structure2RefNodes);

    btVector3 structure1PlaneNormal = ((structure1RefNodes[1] - structure1RefNodes[0]).
        cross(structure1RefNodes[2] - structure1RefNodes[0])).normalize();
    btVector3 structure2PlaneNormal = ((structure2RefNodes[1] - structure2RefNodes[0]).
        cross(structure2RefNodes[2] - structure2RefNodes[0])).normalize();

    // rotate structure 2 to align normals
    btVector3 fallBackAxis = (structure2RefNodes[1] - structure2RefNodes[0]).normalize();
    childStructure2.addRotation(structure2RefNodesCentroid,
        tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));

    // rotate structure 2 ref nodes
    tgUtil::addRotation(structure2RefNodes[0], structure2RefNodesCentroid,
        tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));
    tgUtil::addRotation(structure2RefNodesCentroid, structure2RefNodesCentroid,
        tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));

    // translate structure 2 to match up centroid points
    childStructure2.move(structure1RefNodesCentroid - structure2RefNodesCentroid);

    // translate structure 2 ref nodes
    structure2RefNodes[0] += structure1RefNodesCentroid - structure2RefNodesCentroid;
    structure2RefNodesCentroid += structure1RefNodesCentroid - structure2RefNodesCentroid;

    // rotate structure 2 around structure1PlaneNormal axis to match up node with edge midpoints
    childStructure2.addRotation(structure1RefNodesCentroid,
        tgUtil::getQuaternionBetween(structure2RefNodes[0] - structure1RefNodesCentroid,
        structure1RefNodes[0] - structure1RefNodesCentroid, structure1PlaneNormal));
}

void BigPuppySymmetricSpiralSegments::addRodsLeg(tgStructure& s){
    s.addPair(0,1,"rod");
    s.addPair(1,2,"rod");
    s.addPair(1,3,"rod");
    s.addPair(1,4,"rod");
    s.addPair(0,5,"rod");
}

void BigPuppySymmetricArching::addRodsLeg(tgStructure& s) {
    s.addPair(0,1,"rod");
    s.addPair(1,2,"rod");
    s.addPair(1,3,"rod");
    s.addPair(1,4,"rod");
    s.addPair(0,5,"rod");
}

void BigDoxieNoFeet::addNodesVertebra(tgStructure& s, double r){
    s.addNode(0,0,0); //Node 0 
    s.addNode(r,0,r); //Node 1 
    s.addNode(r,0,-r); //Node 2
    s.addNode(-r,0,-r); //Node 3
    s.addNode(-r,0,r); //Node 4
}

void BigDoxieNoFeet::addNodesLeg(tgStructure& s, double r){ 
    s.addNode(0,0,0); //0: Bottom Center of lower leg segment
    s.addNode(0,r,0);  //1: Center of lower leg segment
    s.addNode(r,r,0); //2: Right of lower leg segment
    s.addNode(-r,r,0);  //3: Left of lower leg segment
    s.addNode(0,2*r,0);  //4: Top of lower leg segment
    s.addNode(0,-r/2,0);  //5: Leg segment extension for connections to foot.
}

void VerticalSpineModel::addPairsB(tgStructure& tetra)
{
    tetra.addPair(0, 4, "rodB");
    tetra.addPair(1, 4, "rodB");
    tetra.addPair(2, 4, "rodB");
    tetra.addPair(3, 4, "rodB");

}

void T6Model::addRods(tgStructure& s)
{
    s.addPair( 0,  1, "rod");
    s.addPair( 2,  3, "rod");
    s.addPair( 4,  5, "rod");
    s.addPair( 6,  7, "rod");
    s.addPair( 8,  9, "rod");
    s.addPair(10, 11, "rod");
}

void BigPuppySymmetricArching::addRodsFoot(tgStructure& s) {
    s.addPair(0,6,"rod");
    s.addPair(1,7,"rod");
    s.addPair(2,4,"rod");
    s.addPair(3,5,"rod");
    s.addPair(0,1,"rod");
    s.addPair(0,3,"rod");
    s.addPair(1,2,"rod");
    s.addPair(2,3,"rod");
}

void T12SuperBallPayload::addRods(tgStructure& s)
{
    s.addPair( 0,  6, "r1 rod");
    s.addPair( 1,  7, "r2 rod");
    s.addPair( 2,  8, "r3 rod");
    s.addPair( 3,  4, "r4 rod");
    s.addPair( 5, 11, "r5 rod");
    s.addPair( 9, 10, "r6 rod");

}

void DuCTTTestModel::addNodes(tgStructure& tetra, double edge, double height)
{
    // right
    tetra.addNode(-edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
    // left
    tetra.addNode( edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
    // front
    tetra.addNode(0, edge/2.0, 0);
    // back
    tetra.addNode(0, -edge/2.0, 0);
}

void TensegrityModel::addChildren(tgStructure& structure, const std::string& structurePath, tgBuildSpec& spec, const Yam& children) {
    if (!children) return;
    std::string structureAttributeKeys[] = {"path", "rotation", "translation", "scale", "offset"};
    std::vector<std::string> structureAttributeKeysVector(structureAttributeKeys, structureAttributeKeys + sizeof(structureAttributeKeys) / sizeof(std::string));

    // add all the children first
    for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
        Yam childAttributes = child->second;
        yamlContainsOnly(childAttributes, structurePath, structureAttributeKeysVector);
        // multiple children can be defined using the syntax: child1/child2/child3...
        // (add a slash so that each child is a string with a its name and a slash at the end)
        std::string childCombos = child->first.as<std::string>() + "/";
        while (childCombos.find("/") != std::string::npos) {
            std::string childName = childCombos.substr(0, childCombos.find("/"));
            addChild(structure, structurePath, childName, childAttributes["path"], spec);
            childCombos = childCombos.substr(childCombos.find("/") + 1);
        }
    }

    // apply rotation attribute to children
    for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
        Yam childAttributes = child->second;
        // multiple children can be defined using the syntax: child1/child2/child3...
        // (add a slash so that each child is a string with a its name and a slash at the end)
        std::string childCombos = child->first.as<std::string>() + "/";
        while (childCombos.find("/") != std::string::npos) {
            std::string childName = childCombos.substr(0, childCombos.find("/"));
            tgStructure& childStructure = structure.findChild(childName);
            addChildRotation(childStructure, childAttributes["rotation"]);
            childCombos = childCombos.substr(childCombos.find("/") + 1);
        }
    }

    // apply scale, offset and translation attributes to children
    for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
        Yam childAttributes = child->second;
        // multiple children can be defined using the syntax: child1/child2/child3...
        // (add a slash so that each child is a string with a its name and a slash at the end)
        std::string childCombos = child->first.as<std::string>() + "/";
        int childComboIndex = 0;
        while (childCombos.find("/") != std::string::npos) {
            std::string childName = childCombos.substr(0, childCombos.find("/"));
            tgStructure& childStructure = structure.findChild(childName);
            addChildScale(childStructure, childAttributes["scale"]);
            addChildOffset(childStructure, childComboIndex, childAttributes["offset"]);
            addChildTranslation(childStructure, childAttributes["translation"]);
            childCombos = childCombos.substr(childCombos.find("/") + 1);
            childComboIndex++;
        }
    }
}

void VerticalSpineModel::addNodes(tgStructure& tetra, double edge, double height)
{
    // right
    tetra.addNode( c.edge / 2.0, 0, 0); // node 0
    // left
    tetra.addNode( -c.edge / 2.0, 0, 0); // node 1
    // top
    tetra.addNode(0, c.height, -edge / 2.0); // node 2
    // front
    tetra.addNode(0, c.height, edge / 2.0); // node 3
    // middle
    tetra.addNode(0, c.height/2, 0); // node 4

}

void TensegrityModel::addChildOffset(tgStructure& childStructure, int offsetIndex, const Yam& offset) {
    if (!offset) return;
    double offsetX = offset[0].as<double>();
    double offsetY = offset[1].as<double>();
    double offsetZ = offset[2].as<double>();
    btVector3 offsetVector = btVector3(offsetX, offsetY, offsetZ);
    childStructure.move(offsetVector * offsetIndex);
}

void TensegrityModel::addChildTranslation(tgStructure& childStructure, const Yam& translation) {
    if (!translation) return;
    double translationX = translation[0].as<double>();
    double translationY = translation[1].as<double>();
    double translationZ = translation[2].as<double>();
    btVector3 translationVector = btVector3(translationX, translationY, translationZ);
    childStructure.move(translationVector);
}

void TensegrityModel::addNodeEdgePairs(tgStructure& structure, const std::string& tags, const Yam& pairs,
    const std::string* childStructure1Name, const std::string* childStructure2Name, tgBuildSpec& spec) {

    if (pairs.size() < 3) {
        throw std::invalid_argument("Error: node_edge bonds must specify at least 3 node_edge pairs");
    }

    tgStructure& childStructure1 = structure.findChild(*childStructure1Name);
    tgStructure& childStructure2 = structure.findChild(*childStructure2Name);

    // these are used for transformations
    std::vector<btVector3> structure1RefNodes;
    std::vector<btVector3> structure2RefNodes;

    // these are nodes
    std::vector<tgNode*> ligands;
    // these are edges
    std::vector< std::pair<tgNode*, tgNode*> > receptors;

    // populate refNodes arrays, ligands and receptors
    parseNodeEdgePairs(childStructure1, childStructure2, structure1RefNodes, structure2RefNodes, ligands, receptors, pairs);
    rotateAndTranslate(childStructure2, structure1RefNodes, structure2RefNodes);

    std::vector<tgBuildSpec::RigidAgent*> rigidAgents = spec.getRigidAgents();
    for (unsigned int i = 0; i < ligands.size(); i++) {

        // remove old edge connections
        // try removing from both children since we are not sure which child the pair belongs to
        // (could add more information to receptors array so we don't have to do this)
        removePair(childStructure1, receptors[i].first, receptors[i].second, true, rigidAgents, spec);
        removePair(childStructure2, receptors[i].first, receptors[i].second, true, rigidAgents, spec);
        for (unsigned int j = 0; j < ligands.size(); j++) {
            // remove old string connections between nodes/ligands
            // try removing from both children since we are not sure which child the node belongs to
            removePair(childStructure1, ligands[i], ligands[j], false, rigidAgents, spec);
            removePair(childStructure2, ligands[i], ligands[j], false, rigidAgents, spec);
        }
        // make new connection from edge -> node -> edge
        structure.addPair(*(receptors[i].first), *ligands[i], tags);
        structure.addPair(*ligands[i], *(receptors[i].second), tags);

    }
}

void TensegrityModel::addNodeNodePairs(tgStructure& structure, const std::string& tags, const Yam& pairs,
    const std::string* childStructure1Name, const std::string* childStructure2Name) {

    for (YAML::const_iterator pairPtr = pairs.begin(); pairPtr != pairs.end(); ++pairPtr) {
        Yam pair = *pairPtr;
        std::string node1Path = pair[0].as<std::string>();
        std::string node2Path = pair[1].as<std::string>();
        tgNode* node1;
        tgNode* node2;
        if (childStructure1Name && childStructure2Name) {
            node1 = &getNode(structure.findChild(*childStructure1Name), node1Path);
            node2 = &getNode(structure.findChild(*childStructure2Name), node2Path);
        }
        else {
            node1 = &getNode(structure, node1Path);
            node2 = &getNode(structure, node2Path);
        }
        structure.addPair(*node1, *node2, tags);
    }
}

void TensegrityModel::addNodes(tgStructure& structure, const Yam& nodes) {
    if (!nodes) return;
    for (YAML::const_iterator node = nodes.begin(); node != nodes.end(); ++node) {
        std::string name = node->first.as<std::string>();
        Yam xyz = node->second;
        double x = xyz[0].as<double>();
        double y = xyz[1].as<double>();
        double z = xyz[2].as<double>();
        structure.addNode(x, y, z, name);
    }
}