#!/usr/bin/env python
# -*- coding: latin-1 -*-


from math import sqrt
from math import radians
from math import tan
from math import hypot


#--------------------
#  Constraint-Löser
#--------------------

class Rectangle:

    """Rechteckiger Bereich."""

    def __init__(self, x_min, x_max, y_min, y_max):
        """Initialisierung."""
        self.x_min = x_min
        self.x_max = x_max
        self.y_min = y_min
        self.y_max = y_max

    def __cmp__(self, other):
        """Vergleiche zwei Rechtecke."""
        return cmp((self.x_min, self.x_max, self.y_min, self.x_max),
                   (other.x_min, other.x_max, other.y_min, other.x_max))

    def to_svg(self, color=u"blue"):
        """SVG-Darstellung"""
        return u"""
            <rect x="%f" y="%f" width="%f" height="%f"
                  style="fill: none; stroke: %s; stroke-width: 7"/>""" \
            % (self.x_min, self.y_min,
               self.x_max - self.x_min, self.y_max - self.y_min, color)


class DistanceConstraint:

    """Constraint, das durch eine Abstands-Ungleichung beschrieben wird."""

    def __init__(self, center_x, center_y, dist_min, dist_max):
        """Initialisierung."""
        assert(dist_min < dist_max)
        self.center_x = center_x
        self.center_y = center_y
        self.dist_min = dist_min
        self.dist_max = dist_max

    def delta_x_min(self, dist, center_y, y_min, y_max):
        return sqrt(min(0, max(dist**2 - (y_min - center_y)**2,
                               dist**2 - (y_max - center_y)**2)))

    def delta_x_max(self, dist, center_y, y_min, y_max):
        if y_min <= center_y <= y_max:
            return dist
        return sqrt(max(0, dist**2 - (y_min - center_y)**2,
                           dist**2 - (y_max - center_y)**2))

    def cut_x(self, dist_min, dist_max, center_x, center_y,
                    x_min, x_max, y_min, y_max):
        """Ermittle den x-Bereich für die maximal konsistente Einschränkung.

        Funktioniert aus Symmetriegründen genauso auch für den y-Bereich.
        """
        dx_min = self.delta_x_min(dist_min, center_y, y_min, y_max)
        dx_max = self.delta_x_max(dist_max, center_y, y_min, y_max)

        if x_min <= center_x - dx_max:
            x_min = center_x - dx_max
        elif center_x - dx_min <= x_min <= center_x + dx_min:
            x_min = center_x + dx_min

        if center_x + dx_max <= x_max:
            x_max = center_x + dx_max
        elif center_x - dx_min <= x_max <= center_x + dx_min:
            x_max = center_x - dx_min

        return x_min, x_max

    def cut(self, rect):
        """Ermittle die maximal konsistente Einschränkung eines Rechtecks.

        Geometrisch entspricht dies dem umschließenden Rechteck der
        Schnittmenge zwischen diesem Constraint und dem Ausgangs-Rechteck.

        Argumente
        =========
            - rect -- Ausgangs-Rechteck, das eingeschränkt wird
        """
        x_min, x_max = self.cut_x(self.dist_min, self.dist_max,
                                  self.center_x, self.center_y,
                                  rect.x_min, rect.x_max,
                                  rect.y_min, rect.y_max)
        y_min, y_max = self.cut_x(self.dist_min, self.dist_max,
                                  self.center_y, self.center_x,
                                  rect.y_min, rect.y_max,
                                  rect.x_min, rect.x_max)
        return Rectangle(x_min, x_max, y_min, y_max)

    def to_svg(self, color=u"black"):
        return u"""
            <circle cx="%f" cy="%f" r="%f"
                    style="fill: none; stroke: %s; stroke-width: 7"/>
            <circle cx="%f" cy="%f" r="%f"
                    style="fill: none; stroke: %s; stroke-width: 7"/>""" \
            % (self.center_x, self.center_y, self.dist_min, color,
               self.center_x, self.center_y, self.dist_max, color)


def solve_constraints(start, constraints):
    """Löse ein System von Constraints.

    Liefert ein Rectangle-Objekt zurück. Es beschreibt das kleinste
    Recheck, das die Lösungsmenge des Constraint-Systems umfasst.

    Außerdem werden die benötigten Zwischenschritte zurückgegeben.

    Argumente
    =========
        - start -- Ausgangs-Rechteck
        - constraints -- Liste von zu erfüllenden Constraints
    """
    steps = [start]
    rect = start
    while True:
        prev = rect
        for constraint in constraints:
            rect = constraint.cut(rect)
            steps.append(rect)
        if rect == prev:
            return rect, steps


#--------------------------
#  Constraint-Ermittelung
#--------------------------

class MapObject:

    """Ein Objekt (z.B. Flagge, Pfosten) auf dem Spielfeld."""

    def __init__(self, (x, y), h):
        self.x = x
        self.y = y
        self.h = h


