'''
==========================================================================================
 Programa: QuadPod MicroPython - Controle de Robô Quadrúpede MePed V2
 Autor:    Cristiano Teixeira
 Licença:  Apache 2.0
 Repositório: https://github.com/ProfessorCristiano
==========================================================================================

 Descrição:
   Controle de um robô quadrúpede (4 patas) MePed V2.0 com 8 servomotores SG90,
   utilizando ESP32 WROOM DevKit 1 e MicroPython.

   Cada perna possui 2 servos:
     - Pivot (Ombro): controla o movimento frontal/traseiro da pata
     - Lift (Joelho): controla o levantamento/abaixamento da pata

   Mapeamento das pernas:
     Pata Esquerda Frente: motor1 (pivot) + motor5 (lift)
     Pata Esquerda Trás:   motor2 (pivot) + motor6 (lift)
     Pata Direita Trás:    motor3 (pivot) + motor7 (lift)
     Pata Direita Frente:  motor4 (pivot) + motor8 (lift)

   Baseado no código Arduino de referência:
     https://www.meped.io/sites/default/files/2017-06/mePed_IR_Starter_Program_0.ino
     https://github.com/ArminJo/QuadrupedControl
==========================================================================================
'''

print("INICIO")
from machine import Pin, PWM
import time

##################################################################
# Classe Servo - Controla um servomotor SG90 via PWM
# Fonte: https://www.upesy.com/blogs/tutorials/esp32-servo-motor-sg90-on-micropython
#
# O servo SG90 aceita sinais PWM de 50Hz com duty cycle entre ~2.6% e ~12.3%
# para controlar o ângulo entre 0° e 180°.
#
# A classe converte um ângulo (0-180) para um valor de duty cycle (26-123)
# e envia o sinal PWM para o pin GPIO especificado.
##################################################################
class Servo:
    __servo_pwm_freq = 50       # Frequência do PWM: 50Hz (padrão para servos)
    __min_u10_duty = 26         # Duty cycle mínimo (~2.6% = 0°)
    __max_u10_duty = 123        # Duty cycle máximo (~12.3% = 180°)
    min_angle = 0               # Ângulo mínimo permitido
    max_angle = 180             # Ângulo máximo permitido
    current_angle = 0.001       # Ângulo atual do servo (para evitar movimentos redundantes)

    def __init__(self, pin):
        self.__initialise(pin)

    def update_settings(self, servo_pwm_freq, min_u10_duty, max_u10_duty, min_angle, max_angle, pin):
        self.__servo_pwm_freq = servo_pwm_freq
        self.__min_u10_duty = min_u10_duty
        self.__max_u10_duty = max_u10_duty
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.__initialise(pin)

    def move(self, angle):
        # Arredonda para 2 casas decimais para evitar ajustes desnecessários
        angle = round(angle, 2)
        # Se já está no ângulo desejado, não faz nada
        if angle == self.current_angle:
            return
        self.current_angle = angle
        # Converte o ângulo para duty cycle e move o motor
        duty_u10 = self.__angle_to_u10_duty(angle)
        self.__motor.duty(duty_u10)

    def __angle_to_u10_duty(self, angle):
        # Converte ângulo (0-180) para valor PWM (26-123)
        return int((angle - self.min_angle) * self.__angle_conversion_factor) + self.__min_u10_duty

    def __initialise(self, pin):
        self.current_angle = -0.001
        self.__angle_conversion_factor = (self.__max_u10_duty - self.__min_u10_duty) / (self.max_angle - self.min_angle)
        self.__motor = PWM(Pin(pin))
        self.__motor.freq(self.__servo_pwm_freq)
##########################################

# =========================================================================
# INICIALIZAÇÃO DOS MOTORES - Cada pin GPIO controla um servo
# =========================================================================
# Servos de Pivot (Ombro) - controlam o ângulo frontal/traseiro da pata
motor1 = Servo(pin=21)   # Front Left Pivot  - Ombro Frente Esquerdo (OFE)
motor2 = Servo(pin=4)    # Back Left Pivot   - Ombro Trás Esquerdo (OTE)
motor3 = Servo(pin=5)    # Back Right Pivot  - Ombro Trás Direito (OTD)
motor4 = Servo(pin=19)   # Front Right Pivot - Ombro Frente Direito (OFD)

