'''
///////////////////////////////////////////////////////////////////////////
// Programa desenvolvido por Cristiano Teixeira                          //
// Sob Licença Apache 2.0                                                //
// https://github.com/ProfessorCristiano                                 //
// Hexapod com MicroPython e ESP32                                       //
// Programa utiliza ESP32 e Servo motores SG90 para controlar um quadpod //
// MePed V2.0, com 8 servos, para 4 patas                                //
//                                                                       //
// https://meped.io/mepedv2                                              //
// programa exemplo em:                                                  //
// https://www.meped.io/sites/default/files/2017-06/mePed_IR_Starter_Program_0.ino //
//                                                                       //
///////////////////////////////////////////////////////////////////////////


//==========================================================================================
//
//  Program for controlling a mePed Robot using an IR Remote
//
//  The mePed is an open source quadruped robot designed by Scott Pierce of
//  Spierce Technologies (www.meped.io & www.spiercetech.com)
//
//  This program is based on code written by Alexey Butov (www.alexeybutov.wix.com/roboset)
//
//==========================================================================================
'''

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

##################################################################
#Classe Servo do site: https://www.upesy.com/blogs/tutorials/esp32-servo-motor-sg90-on-micropython#
class Servo:
    # these defaults work for the standard TowerPro SG90
    __servo_pwm_freq = 50
    __min_u10_duty = 26 - 0 # offset for correction
    __max_u10_duty = 123- 0  # offset for correction
    min_angle = 0
    max_angle = 180
    current_angle = 0.001


    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):
        # round to 2 decimal places, so we have a chance of reducing unwanted servo adjustments
        angle = round(angle, 2)
        # do we need to move?
        if angle == self.current_angle:
            return
        self.current_angle = angle
        # calculate the new duty cycle and move the motor
        duty_u10 = self.__angle_to_u10_duty(angle)
        self.__motor.duty(duty_u10)

    def __angle_to_u10_duty(self, angle):
        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 (substitua pelos objetos reais)
motor1 = Servo(pin=21)   # Front Left Pivot Servo  - OFE - Ombro Frente Esquerdo
motor2 = Servo(pin=4)    # Back Left Pivot Servo   - OTE - Ombro Tras Esquerdo
motor3 = Servo(pin=5)    # Back Right Pivot Servo  - OTD - Ombro Tras Direito
motor4 = Servo(pin=19)   # Front Right Pivot Servo - OFD - Ombro Frente Direito
motor5 = Servo(pin=33)   # Front Left Lift Servo   - JFE - Joelho Frente Esquerdo
motor6 = Servo(pin=14)   # Back Left Lift Servo    - JTE - Joelho Tras Esquerdo
motor7 = Servo(pin=13)   # Back Right Lift Servo   - JTD - Joelho Tras Direito
motor8 = Servo(pin=32)   # Front Right Lift Servo  - JFD - Joelho Frente Direito

# Variáveis globais
s11, s21, s31, s41 = 90, 90, 90, 90  # Posições iniciais dos servos de pivô
s12, s22, s32, s42 = 90, 90, 90, 90  # Posições iniciais dos servos de elevação
spd = 10  # Velocidade inicial
high = 0  # Ajuste manual de altura

# Função para diminuir a velocidade
def decrease_speed():
    global spd
    if spd < 50:
        spd += 1

# Função para mover os servos com base nos parâmetros fornecidos
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 dos servos de elevação com base no ajuste manual de altura
    p12 += high * 3
    p22 += high * 3
    p32 += high * 3
    p42 += high * 3

    # Enquanto as posições atuais não forem iguais às desejadas
    while (s11 != p11 or s21 != p21 or s31 != p31 or s41 != p41 or
           s12 != p12 or s22 != p22 or s32 != p32 or s42 != p42):

        # Front Left Pivot Servo
        if s11 < p11:
            s11 = min(s11 + sp1, p11)
        elif s11 > p11:
            s11 = max(s11 - sp1, p11)

        # Back Left Pivot Servo
        if s21 < p21:
            s21 = min(s21 + sp2, p21)
        elif s21 > p21:
            s21 = max(s21 - sp2, p21)

        # Back Right Pivot Servo
        if s31 < p31:
            s31 = min(s31 + sp3, p31)
        elif s31 > p31:
            s31 = max(s31 - sp3, p31)

        # Front Right Pivot Servo
        if s41 < p41:
            s41 = min(s41 + sp4, p41)
        elif s41 > p41:
            s41 = max(s41 - sp4, p41)

        # Front Left Lift Servo
        if s12 < p12:
            s12 = min(s12 + sp1, p12)
        elif s12 > p12:
            s12 = max(s12 - sp1, p12)

        # Back Left Lift Servo
        if s22 < p22:
            s22 = min(s22 + sp2, p22)
        elif s22 > p22:
            s22 = max(s22 - sp2, p22)

        # Back Right Lift Servo
        if s32 < p32:
            s32 = min(s32 + sp3, p32)
        elif s32 > p32:
            s32 = max(s32 - sp3, p32)

        # Front Right Lift Servo
        if s42 < p42:
            s42 = min(s42 + sp4, p42)
        elif s42 > p42:
            s42 = max(s42 - sp4, p42)

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

        # Pequeno atraso para simular a velocidade
        time.sleep(0.05)

