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

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

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

# Configuration des variables publiques
# 'nom_variable' :
# - Objet 3D : [nom de l'objet 3D, propriété du stockage de la valeur (activate ou activated_real)]
# - Configuration de la broche : [nom de la propriété stockant l'object broche (pyfirmata), type de broche par défaut('d','a' ou 'p'), 'mode de la broche par défaut ('i' ou 'o')]
# - Configuration du graphique : ['marque', 'type de ligne', 'couleur', linewidth]] (matplotlib)
#
#  'nom_variable_r' est la valeur réelle de la variable (valeur numérique) 'nom_variable' issue du jumelage numérique.
#  Dans ce cas, il n'y a pas configuration de broche car elle est présente sur la variable 'nom_variable'.
#  Ce distinguo ne concerne que les entrées, les sorties réelles sont forcées par les sorties numériques et vice-versa.

public_vars = {
    't' : [['System','time'], [], []],
    'bp_ext' : [['Bp cote rue','activated'], ['pin', 'd','i'], ['.','-', 'green', 1]],
    'bp_ext_r' : [['Bp cote rue','activated_real'], [], ['.','--', 'green', 1]],
    'bp_int' : [['Bp cote cour','activated'], ['pin', 'd','i'], ['.','-', 'darkgreen', 1]],
    'bp_int_r' : [['Bp cote cour','activated_real'], [], ['.','--', 'darkgreen', 1]],
    'fdc_o' : [['Microrupteur fdc ouvert','activated'], ['pin',  'd','i'], ['.','-', 'orange', 1]],
    'fdc_o_r' : [['Microrupteur fdc ouvert','activated_real'], [], ['.','--', 'orange', 1]],
    'fdc_f' : [['Microrupteur fdc ferme','activated'], ['pin',  'd','i'], ['.','-', 'darkorange', 1]],
    'fdc_f_r' : [['Microrupteur fdc ferme','activated_real'], [], ['.','--', 'darkorange', 1]],
    'mot_o' : [['Moteur','open'], ['pin_open', 'd','o'], ['.','-', 'violet', 1]],
    'mot_f' : [['Moteur','close'], ['pin_close', 'd','o'], ['.','-', 'darkviolet', 1]],
    'mot_angle' : [['Moteur','alpha'], [], ['.','-', 'blue', 1]],
    'mot_vitesse' : [['Moteur','speed'], [], ['.','-', 'darkblue', 1]],
    'mot_pas' : [['Moteur','step'], [], []],
    'portail_x' : [['Portail','x'], [], ['.','-', 'turquoise', 1]],
    'portail_vitesse' : [['Portail','speed'], [], ['.','-', 'darkturquoise', 1]],
    'portail_pas' : [['Portail','step'], [], []],
    'gyr' : [['Led','activated'], ['pin', 'd','.'], ['.','-', 'gold', 1]],
    'ir_emet' : [['Emetteur IR', 'activated'], ['pin', 'd','o'],['.','-', 'red', 1]],
    'ir_recep' : [['Recepteur IR','activated'], ['pin',  'd','i'],['.','-', 'darkred', 1]],
    'ir_recep_r' : [['Recepteur IR','activated_real'], [],['.','--', 'darkred', 1]]}

# 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 public_vars:
        if public_vars[pin][1] != []:
            scene.objects[public_vars[pin][0][0]][public_vars[pin][1][0]] = None

    # Mémorisation de la position et orientation des composants du système
    scene.objects['Portail']['init_lx']=scene.objects['Portail'].worldPosition.x
    scene.objects['Portail']['init_ly']=scene.objects['Portail'].worldPosition.y
    scene.objects['Portail']['init_lz']=scene.objects['Portail'].worldPosition.z
    scene.objects['Engrenage']['init_rx']=scene.objects['Engrenage'].worldOrientation.to_euler().x
    scene.objects['Engrenage']['init_ry']=scene.objects['Engrenage'].worldOrientation.to_euler().y
    scene.objects['Engrenage']['init_rz']=scene.objects['Engrenage'].worldOrientation.to_euler().z

    # Groupe de focus pour les actionneurs
    twin.cycle_def_focusgroup([["Moteur","blue"] ,
                               ["Reducteur","blue"],
                               ["Pattes moteur","blue"],
                               ["Engrenage","blue-dark"],
                               ["Engrennage patte","blue-dark"],
                               ["Engrenage vis1","grey"],
                               ["Engrenage vis2","grey"],
                               ["Engrenage vis3","grey"]], "Moteur : mot_o(True | False), mot_f(True | False)")
    twin.cycle_def_focusgroup([["Led", "led_yellow"]], "Gyrophare : gyr(True | False)")

    # Focus sur les boutons et capteurs
    scene.objects['Bp cote cour']['description']="Bouton poussoir coté cour : bp_int()"
    scene.objects['Bp cote rue']['description']="Bouton poussoir coté rue : bp_ext()"
    scene.objects['Microrupteur fdc ouvert']['description']="Capteur fin de course portail ouvert : fdc_o()"
    scene.objects['Microrupteur fdc ferme']['description']="Capteur fin de course portail fermé : fdc_f()" 
    scene.objects['Emetteur IR']['description']="Capteur barrage IR  (absence d\"obstacle) : ir_recep()"
    scene.objects['Recepteur IR']['description']="Capteur barrage IR  (absence d\"obstacle) : ir_recep()"
    
    system_init() # Initialisation du système
    scene.objects['System']['plot_proc'] = None # Initialisation du processus plot 

