
########################################################################################################################
# Copyright (c) 2015, University of Osnabrueck                                                                         #
#   All rights reserved.                                                                                               #
#                                                                                                                      #
#   Redistribution and use in source and binary forms, with or without modification, are permitted provided that the   #
#   following conditions are met:                                                                                      #
#                                                                                                                      #
#   1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following#
#       disclaimer.                                                                                                    #
#                                                                                                                      #
#   2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the       #
#       following disclaimer in the documentation and/or other materials provided with the distribution.               #
#                                                                                                                      #
#   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, #
#   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  #
#   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, #
#   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR    #
#   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,  #
#   WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE   #
#   USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                           #
########################################################################################################################
from lxml import etree
from StringIO import StringIO
import numpy as np
import sys

import utm


#osmfile = "osm/westerberg.osm"
#polyfile = "faces/osmpoly.txt"



SIGMA = 1e-10


def writePoly(outfile, poly):
    polystr = ""
    for p in poly:
        polystr += str(p) + " "
    polystr = polystr[:-1]
    polystr += "\n"
    outfile.write(polystr)


def reversePoly(poly):
    newpoly = np.array([])
    for i in range(len(poly) - 3, -1, -3):
        newpoly = np.append(newpoly, poly[i])
        newpoly = np.append(newpoly, poly[i + 1])
        newpoly = np.append(newpoly, poly[i + 2])
    return newpoly


def parse(osmfile, polyfile, config):
    groundheight = config["groundHeight"]
    buildingheight = config["buildingHeight"]

    tree = etree.parse(osmfile)

    #first the nodes:
    r = tree.findall("node")

    nodes = {}
    for node in r:
        key = node.xpath("./attribute::id")[0]
        #higher values
        #lat = float(node.xpath("./attribute::lat")[0])*10000
        #lon = -float(node.xpath("./attribute::lon")[0])*10000
        #conversion degrees->UTM
        utm_tuple = utm.from_latlon(float(node.xpath("./attribute::lat")[0]), float(node.xpath("./attribute::lon")[0]))
        #nodes[key] = (utm_tuple[1], utm_tuple[0])
        nodes[key] = (utm_tuple[0], utm_tuple[1])

    #now the buildings:
    r = tree.findall('//way')

    ways=[]
    for way in r:
        if len(way.xpath("./tag[@k='building']")) == 1 or len(way.xpath("./tag[@k='building:part']")) == 1:
            ways.append(way)
    for rel in tree.findall('//relation') :
        if len(rel.xpath("./tag[@k='building']")) == 1:
            for member in rel.xpath("./member[@type='way' and @role='outer']") :
                ways.append(tree.find("//way[@id='"+member.get("ref")+"']"))


    outfile = open(polyfile, "w")
    for way in ways:
            polynodekeys = way.xpath('./nd/attribute::ref')
            poly = np.array([])
            for k in polynodekeys:
                poly = np.append(poly, nodes[k][0])
                poly = np.append(poly, nodes[k][1])
                poly = np.append(poly, groundheight)

            #repeat first vertex:
            if np.linalg.norm(poly[:3] - poly[-3:]) > SIGMA:
                #poly.extend(poly[:3])
                poly = np.append(poly, poly[:3])

            #is it ccw or cw? - http://debian.fmi.uni-sofia.bg/~sergei/cgsr/docs/clockwise.htm
            pos = 0
            neg = 0
            for i in range(1, len(poly) / 3 - 1):
                a = poly[(i - 1) * 3:i * 3]
                b = poly[i * 3:(i + 1) * 3]
                c = poly[(i + 1) * 3:(i + 2) * 3]
                if np.cross(a - b, c - b)[2] > 0:
                    pos += 1
                else:
                    neg += 1

                #change orientation if needed:
            if pos < neg:
                poly = reversePoly(poly)
            writePoly(outfile, poly)  #ground
            roofPoly = poly + np.tile([0, 0, buildingheight], len(poly) / 3)
            for i in range(0, len(poly) / 3 - 1):
                wallPoly = []
                wallPoly.extend(poly[i * 3:(i + 1) * 3])  #lower left
                wallPoly.extend(roofPoly[i * 3:(i + 1) * 3])  #upper left
                wallPoly.extend(roofPoly[(i + 1) * 3:(i + 2) * 3])  #upper right
                wallPoly.extend(poly[(i + 1) * 3:(i + 2) * 3])  #lower right
                wallPoly.extend(poly[i * 3:(i + 1) * 3])  #lower left
                writePoly(outfile, wallPoly)
            roofPoly = reversePoly(roofPoly)
            writePoly(outfile, roofPoly)
    outfile.close()


if __name__ == "__main__":
    osmfile = sys.argv[1]
    polyfile = sys.argv[2]
    config = {"groundHeight": 100,
              "buildingHeight": 20}
    parse(osmfile, polyfile, config)

#
#for t in r :
#	outfile.write(t+"\n")
