#!/usr/bin/env python

import os
import math
import xml.etree.ElementTree as ET
import numpy as np

from xmlutil.tensegrity import Strut, Tendon

#================================================================
def make_structure():

    # Based on Pugh's Figure 10 octahedron.  The struts are perpendicular but offset.
    off = 0.1
    xstrut = Strut(np.array((-0.5,  off, 1.0-off)), np.array(( 0.5,  off, 1.0-off)))
    ystrut = Strut(np.array(( off, -0.5, 1.0+off)), np.array(( off,  0.5, 1.0+off)))
    zstrut = Strut(np.array((-off, -off, 0.5    )), np.array((-off, -off, 1.5    )))

    struts = [xstrut, ystrut, zstrut]

    # make list of tendons
    tendons = []

    # XY plane horizontal ring
    tendons.append(Tendon(xstrut, 0, ystrut, 0))
    tendons.append(Tendon(xstrut, 0, ystrut, 1))
    tendons.append(Tendon(xstrut, 1, ystrut, 0))
    tendons.append(Tendon(xstrut, 1, ystrut, 1))

    # XZ plane vertical ring
    tendons.append(Tendon(xstrut, 0, zstrut, 0))
    tendons.append(Tendon(xstrut, 0, zstrut, 1))
    tendons.append(Tendon(xstrut, 1, zstrut, 0))
    tendons.append(Tendon(xstrut, 1, zstrut, 1))

    # YZ plane vertical ring
    tendons.append(Tendon(ystrut, 0, zstrut, 0))
    tendons.append(Tendon(ystrut, 0, zstrut, 1))
    tendons.append(Tendon(ystrut, 1, zstrut, 0))
    tendons.append(Tendon(ystrut, 1, zstrut, 1))

    # return a tuple of struts and tendons
    return struts, tendons

#================================================================

def main():

    # create the XML elements representing a tensegrity structure
    struts, tendons = make_structure()

    # read the template file and insert the elements
    spec = ET.parse('octohedron_template.xml')

    root = spec.getroot()
    world = spec.find('worldbody')

    for s in struts:
        world.append(s.as_element())

    for t in tendons:
        root.append(t.as_element())

    ET.indent(spec)
    spec.write('octohedron.xml')

#================================================================

main()