def get_public_vars():
    return public_vars

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

##
# Moteur et portail
##

def mot (cont):
    if scene.objects['System']['run']:
        fps = 60 # frame per second
        obj = cont.owner
        obj_engrenage = scene.objects['Engrenage']
        obj_portail = scene.objects['Portail']

        # Réducteur
        r = 1/100 # Rapport de réduction

        # Crémaillaire
        pas_dent = 7.85 # 7.85 mm soit 2.35 m (bender) 
        z = 14  # nb dents
        obj['step']= obj['speed_setting'] / fps # Vitesse du moteur numérique : 1,3 rad /s par défaut
        obj_engrenage['step']= obj['step'] * r
        obj_portail['step'] = obj_engrenage['step'] * (pas_dent * z)/(2*math.pi)

        # Ouvrir
        if obj['open']:

            # Physique
            obj_engrenage.applyRotation((0, 0, -obj_engrenage['step']), True)
            obj['alpha']= obj['alpha']-obj['step']
            if scene.objects['System']['time'] != obj['last_time']:
                obj['speed']= (-obj['step'])/(scene.objects['System']['time']-obj['last_time'])
            obj_portail.applyMovement((-obj_portail['step'], 0, 0), True)
            obj_portail['x']= obj_portail['x']-obj_portail['step'] # Echelle pris en compte par le scale de 'System' : 0,3)
            if scene.objects['System']['time'] != obj['last_time']:
                obj_portail['speed']= -obj_portail['step']/(scene.objects['System']['time']-obj['last_time'])
            obj['last_time'] = scene.objects['System']['time']
           
            # Modele 3D -> Arduino
            if scene.objects['System']['twins']:
                if  scene.objects['Moteur']['pin_o'] is not None:
                    if  scene.objects['Moteur']['pin_f'] is not None:
                        scene.objects['Moteur']['pin_f'].write(0)
                        scene.objects['Moteur']['pin_o'].write(1)

        # Fermer
        # else: # Pas de priorité
        if obj['close']:

            # Physique
            obj_engrenage.applyRotation((0, 0, obj_engrenage['step']), True)
            obj['alpha']= obj['alpha']+obj['step']
            if scene.objects['System']['time'] != obj['last_time']:
                obj['speed']= (obj['step'])/(scene.objects['System']['time']-obj['last_time'])
            obj_portail.applyMovement((obj_portail['step'], 0, 0), True)
            obj_portail['x']= obj_portail['x']+obj_portail['step'] # Echelle pris en compte par le scale de 'System' : 0,3)
            if scene.objects['System']['time'] != obj['last_time']:
                obj_portail['speed']= obj_portail['step']/(scene.objects['System']['time']-obj['last_time'])
            obj['last_time'] = scene.objects['System']['time']

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

        # Arrêrer
        if obj['open']== False and obj['close'] == False :

            # Physique
            obj['speed']= 0
            obj_portail['speed']= 0
            obj['last_time'] = scene.objects['System']['time']

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

###############################################################################
# Capteurs fin de course
###############################################################################

##
# Etat capteur fin de course portail ouvert
##

