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 Philippe Roy
# @license: GNU GPL
###############################################################################

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

# 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é :  jaune

# 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

    # Mémorisation de la position et orientation des composants du système au départ
    # scene.objects['Cabine']['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
    
    system_init() # Initialisation du système

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

##
# Action du simulateur pour le clignotant
##

# def sml_clignotant (cont):
#     if scene.objects['System']['run']:
#         obj = cont.owner
#         if obj['actif'] and scene.objects['Led allumee'].visible == False:
#             scene.objects['Led allumee'].setVisible(True,False)
#             scene.objects['Led'].setVisible(False,False)
#             # print ("Clignotant allumée")
#         if obj['actif']==False and scene.objects['Led allumee'].visible == True:
#             scene.objects['Led'].setVisible(True,False)
#             scene.objects['Led allumee'].setVisible(False,False)
#             # print ("Clignotant éteint")

##
# Action du simulateur pour le moteur
##

# def sml_moteur (cont):
#     if scene.objects['System']['run']:
#         obj = cont.owner
#         pas_rot = math.pi/7 # z = 14
#         pas = 2.35619/0.3 # pas echelle 1:1 = 2.35619 -> pas à l'échelle de la maquette (0,3) : 2.35619/0.3 = 7,85396
#         vitesse = 0.05
#         engrenage_obj = scene.objects['Engrenage']
#         portail_obj = scene.objects['Portail']
#         if obj['actif_ouvrir']:
#             # print (scene.objects['Portail'].worldPosition.x)
#             engrenage_obj.applyRotation((0, 0, -pas_rot*vitesse), True)
#             portail_obj.applyMovement((-pas*vitesse, 0, 0), True)
#         # else:
#         if obj['actif_fermer']:
#             # print (scene.objects['Portail'].worldPosition.x)
#             engrenage_obj.applyRotation((0, 0, pas_rot*vitesse), True)
#             portail_obj.applyMovement((pas*vitesse, 0, 0), True)

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

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

# def sml_fdc_ouvert (cont):
#     if scene.objects['System']['run'] :
#         obj = cont.owner
#         obj_etat=obj['actif']
#         obj_microrupteur=scene.objects['Microrupteur fdc ouvert']
#         # 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['actif'] == False :
#             obj['actif'] = True
#         if scene.objects['Portail'].localPosition.x  > 0 and obj_microrupteur['actif'] == False and obj['actif'] == True :
#             obj['actif'] = False
#         #Forçage
#         if obj_microrupteur['actif'] == True:
#             obj['actif'] = True
#         #Couleurs
#         if obj['actif'] == True and obj_microrupteur.color !=couleur_jaune:
#             obj_microrupteur.color =couleur_jaune
#         if obj['actif'] == False :
#             if obj_microrupteur['MO'] == True and obj_microrupteur.color !=couleur_blanc:
#                 obj_microrupteur.color =couleur_blanc
#             if obj_microrupteur['MO'] == False and obj_microrupteur.color !=couleur_orange:
#                 obj_microrupteur.color =couleur_orange

##
# Etat capteur fin de course portail  fermé
##

# def sml_fdc_ferme (cont):
#     if scene.objects['System']['run'] :
#         obj = cont.owner
#         obj_etat=obj['actif']
#         obj_microrupteur=scene.objects['Microrupteur fdc ferme']
#         # 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['actif'] == False :
#             obj['actif'] = True
#         if scene.objects['Portail'].localPosition.x  < 218 and obj_microrupteur['actif'] == False and obj['actif'] == True :
#             obj['actif'] = False
#         #Forçage
#         if obj_microrupteur['actif'] == True:
#             obj['actif'] = True
#         #Couleurs
#         if obj['actif'] == True and obj_microrupteur.color !=couleur_jaune:
#             obj_microrupteur.color =couleur_jaune
#         if obj['actif'] == False :
#             if obj_microrupteur['MO'] == True and obj_microrupteur.color !=couleur_blanc:
#                 obj_microrupteur.color =couleur_blanc
#             if obj_microrupteur['MO'] == False and obj_microrupteur.color !=couleur_orange:
#                 obj_microrupteur.color =couleur_orange

# ##
# # Forçage capteur fin de course avec la souris
# ##

# def sml_microrupteur (cont):
#     if scene.objects['System']['run'] :
#         system_click(cont, cont.owner)

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

# ##
# # Emetteur IR
# ##

