import epuck2
import time
import sys
import gc # Garbage collector

# --- Configuration ---
REFRESH_PERIOD = 0.02  # 50 Hz

# ANSI Codes
CURSOR_HOME = "\033[H"
HIDE_CURSOR = "\033[?25l"
SHOW_CURSOR = "\033[?25h"
CLEAR_SCREEN = "\033[2J"

SENSOR_MAP = [
    ("Accel X", "raw", "{:>5.0f}"), ("Accel Y", "raw", "{:>5.0f}"), ("Accel Z", "raw", "{:>5.0f}"),
    ("Acceleration", "mg", "{:>6.1f}"), ("Orientation", "deg", "{:>6.1f}"), ("Inclination", "deg", "{:>5.1f}"),
    ("Gyro X", "raw", "{:>6.0f}"), ("Gyro Y", "raw", "{:>6.0f}"), ("Gyro Z", "raw", "{:>6.0f}"),
    ("Mag X", "uT", "{:>6.1f}"), ("Mag Y", "uT", "{:>6.1f}"), ("Mag Z", "uT", "{:>6.1f}"),
    ("Temperature", "C", "{:>5.0f}"), ("IR Prox 0", "raw", "{:>4.0f}"), ("IR Prox 1", "raw", "{:>4.0f}"),
    ("IR Prox 2", "raw", "{:>4.0f}"), ("IR Prox 3", "raw", "{:>4.0f}"), ("IR Prox 4", "raw", "{:>4.0f}"),
    ("IR Prox 5", "raw", "{:>4.0f}"), ("IR Prox 6", "raw", "{:>4.0f}"), ("IR Prox 7", "raw", "{:>4.0f}"),
    ("IR Amb 0", "raw", "{:>4.0f}"), ("IR Amb 1", "raw", "{:>4.0f}"), ("IR Amb 2", "raw", "{:>4.0f}"),
    ("IR Amb 3", "raw", "{:>4.0f}"), ("IR Amb 4", "raw", "{:>4.0f}"), ("IR Amb 5", "raw", "{:>4.0f}"),
    ("IR Amb 6", "raw", "{:>4.0f}"), ("IR Amb 7", "raw", "{:>4.0f}"), ("Time of Flight", "mm", "{:>5.0f}"),
    ("Mic 0 Vol", "raw", "{:>4.0f}"), ("Mic 1 Vol", "raw", "{:>4.0f}"), ("Mic 2 Vol", "raw", "{:>4.0f}"),
    ("Mic 3 Vol", "raw", "{:>4.0f}"), ("L Motor Steps", "stp", "{:>5.0f}"), ("R Motor Steps", "stp", "{:>5.0f}"),
    ("Battery", "raw", "{:>4.0f}"), ("uSD State", "bol", "{:>4.0f}"), ("TV Rem Tgl", "raw", "{:>4.0f}"),
    ("TV Rem Addr", "raw", "{:>4.0f}"), ("TV Rem Data", "raw", "{:>4.0f}"), ("Selector", "pos", "{:>4.0f}"),
    ("Grnd Prox 0", "raw", "{:>4.0f}"), ("Grnd Prox 1", "raw", "{:>4.0f}"), ("Grnd Prox 2", "raw", "{:>4.0f}"),
    ("Grnd Amb 0", "raw", "{:>4.0f}"), ("Grnd Amb 1", "raw", "{:>4.0f}"), ("Grnd Amb 2", "raw", "{:>4.0f}"),
    ("Button", "bol", "{:>4.0f}"),
]

def main():
    # Pre-calculate dimensions
    total_sensors = len(SENSOR_MAP)
    cols = 3
    rows = (total_sensors + cols - 1) // cols
    
    # Row Template: | Name (Unit): Value
    # Using simple concatenation in the loop is safer for memory than complex f-strings
    
    # Initial Screen Clear
    sys.stdout.write(CLEAR_SCREEN)
    sys.stdout.write(HIDE_CURSOR)

    # Divider line
    DIVIDER = "-" * 105 + "\n"

    try:
        while True:
            # 1. Move Cursor Home (Overwrite previous data)
            sys.stdout.write(CURSOR_HOME)
            sys.stdout.write("--- E-puck2 Sensor Dashboard (50 Hz) ---\n")
            sys.stdout.write(DIVIDER)

            # 2. Get Data
            data = epuck2.get_all_sensors()

            # 3. Stream Data Line-by-Line (Low Memory)
            for r in range(rows):
                # We construct only ONE line at a time
                line = "|"
                for c in range(cols):
                    idx = c * rows + r
                    if idx < total_sensors:
                        name, unit, fmt = SENSOR_MAP[idx]
                        val = data[idx]
                        
                        # Format the value
                        val_str = fmt.format(val)
                        
                        # Manual column construction to avoid complex format objects
                        # "Name           (Unit): Value      "
                        # We assume a fixed column width of roughly 34 chars
                        cell_content = " {:<14} ({:<3}): {}".format(name, unit, val_str)
                        
                        # Pad manually to 34 chars to ensure alignment
                        padding = 34 - len(cell_content)
                        if padding > 0:
                            cell_content += " " * padding
                        
                        line += cell_content
                    else:
                        # Empty cell
                        line += " " * 34
                    
                    line += "|"
                
                # Print this line immediately and discard the string
                sys.stdout.write(line + "\n")

            sys.stdout.write(DIVIDER)
            sys.stdout.write("Press Ctrl+C to exit.\n")
            
            # 4. Garbage Collection
            # Crucial for embedded loops: clean up the temporary string objects we just made
            gc.collect()

            time.sleep(REFRESH_PERIOD)

    except KeyboardInterrupt:
        sys.stdout.write(SHOW_CURSOR)
        print("\nStopped.")
    except Exception as e:
        sys.stdout.write(SHOW_CURSOR)
        print("\nError:", e)

if __name__ == "__main__":
    main()