def distance_to(height, angle):
    """Distanz zu einem gesehenen Objekt.

    Argumente
    =========
        - size      -- reale Größe des Objektes
        - angle     -- Winkelgröße des Objektes in Grad
    """
    return height / tan(radians(angle))


def post_constraints(obj, (a1, b1), (a2, b2), abs_error):
    """Constraints für einen vom Imageprozessor erkannten Pfosten.

    Liefert eine Liste von Constraint-Objekten zurück.

    Argumente
    =========
        - obj       -- zugehöriges MapObject
        - (a1, b1)  -- Winkelkoordinaten der oberen Ecke in Grad
        - (a2, b2)  -- Winkelkoordinaten der unteren Ecke in Grad
        - abs_error -- absoluter Fehler von Winkeldifferenzen in Grad
    """
    assert(abs(a1-a2) > abs_error)
    assert(abs(b1-b2) > abs_error)
    dist_min = distance_to(obj.h, hypot(abs(a1-a2) + abs_error,
                                        abs(b1-b2) + abs_error))
    dist_max = distance_to(obj.h, hypot(abs(a1-a2) - abs_error,
                                        abs(b1-b2) - abs_error))
    return [DistanceConstraint(obj.x, obj.y, dist_min, dist_max)]


def flag_constraints(obj, (a1, b1), (a2, b2), (a3, b3), (a4, b4), abs_error):
    """Constraints für eine vom Imageprozessor erkannte Flagge.

    Liefert eine Liste von Constraint-Objekten zurück.

    Argumente
    =========
        - obj       -- zugehöriges MapObject
        - (a1, b1)  -- Winkelkoordinaten der linken obere Ecke in Grad
        - (a2, b2)  -- Winkelkoordinaten der linken untere Ecke in Grad
        - (a3, b3)  -- Winkelkoordinaten der rechten obere Ecke in Grad
        - (a4, b4)  -- Winkelkoordinaten der rechten untere Ecke in Grad
        - abs_error -- absoluter Fehler von Winkeldifferenzen in Grad
    """
    return post_constraints(obj, (a1, b1), (a2, b2), abs_error) \
         + post_constraints(obj, (a3, b3), (a4, b4), abs_error)


#---------------
#  SVG-Ausgabe
#---------------

def print_svg(filename, objects):
    file = open(filename, "w")
    print >>file, """<?xml version="1.0"?>
        <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN"
                      "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
        <svg width="100%" height="100%" version="1.1"
             xmlns="http://www.w3.org/2000/svg"
             xmlns:xlink="http://www.w3.org/1999/xlink">

            <image x="-3000" y="-2000" width="6000" height="4000"
                   xlink:href="aufgabe2-spielfeld.png"/>"""
    for obj in objects:
        print >>file, obj.to_svg()
    print >>file, """
        </svg>"""


#-----------------
#  Hauptprogramm
#-----------------

def main():
    #  Wissen über das Spielfeld
    F1 = MapObject((-1350,  1950), 400)
    F2 = MapObject((-1350, -1950), 400)
    F3 = MapObject(( 1350,  1950), 400)
    F4 = MapObject(( 1350, -1950), 400)

    P1 = MapObject((-2700,  400), 300)
    P2 = MapObject((-2700, -400), 300)
    P3 = MapObject(( 2700,  400), 300)
    P4 = MapObject(( 2700, -400), 300)

    # Größe des Spielfeld (=Ausgangsmenge)
    start = Rectangle(-3000, 3000, -2000, 2000)

    # Constraints der vom Imageprozessor erkannten Objekte
    constraints = (
        flag_constraints(F2, (21.45, 23.17), (22.55, 31.68),
                             (23.38, 22.88), (24.75, 31.20), 0.25) +
        flag_constraints(F1, (42.31,  0.83), (41.78, 12.93),
                             (44.95,  0.83), (44.16, 12.93), 0.25) +
        flag_constraints(F3, (34.64, 20.08), (32.26, 29.98),
                             (36.49, 20.63), (34.11, 30.53), 0.25))

    # Berechne Position
    pos, steps = solve_constraints(start, constraints)

    # Text-Ausgabe
    print
    print "Vermutete Position von Bello:"
    print "x = %6.1f mm  (+/- %f mm)" % ((pos.x_max + pos.x_min) / 2,
                                         pos.x_max - pos.x_min)
    print "y = %6.1f mm  (+/- %f mm)" % ((pos.y_max + pos.y_min) / 2,
                                         pos.y_max - pos.y_min)
    print

    # SVG-Ausgabe
    filename = "aufgabe2-loesung.svg"
    print_svg(filename, constraints + steps)
    print "SVG-Bildchen:", filename
    print


if __name__ == "__main__":
    main()