from picarx import Picarx
                from time import sleep
                import time
                
                px = Picarx()
                
                # Track attempts and offsets
                #attempts = []
                offset = 0 # Initial offset angle
                
                def run():
                    offset = 0  
                    minAngle = -10
                    maxAngle = 10
                    
                    while True:
                        print(f"Setting servo angle to offset: {offset}")
                        px.set_dir_servo_angle(offset)
                        
                        # Move forward and recalibrate if it goes off the line
                        px.forward(5)
                        sleep(0.5)
                        while not off_line():  # Move forward until off the line
                            continue
                        # If the robot goes off the line, stop and adjust
                        px.stop()
                       
                        if end_of_line():
                            break
                             
                        data = px.get_grayscale_data()
                        state = px.get_line_status(data)
                        print(f"Off line, Recalibrating...")
                        sleep(0.5)
                        # Backward to return to the starting point
                        px.backward(5)
                        sleep(0.5)
                        while not off_line():  # Move backward until at starting point
                            continue
                        px.stop()
                        
                        
                        if state[0] == 1:  # Left sensor off
                            print("Adjusting to the left")
                            maxAngle = offset;
                        elif state[2] == 1:  # Right sensor off
                            print("Adjusting to the right")
                            minAngle = 0;
                
                        offset = (minAngle + maxAngle) / 2
                          
                        print(f"New recalibrated offset: {offset}")
                        sleep(0.5)  # Stabilization delay
                    print("End of line reached.")
                    px.stop()
                    return offset
                
                def end_of_line():
                    """Check if all sensors detect the end of the line."""
                    data = px.get_grayscale_data()
                    state = px.get_line_status(data)  # [bool, bool, bool]
                    print(f"End of line check - sensor state: {state}")
                    return all(sensor == 1 for sensor in state)
                
                def off_line(): 
                    """Check if any sensor is off the line."""
                    data = px.get_grayscale_data()
                    state = px.get_line_status(data)
                    #print(f"Off line check - sensor state: {state}")
                    return state[0] != 0 or state[2] != 0
                
                
                    
                if __name__ == '__main__':
                    print("Final offset:", run())