# Servos de Lift (Joelho) - controlam o levantamento/abaixamento da pata
# NOTA: Ângulo BAIXO = pata para CIMA (corpo levantado)
#       Ângulo ALTO  = pata para BAIXO (corpo abaixado)
motor5 = Servo(pin=33)   # Front Left Lift   - Joelho Frente Esquerdo (JFE)
motor6 = Servo(pin=14)   # Back Left Lift    - Joelho Trás Esquerdo (JTE)
motor7 = Servo(pin=13)   # Back Right Lift   - Joelho Trás Direito (JTD)
motor8 = Servo(pin=32)   # Front Right Lift  - Joelho Frente Direito (JFD)

# =========================================================================
# VARIÁVEIS GLOBAIS DE ESTADO
# =========================================================================
# s11, s21, s31, s41 = posições atuais dos servos de PIVOT (ombro)
# s12, s22, s32, s42 = posições atuais dos servos de LIFT (joelho)
# spd = velocidade base (unused, mantido para compatibilidade)
# high = ajuste manual de altura do corpo
s11, s21, s31, s41 = 90, 90, 90, 90
s12, s22, s32, s42 = 90, 90, 90, 90
spd = 10
high = 0

def decrease_speed():
    """Diminui a velocidade (incrementa spd, máx 50)"""
    global spd
    if spd < 50:
        spd += 1

# =========================================================================
# FUNÇÃO srv() - Motor de animação dos servos
# =========================================================================
# Esta é a função central que move todos os 8 servos simultaneamente
# de suas posições atuais para as posições desejadas, criando um
# movimento suave e coordenado.
#
# Parâmetros:
#   p11, p21, p31, p41 = posições-ALVO dos 4 servos de PIVOT
#                        (FrenteEsq, TrásEsq, TrásDir, FrenteDir)
#   p12, p22, p32, p42 = posições-ALVO dos 4 servos de LIFT
#   sp1, sp2, sp3, sp4 = velocidade de cada PERNA
#                        (graus avançados por iteração do loop)
#
# Fluxo:
#   1. Ajusta os alvos de lift com a variável 'high' (altura manual)
#   2. Enquanto QUALQUER servo não chegou ao seu alvo:
#      a. Para cada servo, avança ou recua em 'sp' graus (limitado pelo alvo)
#      b. Envia os 8 novos valores para os servos reais
#      c. Aguarda 50ms (controla a taxa de quadros da animação)
#
# Exemplo: srv(120, 60, 180, 0, 6, 33, 33, 42, 1, 1, 1, 3)
#   → Pivot: FrenteEsq=120°, TrásEsq=60°, TrásDir=180°, FrenteDir=0°
#   → Lift:  FrenteEsq=6°(cima), TrásEsq=33°, TrásDir=33°, FrenteDir=42°(baixo)
#   → Velocidade: pernas 1,2,3 = lento(1), perna 4 = rápido(3)
# =========================================================================
def srv(p11, p21, p31, p41, p12, p22, p32, p42, sp1, sp2, sp3, sp4):
    global s11, s21, s31, s41, s12, s22, s32, s42, high

    # Ajusta as posições de lift com a correção de altura manual
    p12 += high * 3
    p22 += high * 3
    p32 += high * 3
    p42 += high * 3

    # Loop de animação: roda até TODOS os servos chegarem ao alvo
    while (s11 != p11 or s21 != p21 or s31 != p31 or s41 != p41 or
           s12 != p12 or s22 != p22 or s32 != p32 or s42 != p42):

        # --- Servos de PIVOT (ombro) ---
        # Front Left Pivot: avança ou recua sp1 graus
        if s11 < p11:
            s11 = min(s11 + sp1, p11)
        elif s11 > p11:
            s11 = max(s11 - sp1, p11)

        # Back Left Pivot: avança ou recua sp2 graus
        if s21 < p21:
            s21 = min(s21 + sp2, p21)
        elif s21 > p21:
            s21 = max(s21 - sp2, p21)

        # Back Right Pivot: avança ou recua sp3 graus
        if s31 < p31:
            s31 = min(s31 + sp3, p31)
        elif s31 > p31:
            s31 = max(s31 - sp3, p31)

        # Front Right Pivot: avança ou recua sp4 graus
        if s41 < p41:
            s41 = min(s41 + sp4, p41)
        elif s41 > p41:
            s41 = max(s41 - sp4, p41)

        # --- Servos de LIFT (joelho) ---
        # Front Left Lift: avança ou recua sp1 graus
        if s12 < p12:
            s12 = min(s12 + sp1, p12)
        elif s12 > p12:
            s12 = max(s12 - sp1, p12)

        # Back Left Lift: avança ou recua sp2 graus
        if s22 < p22:
            s22 = min(s22 + sp2, p22)
        elif s22 > p22:
            s22 = max(s22 - sp2, p22)

        # Back Right Lift: avança ou recua sp3 graus
        if s32 < p32:
            s32 = min(s32 + sp3, p32)
        elif s32 > p32:
            s32 = max(s32 - sp3, p32)

        # Front Right Lift: avança ou recua sp4 graus
        if s42 < p42:
            s42 = min(s42 + sp4, p42)
        elif s42 > p42:
            s42 = max(s42 - sp4, p42)

        # Envia os valores calculados para os 8 servos físicos
        motor1.move(s11)  # OFE - Ombro Frente Esquerdo
        motor2.move(s21)  # OTE - Ombro Trás Esquerdo
        motor3.move(s31)  # OTD - Ombro Trás Direito
        motor4.move(s41)  # OFD - Ombro Frente Direito
        motor5.move(s12)  # JFE - Joelho Frente Esquerdo
        motor6.move(s22)  # JTE - Joelho Trás Esquerdo
        motor7.move(s32)  # JTD - Joelho Trás Direito
        motor8.move(s42)  # JFD - Joelho Frente Direito

        # Aguarda 50ms entre quadros - controla a velocidade visual
        time.sleep(0.05)

