import time
import math

from Control import *

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



#from control import 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):
        # Refined walk cycle for a cat-like gait
        for step in range(steps):
            # Moving diagonal pairs of legs
            
            # Step 1: Move front-left and back-right forward
            self.move_leg('front_left', x=0.1, y=0.2, z=0.0, speed=speed)  # adjust positions
            self.move_leg('back_right', x=0.1, y=0.2, z=0.0, speed=speed)
            self.move_leg('front_right', x=0.0, y=-0.1, z=0.1, speed=speed)
            self.move_leg('back_left', x=0.0, y=-0.1, z=0.1, speed=speed)

            # Intermediate step: All legs slightly touch down to create fluid movement
            self.synchronize_legs(speed=speed * 0.5)  # Smooth transition
        
            # Step 2: Move front-right and back-left forward
            self.move_leg('front_right', x=0.1, y=0.2, z=0.0, speed=speed)
            self.move_leg('back_left', x=0.1, y=0.2, z=0.0, speed=speed)
            self.move_leg('front_left', x=0.0, y=-0.1, z=0.1, speed=speed)
            self.move_leg('back_right', x=0.0, y=-0.1, z=0.1, speed=speed)
            
            # Smooth landing
            self.synchronize_legs(speed=speed * 0.5)
    
    def move_leg(self, leg_name, x, y, z, speed):
        """
        Move the specified leg to the given (x, y, z) coordinate with a given speed.
        The coordinates should be within the robot's active range to prevent out-of-bound movements.
        """
        # Ensure the coordinates are within a valid range (you can fine-tune these based on your robot's specifications)
        x = max(min(x, 1.0), -1.0)  # Adjust range as per robot's limits
        y = max(min(y, 1.0), -1.0)
        z = max(min(z, 1.0), -1.0)

        # Move the leg using the robot's control system
        # (Assuming there's a low-level method to handle this)
        print(f"Moving {leg_name}: to x={x}, y={y}, z={z} at speed={speed}")
        # Here would be a command to send to the robot's hardware or simulation
        
    def synchronize_legs(self, speed):
        """
        Synchronize all legs by performing a smooth, controlled transition of all leg positions.
        This helps create a fluid walking motion, characteristic of a cat's gait.
        """
        # Command to move all legs together in a synchronized manner
        print(f"Synchronizing all legs with speed={speed}")
        # Simulate synchronization logic
        # This is where you'd implement a smooth, in-between motion for all legs

if __name__ == "__main__":
    # Instantiate the CatWalk class
    robot = CatWalk()
    
    # Start the cat-like walking cycle with 20 steps and a moderate speed of 8
    robot.cat_walk_cycle(steps=20, speed=8)