# def sml_emet_ir (cont):
#     if scene.objects['System']['run'] :
#         obj = cont.owner
#         obj_emetteur_ir=scene.objects['Emetteur IR']
#         obj_recepteur_ir=scene.objects['Recepteur IR']
#         if obj['actif'] and scene.objects['Emetteur IR Led allumee'].visible == False:
#             scene.objects['Emetteur IR Led allumee'].setVisible(True,False)
#             scene.objects['Emetteur IR Led'].setVisible(False,False)
#             obj_emetteur_ir.color = couleur_orange
#             obj_recepteur_ir.color = couleur_orange
#         if obj['actif']==False and scene.objects['Emetteur IR Led allumee'].visible == True:
#             scene.objects['Emetteur IR Led'].setVisible(True,False)
#             scene.objects['Emetteur IR Led allumee'].setVisible(False,False)
#             obj_emetteur_ir.color = couleur_magenta
#             obj_recepteur_ir.color = couleur_magenta

# ##
# # Récepteur IR
# ##

# def sml_recep_ir (cont):
#     if scene.objects['System']['run'] and scene.objects['Module emetteur IR']['actif'] :
#         obj = cont.owner
#         system_click(cont, scene.objects['Module recepteur IR'])
#         if scene.objects['Module recepteur IR']['actif']==True :
#             scene.objects['Emetteur IR'].color = couleur_jaune
#             scene.objects['Recepteur IR'].color = couleur_jaune

# ###############################################################################
# # Boutons poussoirs
# ###############################################################################

# ##
# # Bouton pousssoir coté rue
# ##

# def sml_bp_rue (cont):
#     if scene.objects['System']['run'] :
#         system_click(cont, scene.objects['Module bouton cote rue'])

# ##
# # Bouton pousssoir coté cour
# ##

# def sml_bp_cour (cont):
#     if scene.objects['System']['run'] :
#         system_click(cont, scene.objects['Module bouton cote cour'])

###############################################################################
# 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
    # # applyRotationTo(scene.objects['System'], 0, 0, 0)
    # scene.objects['Portail'].worldPosition.x = scene.objects['Portail']['init_lx']
    # scene.objects['Portail'].worldPosition.y = scene.objects['Portail']['init_ly']
    # scene.objects['Portail'].worldPosition.z = scene.objects['Portail']['init_lz']

    # # 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)

    # 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 1']['activated']=False
    scene.objects['Microrupteur niveau 0']['activated']=False
    scene.objects['Microrupteur niveau 1']['activated']=False

##
# Boucle principale
##

def system_run ():

    # # Lecture de la liaison série : programme Arduino : berceau_arduino.ino
    # serial_msg = str(serial_comm.readline()) # Communication série : arduino -> modele 3d

    # # Affiche le message uniquement
    # if serial_msg.find("Print")>0 or serial_msg.find("Debug")>0 or serial_msg.find("Echo")>0:
    #     print ("Communication port série : ", serial_msg)
    #     serial_msg=""
    #     return

    # Position cabine numérique
    print ("serial_msg : ", serial_msg)
    roll_txt = txt_extrac(serial_msg,"Roll (Rx): ", " Pitch")
    # pitch_txt=  txt_extrac(serial_msg,"Pitch (Ry): ", " \\r\\n")
    pitch_txt = txt_extrac(serial_msg,"Pitch (Ry): ", " bt_a_m:")
    if roll_txt !="" and pitch_txt !="" :
        obj1=scene.objects['Plateau']
        roll=float(roll_txt)/57.3
        pitch=float(pitch_txt)/57.3
        # print ("Roll : ", roll, " Pitch : ", pitch)
        applyRotationTo(scene.objects['Plateau'], roll,0, 0)
        applyRotationTo(scene.objects['Plateau'], roll,pitch, 0)
        scene.objects['Plateau']['roll']=roll
        scene.objects['Plateau']['pitch']=pitch

    # Capteurs numériques

    
    # Modele 3d -> Arduino : FIXME
    # Arduino -> Modele 3d : FIXME

    # Arduino -> numérique
    # bt_a_m_txt = txt_extrac_bool(serial_msg,"bt_a_m: ")
    # bt_a_d_txt = txt_extrac_bool(serial_msg,"bt_a_d: ")
    # bt_b_m_txt = txt_extrac_bool(serial_msg,"bt_b_m: ")
    # bt_b_d_txt = txt_extrac_bool(serial_msg,"bt_b_d: ")
    # bt_c_m_txt = txt_extrac_bool(serial_msg,"bt_c_m: ")
    # bt_c_d_txt = txt_extrac_bool(serial_msg,"bt_c_d: ")
    # bp_phy('Bp Am', bt_a_m_txt)
    # bp_phy('Bp Ad', bt_a_d_txt)
    # bp_phy('Bp Bm', bt_b_m_txt)
    # bp_phy('Bp Bd', bt_b_d_txt)
    # bp_phy('Bp Cm', bt_c_m_txt)
    # bp_phy('Bp Cd', bt_c_d_txt)

    # # 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
    # # applyRotationTo(scene.objects['System'], 0, 0, 0)
    # scene.objects['Portail'].worldPosition.x = scene.objects['Portail']['init_lx']
    # scene.objects['Portail'].worldPosition.y = scene.objects['Portail']['init_ly']
    # scene.objects['Portail'].worldPosition.z = scene.objects['Portail']['init_lz']

    # # 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)

    # 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 1']['activated']=False
    # scene.objects['Microrupteur niveau 0']['activated']=False
    # scene.objects['Microrupteur niveau 1']['activated']=False

