import csv
import subprocess
import time
import argparse
from datetime import datetime
from rplidar import RPLidar

def run_ticcmd(*args):
    try:
        result = subprocess.run(['ticcmd'] + list(args), check=True)
        print(f"Command success: {' '.join(args)}")
        return result
    except subprocess.CalledProcessError as e:
        print(f"Command failed: {' '.join(args)}\nError: {e}")
        return str(e)

def prepare_motor():
    run_ticcmd('--reset-command-timeout')
    run_ticcmd('--clear-driver-error')
    run_ticcmd('--exit-safe-start')

# Motor setup
run_ticcmd('--reset')
time.sleep(0.5)
run_ticcmd('--step-mode', '4')  # Updated to 1/4 step mode
prepare_motor()
run_ticcmd('--energize')

# Motion parameters for 1/4 steps
steps_per_rotation = 200  # Full steps for one full rotation (360°)
microstep_factor = 4  # 1/4 microstepping
total_microsteps_for_180 = (steps_per_rotation / 2) * microstep_factor  # 400 microsteps for 180°
degree_step_size = 180.0 / total_microsteps_for_180  # 0.45° per microstep for 180°

max_steps = total_microsteps_for_180  # Set max steps to 400 for 180° rotation

# Parse arguments
parser = argparse.ArgumentParser(description='LIDAR scanning script.')
parser.add_argument('-scan', type=int, default=30, help='Maximum scans per step (default: 30)')
args = parser.parse_args()
max_scans_per_step = max(30, args.scan)

# LIDAR setup
lidar = RPLidar('/dev/tty.usbserial-0001')

current_step = 0
scans_count = 0
seen_measurements = set()
file_path = f'./CSV/scanData_{datetime.now().strftime("%Y%m%d_%H%M%S")}.csv'

try:
    print("Starting LIDAR scanning... This will run for multiple scans.")
    
    lidar.start_motor()
    
    with open(file_path, mode='w', newline='') as file:
        writer = csv.writer(file)
        writer.writerow(['Quality', 'Angle (degrees)', 'Distance (mm)', 'Rotation'])
        print(f"CSV file opened for writing: {file_path}")

        for scan in lidar.iter_scans():
            if len(scan) > 0:
                rotation = current_step * degree_step_size
                print(f"New scan with {len(scan)} measurements at Rotation {rotation:.3f} degrees:")
                print(f"Scan data: {scan}")

                for measurement in scan:
                    try:
                        if len(measurement) != 3:
                            raise ValueError(f"Unexpected measurement length: {len(measurement)}")
                        
                        quality, angle, distance = measurement
                        measurement_id = (angle, distance)
                        
                        if measurement_id not in seen_measurements and quality >= 1:
                            seen_measurements.add(measurement_id)
                            print(f"Quality: {quality}, Angle: {angle:.2f} degrees, Distance: {distance:.2f} mm, Rotation: {rotation:.3f}")
                            writer.writerow([quality, angle, distance, rotation])
                    except Exception as e:
                        print(f"Error processing measurement: {e}")

                scans_count += 1
                if scans_count >= max_scans_per_step:
                    scans_count = 0
                    current_step += 1
                    print(f"Rotation step incremented to {current_step}")

                    target_position = current_step
                    try:
                        prepare_motor()
                        run_ticcmd('--position', str(target_position))
                        time.sleep(1)
                    except Exception as e:
                        print(f"Error moving motor: {e}")

                    print("Saving data to CSV after rotation step.")
                    try:
                        file.flush()
                    except Exception as e:
                        print(f"Error flushing file: {e}")

                    if current_step >= max_steps:
                        print("Maximum steps reached. Stopping LIDAR.")
                        break

except Exception as e:
    print(f"Error with CSV file handling: {e}")

finally:
    print("Stopping the LIDAR...")
    lidar.stop_motor()
    lidar.disconnect()
    print("LIDAR stopped and disconnected.")

    print("Rotating back to 0 degrees...")
    try:
        prepare_motor()
        run_ticcmd('--position', '0')
        time.sleep(1)
    except Exception as e:
        print(f"Error rotating back to 0: {e}")