import time
import math

from Control import *

#Creating object 'control' of 'Control' class.
control=Control()

class CatWalk(Control):  # Inheriting from the Control class
    def __init__(self):
        super().__init__()  # Initialize the parent class
    
    def cat_walk_cycle(self, steps=10, speed=5):
        """
        Perform a walking cycle that mimics a cat-like movement.
        :param steps: The number of steps the robot should take.
        :param speed: The speed of the movement.
        """
        for step in range(steps):
            # Cat alternates between front and back legs.
            # Step 1: Lift and move the front left and back right legs forward
            self.move_leg(0, forward=True, height_offset=10)  # Front left leg up and forward
            self.move_leg(3, forward=True, height_offset=10)  # Back right leg up and forward
            time.sleep(0.2 / speed)
            
            # Step 2: Lower those legs and move the front right and back left legs
            self.move_leg(0, forward=False, height_offset=0)  # Front left leg down
            self.move_leg(3, forward=False, height_offset=0)  # Back right leg down
            time.sleep(0.2 / speed)
            
            self.move_leg(1, forward=True, height_offset=10)  # Front right leg up and forward
            self.move_leg(2, forward=True, height_offset=10)  # Back left leg up and forward
            time.sleep(0.2 / speed)
            
            # Step 3: Lower those legs
            self.move_leg(1, forward=False, height_offset=0)  # Front right leg down
            self.move_leg(2, forward=False, height_offset=0)  # Back left leg down
            time.sleep(0.2 / speed)
            
            # Slight body sway, cats have a natural sway when walking
            self.sway_body()

    def move_leg(self, leg_index, forward=True, height_offset=10):
        """
        Move a single leg forward or backward.
        :param leg_index: Index of the leg (0 to 3).
        :param forward: True to move forward, False to move backward.
        :param height_offset: How much to lift the leg.
        """
        x_move = 20 if forward else -20
        y_move = self.height + height_offset
        
        # Adjust point for the leg movement
        self.point[leg_index][0] += x_move
        self.point[leg_index][1] = y_move
        
        self.run()

    def sway_body(self):
        """
        Add a slight body sway, simulating the natural motion of a cat's body.
        """
        for i in range(2):
            self.angle[i][0] += 5  # Small tilt to the left
            self.angle[i+2][0] -= 5  # Small tilt to the right
        self.run()
        time.sleep(0.05)
        
        for i in range(2):
            self.angle[i][0] -= 5  # Reset tilt
            self.angle[i+2][0] += 5  # Reset tilt
        self.run()
        time.sleep(0.05)

if __name__ == "__main__":
    # Create an instance of the CatWalk class and run the walking cycle
    robot = CatWalk()
    robot.cat_walk_cycle(steps=20, speed=8)  # 20 steps with moderate speed