# =========================================================================
# FUNÇÃO center_servos() - Centraliza todos os servos
# =========================================================================
# Move todos os 8 servos para 90° (posição neutra).
# Usado como ponto de partida para calibração e para voltar à posição inicial.
# =========================================================================
def center_servos():
    global s11, s21, s31, s41, s12, s22, s32, s42

    # Define as posições centrais
    s11, s21, s31, s41 = 90, 90, 90, 90
    s12, s22, s32, s42 = 90, 90, 90, 90

    # Move os motores para as posições centrais
    motor1.move(s11)
    motor2.move(s21)
    motor3.move(s31)
    motor4.move(s41)
    motor5.move(s12)
    motor6.move(s22)
    motor7.move(s32)
    motor8.move(s42)

# =========================================================================
# FUNÇÃO turn_left() - Gira o corpo para a esquerda
# =========================================================================
# Implementa o giro using o padrão de Creep Turn.
#
# O giro funciona assim:
#   1. Levanta uma pata e move ela 120° na direção do giro
#   2. Move as outras 3 patas 40° na direção oposta ao giro
#   3. Repete para cada pata na sequência
#
# Sequência de 4 passos (um para cada pata):
#   Passo 1: Gira com frente-esquerda levantada
#   Passo 2: Gira com traseira-esquerda levantada
#   Passo 3: Gira com traseira-direita levantada
#   Passo 4: Gira com frente-direita levantada
# =========================================================================
def turn_left():
    # Passo 1: Levanta frente-esquerda (lift=6), move pivot para 150°
    #          As outras 3 patas recuam 40° (pivot diminui)
    srv(150, 50, 170, 10, 6, 42, 42, 42, 3, 1, 1, 1)
    # Passo 2: Abaixa frente-esquerda (lift=42), mantém posição
    srv(150, 50, 170, 10, 42, 42, 42, 42, 1, 1, 1, 1)
    # Passo 3: Levanta traseira-esquerda (lift=6), move pivot para 150°
    #          As outras 3 patas recuam 40°
    srv(110, 150, 130, 50, 42, 6, 42, 42, 1, 3, 1, 1)
    # Passo 4: Abaixa traseira-esquerda (lift=42), mantém posição
    srv(110, 150, 130, 50, 42, 42, 42, 42, 1, 1, 1, 1)

