# ====== Legal notices
#
# Copyright (C) 2013 - 2018 GEATEC engineering
#
# This program is free software.
# You can use, redistribute and/or modify it, but only under the terms stated in the QQuickLicence.
#
# This program 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 QQuickLicence for details.
#
# The QQuickLicense can be accessed at: http://www.qquick.org/license.html
#
# __________________________________________________________________________
#
#
#  THIS PROGRAM IS FUNDAMENTALLY UNSUITABLE FOR CONTROLLING REAL SYSTEMS !!
#
# __________________________________________________________________________
#
# It is meant for training purposes only.
#
# Removing this header ends your licence.
#

'''

      z
      |
      o -- y
     /
    x

'''

import random as rd

import SimPyLC as sp

import parameters as pm

normalFloorColor = (0, 0.003, 0)
collisionFloorColor = (1, 0, 0.3)
nrOfObstacles = 32

class Line (sp.Cylinder):
    def __init__ (self, **arguments):
       super () .__init__ (size = (0.01, 0.01, 0), axis = (1, 0, 0), angle = 90, color = (0, 1, 1), **arguments)


class BodyPart (sp.Beam):
    def __init__ (self, **arguments):
        super () .__init__ (color = (1, 0, 0), **arguments)

class Wheel:
    def __init__ (self, **arguments):
        self.suspension = sp.Cylinder (size = (0.01, 0.01, 0.001), axis = (1, 0, 0), angle = 90, pivot = (0, 0, 1), **arguments)
        self.rim = sp.Beam (size = (0.08, 0.06, 0.02), pivot = (0, 1, 0), color = (0, 0, 0))
        self.tire = sp.Cylinder (size = (pm.wheelDiameter, pm.wheelDiameter, 0.04), axis = (1, 0, 0), angle = 90, color = (1, 1, 0))
        self.line = Line ()
        
    def __call__ (self, wheelAngle, slipping, steeringAngle = 0):
        return self.suspension (rotation = steeringAngle, parts = lambda:
            self.rim (rotation = wheelAngle, parts = lambda:
                self.tire (color = (rd.random (), rd.random (), rd.random ()) if slipping else (1, 1, 0)) +
                self.line ()
        )   )
        
class Window (sp.Beam):
    def __init__ (self, **arguments):
        super () .__init__ (axis = (0, 1, 0), color = (0, 0, 1), **arguments)
        
class Floor (sp.Beam):
    side = 10
    spacing = 0.2
    halfSteps = round (0.5 * side / spacing)

    class Stripe (sp.Beam):
        def __init__ (self, **arguments):
            super () .__init__ (size = (0.01, Floor.side, 0.001), **arguments)
            
    def __init__ (self, **arguments):
        super () .__init__ (size = (self.side, self.side, 0.0005), color = normalFloorColor)
        self.xStripes = [self.Stripe (center = (0, nr * self.spacing, 0.0001), angle = 90, color = (1, 1, 1)) for nr in range (-self.halfSteps, self.halfSteps)]
        self.yStripes = [self.Stripe (center = (nr * self.spacing, 0, 0), color = (0, 0, 0)) for nr in range (-self.halfSteps, self.halfSteps)]
        
    def __call__ (self, parts):
        return super () .__call__ (color = collisionFloorColor if self.scene.collided else  normalFloorColor, parts = lambda:
            parts () +
            sum (xStripe () for xStripe in self.xStripes) +
            sum (yStripe () for yStripe in self.yStripes)
        )

class Visualisation (sp.Scene):
    def __init__ (self):
        super () .__init__ ()
        
        self.camera = sp.Camera ()
        # self.collider = sp.Collider (groups = (0, 1), )
        
        self.floor = Floor (scene = self)
        
        self.fuselage = BodyPart (size = (0.70, 0.16, 0.08), center = (0, 0, 0.07), pivot = (0, 0, 1), group = 0)
        self.fuselageLine = Line ()
        self.cabin = BodyPart (size = (0.20, 0.16, 0.06), center = (-0.06, 0, 0.07))
        
        self.wheelFrontLeft = Wheel (center = (pm.wheelShift, 0.08, -0.02))
        self.wheelFrontRight = Wheel (center = (pm.wheelShift, -0.08, -0.02))
        
        self.wheelRearLeft = Wheel (center = (-pm.wheelShift, 0.08, -0.02))
        self.wheelRearRight = Wheel (center = (-pm.wheelShift, -0.08, -0.02))
        
        self.windowFront = Window (size = (0.05, 0.14, 0.14), center = (0.14, 0, -0.025), angle = -60)    
        self.windowRear = Window (size = (0.05, 0.14, 0.18), center = (-0.18, 0, -0.025),angle = 72) 
                
        self.obstacles = [sp.Beam (
            size = (rd.uniform (0.3, 0.6), rd.uniform (0.3, 0.6), rd.uniform (0.2, 0.4)),
            center = (rd.uniform (-self.floor.side / 2, self.floor.side / 2), rd.uniform (-self.floor.side / 2, self.floor.side / 2), 0),
            angle = rd.uniform (0, 90),
            group = 1
        ) for i in range (nrOfObstacles)]
        # self.obstacles = [obstacle for obstacle in self.obstacles if sp.vNor (obstacle.center) > 1]

    def display (self):
        
        self.camera (
            position = sp.tEva ((sp.world.physics.positionX + 2, sp.world.physics.positionY, 2)),
            focus = sp.tEva ((sp.world.physics.positionX + 0.001, sp.world.physics.positionY, 0))
            # focus = sp.tEva ((0, 0, 0))
        )
        
        
        self.floor (parts = lambda:
            self.fuselage (position = (sp.world.physics.positionX, sp.world.physics.positionY, 0), rotation = sp.world.physics.attitudeAngle, parts = lambda:
                self.cabin (parts = lambda:
                    self.windowFront () +
                    self.windowRear ()
                ) +
                
                self.wheelFrontLeft (
                    wheelAngle = sp.world.physics.midWheelAngle,
                    slipping = sp.world.physics.slipping,
                    steeringAngle = sp.world.physics.steeringAngle
                ) +
                self.wheelFrontRight (
                    wheelAngle = sp.world.physics.midWheelAngle,
                    slipping = sp.world.physics.slipping,
                    steeringAngle = sp.world.physics.steeringAngle
                ) +
                
                self.wheelRearLeft (
                    wheelAngle = sp.world.physics.midWheelAngle,
                    slipping = sp.world.physics.slipping
                ) +
                self.wheelRearRight (
                    wheelAngle = sp.world.physics.midWheelAngle,
                    slipping = sp.world.physics.slipping
                ) +
                
                self.fuselageLine ()
            ) +
            
            sum (obstacle () for obstacle in self.obstacles)
        )