# -*- coding: utf-8 -*-
"""
$Id$

Copyright 2010 Lars Kruse <devel@sumpfralle.de>

This file is part of PyCAM.

PyCAM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

PyCAM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with PyCAM.  If not, see <http://www.gnu.org/licenses/>.
"""

import pycam.Cutters
from pycam.Geometry.Point import Point
import ode

try:
    import OpenGL.GL as GL
    GL_enabled = True
except ImportError:
    GL_enabled = False


class ODEBlocks(object):

    def __init__(self, tool_settings, (minx, maxx, miny, maxy, minz, maxz),
            x_steps=None, y_steps=None):
        self.cutter = pycam.Cutters.get_tool_from_settings(tool_settings)
        # we don't want to use the "material allowance" distance
        self.cutter.set_required_distance(0)
        if x_steps is None:
            x_steps = 10
        if y_steps is None:
            y_steps = 10
        self.x_steps = x_steps
        self.y_steps = y_steps
        self.x_offset = minx
        self.y_offset = miny
        self.z_offset = minz
        self.x_width = maxx - minx
        self.y_width = maxy - miny
        self.z_width = maxz - minz
        if self.z_width <= 0:
            # Use a default height of "1" if the model is flat.
            # Half of the height is above and half below the plane.
            self.z_width = 1.0
            self.z_offset = minz - self.z_width / 2.0
        self.world = ode.World()
        self.space = ode.Space()
        self.boxes = []
        z_pos = self.z_offset + 0.5 * self.z_width
        self.x_step_width = self.x_width / (self.x_steps - 1)
        self.y_step_width = self.y_width / (self.y_steps - 1)
        for x_pos in [self.x_offset + (index + 0.5) / x_steps * self.x_width \
                for index in range(x_steps)]:
            for y_pos in [self.y_offset + (index + 0.5) / y_steps \
                    * self.y_width for index in range(y_steps)]:
                body = ode.Body(self.world)
                box = ode.GeomBox(self.space, (self.x_step_width,
                        self.y_step_width, self.z_width))
                box.setBody(body)
                box.setPosition((x_pos, y_pos, z_pos))
                box.position = Point(x_pos, y_pos, z_pos)
                self.boxes.append(box)

    def process_cutter_movement(self, location_start, location_end):
        # TODO: fix this workaround in the cutters shape defintions (or in ODE?)
        # for now we may only move from low x/y values to higher x/y values
        if (location_start.x > location_end.x) \
                or (location_start.y > location_end.y):
            location_start, location_end = location_end, location_start
        cutter_body = ode.Body(self.world)
        cutter_shape, cutter_position_func = self.cutter.get_shape("ODE")
        self.space.add(cutter_shape)
        cutter_shape.space = self.space
        cutter_shape.setBody(cutter_body)
        cutter_position_func(location_start.x, location_start.y,
                location_start.z)
        cutter_shape.extend_shape(location_end.x - location_start.x,
                location_end.y - location_start.y,
                location_end.z - location_start.z)
        aabb = cutter_shape.getAABB()
        cutter_height = aabb[5] - aabb[4]
        # add a ray along the drill to work around an ODE bug in v0.11.1
        # http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799
        currx, curry, currz = cutter_shape.getPosition()
        ray = ode.GeomRay(self.space, cutter_height)
        ray.set((currx, curry, aabb[5]), (0.0, 0.0, -1.0))
        # check for collisions
        all_cutter_shapes = [cutter_shape] + cutter_shape.children + [ray]
        def check_collision(item):
            for one_cutter_shape in all_cutter_shapes:
                if ode.collide(item, one_cutter_shape):
                    return True
            return False
        for index in range(len(self.boxes)):
            if check_collision(self.boxes[index]):
                self.shrink_box_avoiding_collision(index, check_collision)

    def shrink_box_avoiding_collision(self, box_index, collision_func):
        box = self.boxes[box_index]
        aabb = box.getAABB()
        end_height, start_height = aabb[-2:]
        height_half = (start_height - end_height) / 2.0
        x_pos = box.position.x
        y_pos = box.position.y
        new_z = end_height
        box.setPosition((x_pos, y_pos, end_height - height_half))
        loops_left = 12
        upper_limit = start_height
        lower_limit = end_height
        if collision_func(box):
            # the cutter goes down to zero (end_height) - we can skip the rest
            loops_left = 0
        while loops_left > 0:
            new_z = (upper_limit + lower_limit) / 2.0
            box.setPosition((x_pos, y_pos, new_z - height_half))
            if collision_func(box):
                upper_limit = new_z
            else:
                lower_limit = new_z
            loops_left -= 1
        del self.boxes[box_index]
        # The height should never be zero - otherwise ODE will throw a
        # "bNormalizationResult" assertion.
        new_height = max(new_z - end_height, 0.1)
        z_pos = new_z - new_height / 2.0
        new_box = ode.GeomBox(self.space, (aabb[1] - aabb[0], aabb[3] - aabb[2],
                new_height))
        new_box.position = Point(x_pos, y_pos, z_pos)
        new_box.setBody(box.getBody())
        new_box.setPosition((x_pos, y_pos, z_pos))
        self.boxes.insert(box_index, new_box)

    def get_height_field(self):
        """ returns a two-dimensional height field of the current "landscape"
        """
        result = []
        heights = []
        for box in self.boxes:
            aabb = box.getAABB()
            heights.append(Point((aabb[1] + aabb[0]) / 2.0,
                    (aabb[3] + aabb[2]) / 2.0, aabb[5]))
        for column in range(self.x_steps):
            result.append(heights[
                    column * self.y_steps : (column + 1) * self.y_steps])
        return result

    def _normal(self, z0, z1, z2):
        nx = self.x_steps / self.x_width
        ny = self.y_steps / self.y_width
        nz = 1.0 / (self.z_width)
        return (-ny * (z1 - z0) * nz / nx, -nx * (z2 - z1) * nz / ny,
                nx * ny / nz * 100)
        
    def to_OpenGL(self):
        if not GL_enabled:
            return
        height_field = self.get_height_field()
        def get_box_height_points(x, y):
            """ Get the positions and heights of the the four top corners of a
            height box.

            The result is a tuple of four Points (pycam.Geometry.Point) in the
            following order:
             - left below
             - right below
             - right above
             - left above
            ("above": greater x value; "right": greater y value)
            The height of each corner point is calculated as the average of the
            four neighbouring boxes. Thus a set of 3x3 adjacent boxes is used
            for calculating the heights of the four corners.
            """
            points = []
            # Go through a set of box index combinations (sharing a common
            # corner). The "offsets" tuple is used for indicating the relative
            # position of each corner.
            for offsets, index_list in (
                ((-1, -1), ((x - 1, y - 1), (x, y - 1), (x, y), (x - 1, y))),
                ((+1, -1), ((x, y - 1), (x, y), (x + 1, y), (x + 1, y - 1))),
                ((+1, +1), ((x, y), (x + 1, y), (x + 1, y + 1), (x, y + 1))),
                ((-1, +1), ((x - 1, y), (x, y), (x, y + 1), (x - 1, y + 1)))):
                divisor = 0
                height_sum = 0
                x_positions = []
                y_positions = []
                for ix, iy in index_list:
                    if (0 <= ix < len(height_field)) \
                            and (0 <= iy < len(height_field[ix])):
                        point = height_field[ix][iy]
                        height_sum += point.z
                        x_positions.append(point.x)
                        y_positions.append(point.y)
                        divisor += 1
                # Use the middle between the x positions of two adjacent boxes,
                # _if_ there is a neighbour attached to that corner.
                if (min(x_positions) < height_field[x][y].x) \
                        or (max(x_positions) > height_field[x][y].x):
                    x_value = (min(x_positions) + max(x_positions)) / 2.0
                else:
                    # There is no adjacent box in x direction. Use the step size
                    # to calculate the x value of this edge.
                    x_value = height_field[x][y].x \
                            + offsets[0] * self.x_step_width / 2.0
                # same as above for y instead of x
                if (min(y_positions) < height_field[x][y].y) \
                        or (max(y_positions) > height_field[x][y].y):
                    y_value = (min(y_positions) + max(y_positions)) / 2.0
                else:
                    y_value = height_field[x][y].y \
                            + offsets[1] * self.y_step_width / 2.0
                # Create a Point instance describing the position and the
                # average height.
                points.append(Point(x_value, y_value, height_sum / divisor))
            return points
        # draw the surface
        GL.glBegin(GL.GL_QUADS)
        for x in xrange(self.x_steps):
            for y in xrange(self.y_steps):
                # Get the positions and heights of the four corners surrounding
                # the current box.
                points_around = get_box_height_points(x, y)
                # Calculate the "normal" of polygon. We picked up three random
                # points of this quadrilateral.
                n = self._normal(points_around[1].z, points_around[2].z,
                        points_around[3].z)
                GL.glNormal3f(n[0], n[1], n[2])
                for point in points_around:
                    GL.glVertex3f(point.x, point.y, point.z)
                # go through the conditions for an edge box and use the
                # appropriate corners for the side faces of the material
                for condition, i1, i2 in ((x == 0, 3, 0), (y == 0, 0, 1),
                        (x == self.x_steps - 1, 1, 2),
                        (y == self.y_steps - 1, 2, 3)):
                    # check if this point belongs to an edge of the material
                    if condition:
                        n = self._normal(points_around[1].z, points_around[2].z,
                                points_around[3].z)
                        GL.glNormal3f(n[0], n[1], n[2])
                        GL.glVertex3f(points_around[i1].x, points_around[i1].y,
                                self.z_offset)
                        GL.glVertex3f(points_around[i1].x, points_around[i1].y,
                                points_around[i1].z)
                        GL.glVertex3f(points_around[i2].x, points_around[i2].y,
                                points_around[i2].z)
                        GL.glVertex3f(points_around[i2].x, points_around[i2].y,
                                self.z_offset)
        GL.glEnd()