# =========================================================================
# FUNÇÃO turn_right() - Gira o corpo para a direita
# =========================================================================
# Inverso de turn_left(): os ângulos de pivot são invertidos.
# Sequência de 4 passos (um para cada pata):
#   Passo 1: Gira com frente-direita levantada
#   Passo 2: Gira com traseira-direita levantada
#   Passo 3: Gira com traseira-esquerda levantada
#   Passo 4: Gira com frente-esquerda levantada
# =========================================================================
def turn_right():
    # Passo 1: Levanta frente-direita (lift=6), move pivot para 30°
    #          As outras 3 patas avançam 40° (pivot aumenta)
    srv(30, 130, 10, 170, 42, 42, 42, 6, 1, 1, 1, 3)
    # Passo 2: Abaixa frente-direita (lift=42), mantém posição
    srv(30, 130, 10, 170, 42, 42, 42, 42, 1, 1, 1, 1)
    # Passo 3: Levanta traseira-direita (lift=6), move pivot para 30°
    #          As outras 3 patas avançam 40°
    srv(70, 30, 50, 130, 42, 42, 6, 42, 1, 1, 3, 1)
    # Passo 4: Abaixa traseira-direita (lift=42), mantém posição
    srv(70, 30, 50, 130, 42, 42, 42, 42, 1, 1, 1, 1)

# =========================================================================
# FUNÇÃO bow() - Inclina o corpo para frente (gesto de reverência)
# =========================================================================
# Sequência de 3 passos:
#   1. Centraliza os servos
#   2. Abaixa a frente do corpo (lift servos da frente para baixo = 150°)
#   3. Retorna à posição vertical
#
# NOTA: Usa os servos de LIFT (joelho), não de PIVOT (ombro).
#   motor5 (JFE) = Joelho Frente Esquerdo
#   motor8 (JFD) = Joelho Frente Direito
#   Angulos ALTOS (150°) = pata para BAIXO = frente ABAIXADA
# =========================================================================
def bow():
    center_servos()
    time.sleep(0.2)
    # Abaixa a frente - lift servos da frente para baixo (150°)
    motor5.move(150)   # JFE - Joelho Frente Esquerdo para baixo
    motor8.move(150)   # JFD - Joelho Frente Direito para baixo
    time.sleep(0.7)
    # Retorna à posição vertical (90° = neutro)
    motor5.move(90)
    motor8.move(90)
    time.sleep(0.7)

# =========================================================================
# FUNÇÃO lean_left() - Inclina o corpo para a esquerda
# =========================================================================
# Move os servos de LIFT para criar uma inclinação lateral:
#   - Lado esquerdo: patas levantadas (15°)
#   - Lado direito: patas abaixadas (150°)
#
# Lembrete do mapeamento dos lift servos:
#   motor5 (JFE) = Joelho Frente Esquerdo
#   motor6 (JTE) = Joelho Trás Esquerdo
#   motor7 (JTD) = Joelho Trás Direito
#   motor8 (JFD) = Joelho Frente Direito
# =========================================================================
def lean_left():
    # Lado ESQUERDO levantado (15° = pata para cima)
    motor5.move(15)    # JFE - Joelho Frente Esquerdo para cima
    motor6.move(15)    # JTE - Joelho Trás Esquerdo para cima
    # Lado DIREITO abaixado (150° = pata para baixo)
    motor7.move(150)   # JTD - Joelho Trás Direito para baixo
    motor8.move(150)   # JFD - Joelho Frente Direito para baixo

# =========================================================================
# FUNÇÃO lean_right() - Inclina o corpo para a direita
# =========================================================================
# Inverso de lean_left():
#   - Lado direito: patas levantadas (15°)
#   - Lado esquerdo: patas abaixadas (150°)
# =========================================================================
def lean_right():
    # Lado ESQUERDO abaixado (150° = pata para baixo)
    motor5.move(150)   # JFE - Joelho Frente Esquerdo para baixo
    motor6.move(150)   # JTE - Joelho Trás Esquerdo para baixo
    # Lado DIREITO levantado (15° = pata para cima)
    motor7.move(15)    # JTD - Joelho Trás Direito para cima
    motor8.move(15)    # JFD - Joelho Frente Direito para cima