def fdc_o (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 grille : worldPosition.x : 0 -> 65.5  et localPosition.x : 0 -> 218
        if scene.objects['Portail'].localPosition.x  <= 0 and obj['activated'] == False :
            obj['activated'] = True
        if scene.objects['Portail'].localPosition.x  > 0 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 fin de course portail  fermé
##

def fdc_f (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 grille : worldPosition.x : 0 -> 65.5  et localPosition.x : 0 -> 218
        if scene.objects['Portail'].localPosition.x  >= 218 and obj['activated'] == False :
            obj['activated'] = True
        if scene.objects['Portail'].localPosition.x  < 218 and obj['activated'] == True :
            obj['activated'] = False

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

        # Couleurs
        twin.cycle_sensitive_color(obj)

###############################################################################
# Capteur barrage
###############################################################################

##
# Emetteur IR
##

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

        # Mouse over
        if obj['mo'] == True and obj['click'] == False and obj.color !=color_hl:
            obj.color =color_hl
            return

        # Passif
        if obj['active'] == False and obj.color !=color_passive:
            obj.color =color_passive
            scene.objects['Emetteur IR Led'].setVisible(True,False)
            scene.objects['Emetteur IR Led-on'].setVisible(False,False)
            scene.objects['Recepteur IR']['active'] = False

            # Modele 3D -> Arduino
            if scene.objects['System']['twins']:
                if  scene.objects['Emetteur IR']['pin'] is not None:
                    scene.objects['Emetteur IR']['pin'].write(0)
            return

        # Active
        if obj['active']:

            # Allumage
            if  scene.objects['Emetteur IR Led-on'].visible == False:
                scene.objects['Emetteur IR Led-on'].setVisible(True,False)
                scene.objects['Emetteur IR Led'].setVisible(False,False)
                obj.color = color_active
                scene.objects['Recepteur IR']['active'] = True

                # Modele 3D -> Arduino
                if scene.objects['System']['twins']:
                    if  scene.objects['Emetteur IR']['pin'] is not None:
                        scene.objects['Emetteur IR']['pin'].write(1)

            # Forçage par clic
            if obj['click'] == True:
                obj['activated'] = True
                scene.objects['Recepteur IR']['activated'] = True

            # Couleurs
            # FIXME : à faire
            if obj['activated'] == True and obj.color !=color_activated:
                obj.color =color_activated
            if obj['activated'] == False :
                if obj['mo'] == True and obj.color !=color_hl:
                    obj.color =color_hl
                if obj['mo'] == False and obj.color !=color_active:
                    obj.color =color_active

##
# Récepteur IR
# FIXME : Modele 3D -> Arduino à faire
##

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

        # Mouse over
        if obj['mo'] == True and obj['click'] == False and obj.color !=color_hl:
            obj.color =color_hl
            return

        # Passif
        if obj['active'] == False and obj.color !=color_passive:
            obj.color =color_passive
            return

        # Active
        if obj['active']:
           
            # Forçage par clic
            if obj['click'] == True:
                obj['activated'] = True
                scene.objects['Emetteur IR']['activated'] = True

            # Couleurs
            # FIXME : à faire
            if obj['activated'] == True and obj.color !=color_activated:
                obj.color =color_activated
            if obj['activated'] == False :
                if obj['mo'] == True and obj.color !=color_hl:
                    obj.color =color_hl
                if obj['mo'] == False and obj.color !=color_active:
                    obj.color =color_active

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

##
# Initialisation du système
##

def system_init ():
    system_reset()

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

def system_reset ():

    # Grille à l'état initial
    scene.objects['Portail'].worldPosition.x = scene.objects['Portail']['init_lx']-scene.objects['System']['init_lx']+scene.objects['System'].worldPosition.x
    scene.objects['Portail'].worldPosition.y = scene.objects['Portail']['init_ly']-scene.objects['System']['init_ly']+scene.objects['System'].worldPosition.y
    scene.objects['Portail'].worldPosition.z = scene.objects['Portail']['init_lz']-scene.objects['System']['init_lz']+scene.objects['System'].worldPosition.z

    # Moteur à l'état initial
    rres=0.001 # resolution rotation
    obj1=scene.objects['Engrenage']
    while (obj1.localOrientation.to_euler().y) > 1.1*rres :
        obj1.applyRotation((0, 0, -rres), True)
    while (obj1.localOrientation.to_euler().y) < -1.1*rres :
        obj1.applyRotation((0, 0, rres), True)

    # Gyrophare à l'état initial
    scene.objects['Led'].setVisible(True,False)
    scene.objects['Led-on'].setVisible(False,False)

    # Capteur barrage IR
    scene.objects['Emetteur IR Led'].setVisible(True,False)
    scene.objects['Emetteur IR Led-on'].setVisible(False,False)
    scene.objects['Emetteur IR'].color = color_passive
    scene.objects['Recepteur IR'].color = color_passive

    # I/O à l'état initial
    scene.objects['Led']['activated']=False
    scene.objects['Moteur']['open']=False
    scene.objects['Moteur']['close']=False
    scene.objects['Moteur']['alpha']=0
    scene.objects['Moteur']['speed']=0
    scene.objects['Moteur']['speed_setting']=125.6 # Vitesse du moteur numérique : 125.6 rad /s ( 20 tr / s )
    scene.objects['Moteur']['step']=0
    scene.objects['Portail']['x']=0
    scene.objects['Portail']['speed']=0
    scene.objects['Portail']['step']=0
    scene.objects['Microrupteur fdc ouvert']['activated']=False
    scene.objects['Microrupteur fdc ouvert']['activated_real']=False
    scene.objects['Microrupteur fdc ferme']['activated']=False
    scene.objects['Microrupteur fdc ferme']['activated_real']=False
    scene.objects['Bp cote cour']['activated'] =False
    scene.objects['Bp cote cour']['activated_real'] =False
    scene.objects['Bp cote rue']['activated'] =False
    scene.objects['Bp cote rue']['activated_real'] =False
    scene.objects['Emetteur IR']['activated'] =False
    scene.objects['Emetteur IR']['activated_real'] =False
    scene.objects['Emetteur IR']['active'] =False
    scene.objects['Recepteur IR']['activated'] =False
    scene.objects['Recepteur IR']['active'] =False
    scene.objects['Recepteur IR']['activated_real'] =True # Absence d'obstacle -> True, présence d'obstacle -> False