import bge # Bibliothèque Blender Game Engine (UPBGE)
import twin # Bibliothèque de l'environnement 3D des jumeaux numériques
import math
import time

###############################################################################
# montchg.py
# @title: Commandes pour le monte-charge
# @project: Blender-EduTech
# @lang: fr
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
# @copyright: Copyright (C) 2022-2023 Philippe Roy
# @license: GNU GPL
###############################################################################

# Récupérer la scène UPBGE
scene = bge.logic.getCurrentScene()

# Configuration du brochage du jumeau réel
pin_config = {
    'voy_0' : [['d','o'],['Led niveau 0','pin']],
    'voy_1' : [['d','o'],['Led niveau 1','pin']],
    'pc_0' : [['d','i'],['Microrupteur niveau 0','pin']],
    'pc_1' : [['d','i'],['Microrupteur niveau 1','pin']],
    'ba_0' : [['d','i'],['Bp niveau 0','pin']],
    'ba_1' : [['d','i'],['Bp niveau 1','pin']],
    'mot_m' : [['d','o'],['Moteur','pin_m',]],
    'mot_d' : [['d','o'],['Moteur','pin_d']]}

# Couleurs
color_passive = (0.800, 0.005, 0.315,1) # bouton non activable :  magenta
color_active =  (0.799, 0.130, 0.063,1) # bouton activable :  orange
color_hl =  (0.8, 0.8, 0.8, 1) # bouton focus : blanc
color_activated =  (0.8, 0.619, 0.021, 1) # bouton activé numériquement uniquement :  jaune
color_activated_real =  (0.799, 0.031, 0.038, 1) # élément activé physiquement uniquement :  rouge (hors clic)
color_activated_dbl =  (0.246, 0.687, 0.078, 1) # élément activé physiquement et numériquement :  vert clair

# Constantes UPBGE
JUST_ACTIVATED = bge.logic.KX_INPUT_JUST_ACTIVATED
JUST_RELEASED = bge.logic.KX_INPUT_JUST_RELEASED
ACTIVATE = bge.logic.KX_INPUT_ACTIVE
# JUST_DEACTIVATED = bge.logic.KX_SENSOR_JUST_DEACTIVATED

###############################################################################
# Initialisation de la scène
###############################################################################

def  init(cont):
    if cont.sensors['Init'].positive == False: # 1 seule fois 
        return False

    twin.manip_init() # Manipulation du modèle 3D 
    twin.cmd_init() # Commandes

    # Brochage
    for pin in pin_config:
        # print (pin_config[pin][1][0], pin_config[pin][1][1])
        scene.objects[pin_config[pin][1][0]][pin_config[pin][1][1]] = None

    # Mémorisation de la position et orientation des composants du système au départ
    scene.objects['Cabine']['init_lx']=scene.objects['Cabine'].worldPosition.x
    scene.objects['Cabine']['init_ly']=scene.objects['Cabine'].worldPosition.y
    scene.objects['Cabine']['init_lz']=scene.objects['Cabine'].worldPosition.z
    scene.objects['Contrepoids']['init_lx']=scene.objects['Contrepoids'].worldPosition.x
    scene.objects['Contrepoids']['init_ly']=scene.objects['Contrepoids'].worldPosition.y
    scene.objects['Contrepoids']['init_lz']=scene.objects['Contrepoids'].worldPosition.z
    scene.objects['Moteur vis sans fin']['init_rx']=scene.objects['Moteur vis sans fin'].worldOrientation.to_euler().x
    scene.objects['Moteur vis sans fin']['init_ry']=scene.objects['Moteur vis sans fin'].worldOrientation.to_euler().y
    scene.objects['Moteur vis sans fin']['init_rz']=scene.objects['Moteur vis sans fin'].worldOrientation.to_euler().z
    scene.objects['Moteur pignon']['init_rx']=scene.objects['Moteur pignon'].worldOrientation.to_euler().x
    scene.objects['Moteur pignon']['init_ry']=scene.objects['Moteur pignon'].worldOrientation.to_euler().y
    scene.objects['Moteur pignon']['init_rz']=scene.objects['Moteur pignon'].worldOrientation.to_euler().z

    # Groupe de focus pour les actionneurs
    twin.cycle_def_focusgroup([["Moteur","blue-dark"] ,
                               ["Moteur arriere","blue-dark"] ,
                               ["Moteur condensateur","grey"],
                               ["Reducteur flanc droit","blue"],
                               ["Reducteur flanc gauche","blue"],
                               ["Moteur vis sans fin","blue-dark"],
                               ["Moteur pignon","blue-dark"],
                               ["Moteur axe 1","blue"],
                               ["Moteur axe 2","blue-dark"],
                               ["Moteur gaine thermo","blue"],
                               ["Cable","blue"]], "Moteur : mot_m(True | False), mot_d(True | False)")
    twin.cycle_def_focusgroup([["Led niveau 0", "led_yellow"]], "Voyant témoin d\"étage niveau 0 : voy_0(True | False)")
    twin.cycle_def_focusgroup([["Led niveau 1", "led_yellow"]], "Voyant témoin d\"étage niveau 1 : voy_1(True | False)")

    # Description des composants sensibles
    scene.objects['Bp niveau 0']['description']="Bouton poussoir appel niveau 0 : ba_0()"
    scene.objects['Bp niveau 1']['description']="Bouton poussoir appel niveau 1 : ba_1()"
    scene.objects['Microrupteur niveau 0']['description']="Capteur présence cabine niveau 0 : pc_0()"
    scene.objects['Microrupteur niveau 1']['description']="Capteur présence cabine niveau 1 : pc_1()" 
    
    system_init() # Initialisation du système