# =========================================================================
# FUNÇÃO trim_left() - Ajusta o trim para a esquerda
# =========================================================================
# Ajusta finamente o alinhamento dos servos de pivot.
# Usado para calibrar o centro de massa do robô.
#
# Reduz 1° dos servos de pivot esquerdo e aumenta 1° do direito,
# fazendo o corpo inclinar levemente para a esquerda.
# =========================================================================
def trim_left():
    global s11, s21, s31, s41
    s11 -= 1  # OFE - Ombro Frente Esquerdo diminui
    s21 -= 1  # OTE - Ombro Trás Esquerdo diminui
    s31 += 1  # OTD - Ombro Trás Direito aumenta
    s41 += 1  # OFD - Ombro Frente Direito aumenta
    # Aplica as novas posições
    motor1.move(s11)
    motor2.move(s21)
    motor3.move(s31)
    motor4.move(s41)

# =========================================================================
# FUNÇÃO trim_right() - Ajusta o trim para a direita
# =========================================================================
# Inverso de trim_left(): aumenta pivot esquerdo, reduz direito.
# =========================================================================
def trim_right():
    global s11, s21, s31, s41
    s11 += 1  # OFE - Ombro Frente Esquerdo aumenta
    s21 += 1  # OTE - Ombro Trás Esquerdo aumenta
    s31 -= 1  # OTD - Ombro Trás Direito diminui
    s41 -= 1  # OFD - Ombro Frente Direito diminui
    # Aplica as novas posições
    motor1.move(s11)
    motor2.move(s21)
    motor3.move(s31)
    motor4.move(s41)

# =========================================================================
# FUNÇÃO dance() - Executa uma sequência de dança
# =========================================================================
# Alterna entre lean_left e lean_right 3 vezes,
# depois retorna à posição central.
# =========================================================================
def dance():
    for _ in range(3):
        lean_left()
        time.sleep(0.5)
        lean_right()
        time.sleep(0.5)
    center_servos()

# =========================================================================
# FUNÇÃO wave() - Acena com uma pata (gesto de "tchau")
# =========================================================================
# Sequência mais natural:
#   1. Levanta a pata frente-esquerda (lift para cima)
#   2. Move o pivot para cima e para baixo 3 vezes (aceno)
#   3. Abaixa a pata e retorna à posição neutra
#
# NOTA: Precisa usar lift E pivot para um aceno realista.
#   motor1 (OFE) = Ombro Frente Esquerdo (Pivot)
#   motor5 (JFE) = Joelho Frente Esquerdo (Lift)
# =========================================================================
def wave():
    center_servos()
    time.sleep(0.2)
    # Levanta a pata frente-esquerda
    motor5.move(6)     # JFE - Joelho para cima (lift=6°)
    time.sleep(0.3)
    # Acena: move o pivot para cima e para baixo
    for _ in range(3):
        motor1.move(45)   # OFE - Ombro para cima (aceno)
        time.sleep(0.4)
        motor1.move(135)  # OFE - Ombro para baixo (aceno)
        time.sleep(0.4)
    # Retorna à posição neutra
    motor1.move(90)    # OFE - Ombro no centro
    time.sleep(0.2)
    motor5.move(90)    # JFE - Joelho no centro (abaixa a pata)
    time.sleep(0.3)