###############################################################################
# Interface
###############################################################################

##
# Highlight  sur les éléments cliquables du systèmes
##

def hl(cont):
    obj = cont.owner
    name=obj.name

    # Activation
    if cont.sensors['MO'].status == JUST_ACTIVATED and scene.objects['System']['run']:
        scene.objects[name].color = color_hl

    # Désactivation
    if cont.sensors['MO'].status == JUST_RELEASED:
        scene.objects[name].color = color_active

##
# Click sur les éléments cliquables du systèmes (activation numérique)
##

def click(cont):
    obj = cont.owner
    name=obj.name

    # Activation
    if cont.sensors['Click'].status == JUST_ACTIVATED and cont.sensors['MO'].positive and scene.objects['System']['manip_mode']==0:
        scene.objects[name].color = color_activated
        obj['activated'] = True
        # Modele 3d -> Arduino : FIXME

    # Désactivation
    if cont.sensors['Click'].status == JUST_RELEASED:
        obj['activated'] = False
        # Modele 3d -> Arduino : FIXME
        if cont.sensors['MO'].positive:
            scene.objects[name].color = color_hl
        else:
            scene.objects[name].color = color_active

##
# Activation réel
##

# FIXME

    # obj = cont.owner
    # if cont.sensors['MO'].status == JUST_ACTIVATED :
    #     obj.color = couleur_blanc
    #     obj['MO']=True
    # if cont.sensors['Click'].status == JUST_ACTIVATED and scene.objects['System']['manip_mode']==0:
    #     if obj['MO']:
    #         obj_activation['actif'] = True
    #         obj.color = couleur_jaune
    # if cont.sensors['MO'].status == JUST_RELEASED :
    #     obj['MO']=False
    #     if cont.sensors['Click'].status == ACTIVATE :
    #         obj.color = couleur_jaune
    #     else:
    #         obj.color = couleur_orange
    # if cont.sensors['Click'].status == JUST_RELEASED:
    #     obj_activation['actif'] = False
    #     obj.color = couleur_blanc
    #     if cont.sensors['MO'].status != ACTIVATE :
    #         obj.color = couleur_orange

# ###############################################################################    
# # Boutons poussoirs
# ###############################################################################

# # Activation numérique des boutons
# def bp_num(cont):
#     if scene.objects['Systeme']['run'] :
#         obj = cont.owner
#         # print(obj.name, "obj.name")
#         if cont.sensors['MO'].status == JUST_ACTIVATED :
#             obj.color = couleur_blanc
#             obj['MO']=True
#         if cont.sensors['Click'].status == JUST_ACTIVATED and scene.objects['Systeme']['manip_mode']==0:
#             if obj['MO']:
#                 obj['actif_num'] = True
#                 # print(obj.name, "activé")
#                 obj.color = couleur_jaune
#                 serial_msg = obj.name+"\n"
#                 serial_comm.write(serial_msg.encode()) # Communication série : modele 3d -> arduino
#         if cont.sensors['MO'].status == JUST_RELEASED :
#             if obj['MO']:
#                 obj['MO']=False
#                 if cont.sensors['Click'].status == ACTIVATE :
#                     obj.color = couleur_jaune
#                 else:
#                     obj.color = couleur_orange
#         if cont.sensors['Click'].status == JUST_RELEASED:
#             if obj['actif_num']:
#                 obj['actif_num'] = False
#                 # print(obj.name, "desactivé")
#                 obj.color = couleur_blanc
#                 serial_msg = obj.name+" R\n"
#                 # serial_msg = "RELEASED"+"\n"
#                 serial_comm.write(serial_msg.encode()) # Communication série : modele 3d -> arduino
#                 if cont.sensors['MO'].status != ACTIVATE :
#                     obj.color = couleur_orange

# # Affichage de l'activation physique des boutons
# def bp_phy(obj_name, bp_phy_sig):
#     obj=scene.objects[obj_name]
#     if bp_phy_sig =="0":
#         obj['actif_phy'] = True
#         obj.color = couleur_jaune
#     else:
#         if  obj['actif_phy']:
#             obj['actif_phy'] = False
#             obj.color = couleur_orange