def get_pin_config():
    return pin_config

###############################################################################
# Actionneurs
###############################################################################

##
# Moteur et cabine
##

def mot (cont):
    if scene.objects['System']['run']:
        obj = cont.owner
        vitesse = 0.015
        pas_cabine = 10 # Diam axe = 3 mm -> 1 tour = 3*math.pi
        pas_pignon = pas_cabine/(3*math.pi) # Z pignon = 48 dents
        pas_vissansfin = pas_pignon*48
        obj_vissansfin = scene.objects['Moteur vis sans fin']
        obj_pignon = scene.objects['Moteur pignon']
        obj_cabine = scene.objects['Cabine']
        obj_cabine['z']= scene.objects['Cabine'].localPosition.z # Affichage de l'altitude de la cabine
        obj_contrepoids = scene.objects['Contrepoids']
        obj_cable = scene.objects['Cable'] # FIXME : plus tard

        # Monter
        if obj['up']:
            obj_vissansfin.applyRotation((0, 0, pas_vissansfin*vitesse), True)
            obj_pignon.applyRotation((pas_pignon*vitesse, 0, 0), True)
            obj_cabine.applyMovement((0, 0, pas_cabine*vitesse), True)
            obj_contrepoids.applyMovement((0, 0, -pas_cabine*vitesse), True)

            # Modele 3D -> Arduino
            if scene.objects['System']['twins']:
                if  scene.objects['Moteur']['pin_m'] is not None:
                    if  scene.objects['Moteur']['pin_d'] is not None:
                        scene.objects['Moteur']['pin_d'].write(0)
                        scene.objects['Moteur']['pin_m'].write(1)
     
        # Descendre
        # else: # Pas de priorité
        if obj['down']:
            obj_vissansfin.applyRotation((0, 0, -pas_vissansfin*vitesse), True)
            obj_pignon.applyRotation((-pas_pignon*vitesse, 0, 0), True)
            obj_cabine.applyMovement((0, 0, -pas_cabine*vitesse), True)
            obj_contrepoids.applyMovement((0, 0, pas_cabine*vitesse), True)

        # Arrêter
        if obj['up']== False and obj['down'] == False :

            # Modele 3D -> Arduino
            if scene.objects['System']['twins']:
                if  scene.objects['Moteur']['pin_d'] is not None:
                    if  scene.objects['Moteur']['pin_m'] is not None:
                        scene.objects['Moteur']['pin_m'].write(0)
                        scene.objects['Moteur']['pin_d'].write(0)


###############################################################################
# Capteurs
###############################################################################

##
# Etat capteur présence cabine niveau 0
##