# Função para centralizar os servos
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 para girar à esquerda
def turn_left():
    srv(120, 60, 180, 0, 42, 33, 6, 42, 1, 1, 3, 1)
    srv(120, 60, 180, 0, 42, 33, 33, 24, 1, 1, 3, 1)
    srv(90, 30, 150, 90, 42, 33, 33, 6, 1, 1, 1, 3)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 1, 1, 1, 3)
    srv(180, 0, 120, 60, 6, 33, 33, 42, 3, 1, 1, 1)
    srv(180, 0, 120, 60, 42, 33, 33, 33, 3, 1, 1, 1)

# Função para girar à direita
def turn_right():
    srv(90, 30, 150, 90, 6, 33, 33, 42, 3, 1, 1, 1)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 3, 1, 1, 1)
    srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3)
    srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3)
    srv(150, 90, 90, 30, 42, 33, 6, 42, 1, 1, 3, 1)
    srv(150, 90, 90, 30, 42, 33, 33, 42, 1, 1, 3, 1)
    srv(180, 0, 120, 60, 42, 6, 33, 42, 1, 3, 1, 1)
    srv(180, 0, 120, 60, 42, 33, 33, 42, 1, 3, 1, 1)

# Função para inclinar para frente (Bow)
def bow():
    center_servos()
    time.sleep(0.2)
    motor2.move(15)
    motor8.move(15)
    time.sleep(0.7)
    motor2.move(90)
    motor8.move(90)
    time.sleep(0.7)

# Função para inclinar para a esquerda (Lean Left)
def lean_left():
    motor2.move(15)
    motor4.move(15)
    motor6.move(150)
    motor8.move(150)

# Função para inclinar para a direita (Lean Right)
def lean_right():
    motor2.move(150)
    motor4.move(150)
    motor6.move(15)
    motor8.move(15)

# Função para ajustar para a esquerda (Trim Left)
def trim_left():
    global da, db, dc
    da -= 1  # Left Front Pivot
    db -= 1  # Left Back Pivot
    dc -= 1  # Right Back Pivot

# Função para ajustar para a direita (Trim Right)
def trim_right():
    global da, db, dc
    da += 1  # Left Front Pivot
    db += 1  # Left Back Pivot
    dc += 1  # Right Back Pivot

# Função para dançar (Dance)
def dance():
    for _ in range(3):  # Repete a dança 3 vezes
        lean_left()
        time.sleep(0.5)
        lean_right()
        time.sleep(0.5)
    center_servos()

# Função para acenar (Wave)
def wave():
    center_servos()
    time.sleep(0.2)
    for _ in range(3):  # Repete o aceno 3 vezes
        motor1.move(45)  # Move o braço para cima
        time.sleep(0.5)
        motor1.move(90)  # Retorna à posição inicial
        time.sleep(0.5)

# Função para mover para frente (Forward)
def forward():
    srv(120, 60, 180, 0, 42, 33, 6, 42, 1, 1, 3, 1)
    srv(120, 60, 180, 0, 42, 33, 33, 24, 1, 1, 3, 1)
    srv(90, 30, 150, 90, 42, 33, 33, 6, 1, 1, 1, 3)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 1, 1, 1, 3)

# Função para mover para trás (Back)
def back():
    srv(90, 30, 150, 90, 6, 33, 33, 42, 3, 1, 1, 1)
    srv(90, 30, 150, 90, 42, 33, 33, 42, 3, 1, 1, 1)
    srv(120, 60, 180, 0, 42, 33, 33, 6, 1, 1, 1, 3)
    srv(120, 60, 180, 0, 42, 33, 33, 42, 1, 1, 1, 3)

# Programa principal
if __name__ == "__main__":
    center_servos()
    time.sleep(1)

    # Exemplos de execução
    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)
    while True:
        print("Frente continuo")
        forward()