#!/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 Kenneth Snelson 'Sun River', 1967
    bodies = []
    # body[0] and body[1] are rear-tilting
    bodies.append(Strut(np.array((0.5,   -0.5, 0.0)), np.array(( -1.0, -0.5, 2.0))))
    bodies.append(Strut(np.array((0.5,    0.5, 0.0)), np.array(( -1.0,  0.5, 2.0))))

    # body[2] is forward-tilting
    bodies.append(Strut(np.array((-0.5,   0.0, 0.0)), np.array((  0.6,  0.0, 2.0))))

    # body[3] is the floating horizontal spar
    bodies.append(Strut(np.array((-0.5, -1.0, 0.8)), np.array((-0.5, 1.0, 0.8))))

    # make list of tendons connecting the bottoms, tops, and sides
    tendons = []
    tendons.append(Tendon(bodies[0], 0, bodies[2], 0))
    tendons.append(Tendon(bodies[0], 0, bodies[2], 1))
    tendons.append(Tendon(bodies[0], 0, bodies[3], 0))

    tendons.append(Tendon(bodies[0], 1, bodies[2], 1))
    tendons.append(Tendon(bodies[0], 1, bodies[3], 0))
    tendons.append(Tendon(bodies[0], 1, bodies[3], 1))

    tendons.append(Tendon(bodies[1], 0, bodies[2], 0))
    tendons.append(Tendon(bodies[1], 0, bodies[2], 1))
    tendons.append(Tendon(bodies[1], 0, bodies[3], 1))

    tendons.append(Tendon(bodies[1], 1, bodies[2], 1))
    tendons.append(Tendon(bodies[1], 1, bodies[3], 0))
    tendons.append(Tendon(bodies[1], 1, bodies[3], 1))

    tendons.append(Tendon(bodies[2], 0, bodies[3], 0))
    tendons.append(Tendon(bodies[2], 0, bodies[3], 1))

    # return a tuple of spars and tendons
    return bodies, tendons

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

def main():

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

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

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

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

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

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

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

main()