def pc_0 (cont):
    if scene.objects['System']['run'] :
        obj = cont.owner

        # Arduino -> Modele 3D
        if scene.objects['System']['twins']:
            if  obj['pin'] is not None:
                if obj['pin'].read()==True and obj['activated_real'] == False :
                    obj['activated_real'] = True
                if obj['pin'].read()==False and obj['activated_real'] == True :
                    obj['activated_real'] = False

        # Etat capteur en fonction de la position de la cabine : localPosition.z entre -40 et -42
        if scene.objects['Cabine'].localPosition.z  <-40 and scene.objects['Cabine'].localPosition.z  >-42 and obj['activated'] == False :
            obj['activated'] = True
        if (scene.objects['Cabine'].localPosition.z  > -40 or scene.objects['Cabine'].localPosition.z  <-42) and obj['activated'] == True :
            obj['activated'] = False

        # Forçage par clic
        if obj['click'] == True:
            obj['activated'] = True

        # Couleurs
        twin.cycle_sensitive_color(obj)

##
# Etat capteur présence cabine niveau 1
##

def pc_1 (cont):
    if scene.objects['System']['run'] :
        obj = cont.owner

        # Arduino -> Modele 3D
        if scene.objects['System']['twins']:
            if  obj['pin'] is not None:
                if obj['pin'].read()==True and obj['activated_real'] == False :
                    obj['activated_real'] = True
                if obj['pin'].read()==False and obj['activated_real'] == True :
                    obj['activated_real'] = False

        # Etat capteur en fonction de la position de la cabine : localPosition.z entre 0 et -2
        if scene.objects['Cabine'].localPosition.z  <0 and scene.objects['Cabine'].localPosition.z  >-2 and obj['activated'] == False :
            obj['activated'] = True
        if (scene.objects['Cabine'].localPosition.z  > 0 or scene.objects['Cabine'].localPosition.z  <-2) and obj['activated'] == True :
            obj['activated'] = False

        # Forçage par clic
        if obj['click'] == True:
            obj['activated'] = True

        # Couleurs
        twin.cycle_sensitive_color(obj)

###############################################################################
# Système
###############################################################################

##
# Initialisation du système
# # Le moteur est géré en continue.
##

def system_init ():
    system_reset()

##
# Réinitialisation du système
##

def system_reset ():

    # Voyants aux états initiaux
    scene.objects['Led niveau 0'].setVisible(True,False)
    scene.objects['Led niveau 0-on'].setVisible(False,False)
    scene.objects['Led niveau 1'].setVisible(True,False)
    scene.objects['Led niveau 1-on'].setVisible(False,False)

    # Cabine
    scene.objects['Cabine'].worldPosition.x = scene.objects['Cabine']['init_lx']-scene.objects['System']['init_lx']+scene.objects['System'].worldPosition.x
    scene.objects['Cabine'].worldPosition.y = scene.objects['Cabine']['init_ly']-scene.objects['System']['init_ly']+scene.objects['System'].worldPosition.y
    scene.objects['Cabine'].worldPosition.z = scene.objects['Cabine']['init_lz']-scene.objects['System']['init_lz']+scene.objects['System'].worldPosition.z
    scene.objects['Contrepoids'].worldPosition.x = scene.objects['Contrepoids']['init_lx']-scene.objects['System']['init_lx']+scene.objects['System'].worldPosition.x
    scene.objects['Contrepoids'].worldPosition.y = scene.objects['Contrepoids']['init_ly']-scene.objects['System']['init_ly']+scene.objects['System'].worldPosition.y
    scene.objects['Contrepoids'].worldPosition.z = scene.objects['Contrepoids']['init_lz']-scene.objects['System']['init_lz']+scene.objects['System'].worldPosition.z

    # Moteur à l'état initial : pas utile

    # I/O à l'état initial
    scene.objects['Led niveau 0']['activated']=False
    scene.objects['Led niveau 1']['activated']=False
    scene.objects['Bp niveau 0']['activated']=False
    scene.objects['Bp niveau 0']['activated_real']=False
    scene.objects['Bp niveau 1']['activated']=False
    scene.objects['Bp niveau 1']['activated_real']=False
    scene.objects['Microrupteur niveau 0']['activated']=False
    scene.objects['Microrupteur niveau 0']['activated_real']=False
    scene.objects['Microrupteur niveau 1']['activated']=False
    scene.objects['Microrupteur niveau 1']['activated_real']=False
    scene.objects['Moteur']['up']=False
    scene.objects['Moteur']['down']=False