# =========================================================================
# FUNÇÃO forward() - Anda para frente (CORRIGIDA)
# =========================================================================
# Implementa o Creep Gait (marcha de creep) para o robô quadrúpede.
#
# O creep gait funciona em 4 fases por ciclo completo:
#
#   FASE 1: Levantar a diagonal frente-esquerda + traseira-direita
#           (as outras duas patas ficam apoiadas no chão)
#           → As patas levantadas são preparadas para avançar
#
#   FASE 2: Mover o corpo para frente
#           (as patas apoiadas empurram o corpo)
#           → Os servos de pivot mudam de 120/60/180/0 para 90/30/150/90
#
#   FASE 3: Levantar a diagonal traseira-esquerda + frente-direita
#           (a primeira diagonal agora está apoiada)
#
#   FASE 4: Mover o corpo para frente novamente
#           → Os servos de pivot voltam de 90/30/150/90 para 120/60/180/0
#
#   Resultado: o corpo avança a cada ciclo completo de 4 fases.
#
# Parâmetros de velocidade (sp1, sp2, sp3, sp4):
#   sp1 = velocidade da pata frente-esquerda
#   sp2 = velocidade da pata traseira-esquerda
#   sp3 = velocidade da pata traseira-direita
#   sp4 = velocidade da pata frente-direita
#
# Valores de lift:
#   6°   = pata levantada (para cima)
#   33°  = pata no nível do chão (neutro)
#   42°  = pata pressionando o chão (abaixada)
# =========================================================================
def forward():
    # FASE 1: Levanta diagonal frente-esquerda (lift=6) e traseira-direita (lift=6)
    #         As outras patas ficam no chão (lift=33)
    #         Pivot: posição recuada (120, 60, 180, 0)
    srv(120, 60, 180, 0, 6, 33, 33, 42, 1, 1, 1, 3)

    # FASE 2: Move o corpo para frente
    #         Pivot avança: (90, 30, 150, 90) — cada perna avança ~30°
    #         Lift: frente-esquerda volta ao chão (42), traseira-direita volta ao chão (42)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 1, 1, 1, 3)

    # FASE 3: Levanta diagonal traseira-esquerda (lift=6) e frente-direita (lift=6)
    #         As outras patas ficam no chão (lift=33)
    #         Pivot: posição avançada (90, 30, 150, 90)
    srv(90, 30, 150, 90, 42, 6, 42, 6, 1, 3, 1, 1)

    # FASE 4: Move o corpo para frente novamente
    #         Pivot recua: (120, 60, 180, 0) — cada perna avança ~30°
    #         Lift: traseira-esquerda volta ao chão (33), frente-direita volta ao chão (33)
    srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3)

# =========================================================================
# FUNÇÃO back() - Anda para trás (CORRIGIDA)
# =========================================================================
# Implementa o Creep Gait reverso.
# A lógica é idêntica ao forward(), mas os ângulos de pivot
# são invertidos: quando o corpo avança no forward(), aqui ele recua.
#
# FASE 1: Levanta diagonal frente-esquerda + traseira-direita
#         Pivot: posição avançada (90, 30, 150, 90)
#
# FASE 2: Move o corpo para trás
#         Pivot permanece avançado (90, 30, 150, 90)
#
# FASE 3: Levanta diagonal traseira-esquerda + frente-direita
#         Pivot: posição recuada (120, 60, 180, 0)
#
# FASE 4: Move o corpo para trás novamente
#         Pivot permanece recuado (120, 60, 180, 0)
# =========================================================================
def back():
    # FASE 1: Levanta diagonal frente-esquerda (lift=6) e traseira-direita (lift=6)
    #         Pivot: posição avançada (90, 30, 150, 90)
    srv(90, 30, 150, 90, 6, 33, 33, 42, 3, 1, 1, 1)

    # FASE 2: Move o corpo para trás
    #         Pivot permanece (90, 30, 150, 90)
    #         Lift: frente-esquerda volta ao chão (42)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 3, 1, 1, 1)

    # FASE 3: Levanta diagonal traseira-esquerda (lift=6) e frente-direita (lift=6)
    #         Pivot: posição recuada (120, 60, 180, 0)
    srv(120, 60, 180, 0, 42, 6, 42, 6, 1, 1, 3, 1)

    # FASE 4: Move o corpo para trás novamente
    #         Pivot permanece (120, 60, 180, 0)
    #         Lift: traseira-esquerda volta ao chão (33), frente-direita volta ao chão (33)
    srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 3, 1)

# =========================================================================
# PROGRAMA PRINCIPAL
# =========================================================================
if __name__ == "__main__":
    # Centraliza todos os servos ao iniciar
    center_servos()
    time.sleep(1)

    # Sequência de demonstração das funcionalidades
    print("Faz o arco")
    bow()
    time.sleep(2)

    print("Inclina para esquerda")
    lean_left()
    time.sleep(2)

    print("Inclina para Direita")
    lean_right()
    time.sleep(2)

    print("Dança")
    dance()
    time.sleep(2)

    print("Onda")
    wave()
    time.sleep(2)

    print("Frente")
    forward()
    time.sleep(2)

    print("Trás")
    back()
    time.sleep(2)

    # Loop infinito: continua andando para frente
    while True:
        print("Frente continuo")
        forward()