#!/usr/bin/env python3
"""
LIN LED Control Test

This test verifies LIN communication by controlling the LED on the board.
It will fade through different colors (Red, Green, Blue) to verify that
frames are being received correctly.

Frame structure (ALM_Req_A, ID=0x0A, 8 bytes):
- Byte 0: AmbLightColourRed   (0-255)
- Byte 1: AmbLightColourGreen (0-255)
- Byte 2: AmbLightColourBlue  (0-255)
- Byte 3: AmbLightIntensity   (0-255)
- Byte 4: AmbLightUpdate[1:0] | (AmbLightMode[5:0] << 2)
- Byte 5: AmbLightDuration    (0-255)
- Byte 6: AmbLightLIDFrom     (NAD range start — set equal to LIDTo to target one node)
- Byte 7: AmbLightLIDTo       (NAD range end)
"""

import argparse
import logging
import time
import math
from pylin import LinBusManager, LinDevice22
from pymumclient import MelexisUniversalMaster
from config import *

logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)-15s %(levelname)-8s %(message)s'
)
logger = logging.getLogger(__name__)


def read_alm_status(lin_dev):
    """Read ALM_Status frame and return (ALMNadNo, raw_bytes)."""
    try:
        response = lin_dev.send_message(
            master_to_slave=False,
            frame_id=ALM_STATUS_FRAME['frame_id'],
            data_length=ALM_STATUS_FRAME['length'],
            data=None
        )
        if response and len(response) >= ALM_STATUS_FRAME['length']:
            parsed = unpack_frame(ALM_STATUS_FRAME, response)
            return parsed['ALMNadNo'], response
        return None, None
    except Exception as e:
        logger.error(f"Failed to read ALM_Status: {e}")
        return None, None


def set_led_color(lin_dev, nad, red, green, blue, intensity,
                  update=0, mode=0, duration=0):
    """
    Set LED color and intensity via ALM_Req_A frame.

    The node responds only if AmbLightLIDFrom <= ALMNadNo <= AmbLightLIDTo.
    Setting both to the same NAD targets a single node.
    """
    try:
        data = pack_frame(ALM_REQ_A_FRAME,
            AmbLightColourRed=red,
            AmbLightColourGreen=green,
            AmbLightColourBlue=blue,
            AmbLightIntensity=intensity,
            AmbLightUpdate=update,
            AmbLightMode=mode,
            AmbLightDuration=duration,
            AmbLightLIDFrom=nad,
            AmbLightLIDTo=nad,
        )
        lin_dev.send_message(
            master_to_slave=True,
            frame_id=ALM_REQ_A_FRAME['frame_id'],
            data_length=ALM_REQ_A_FRAME['length'],
            data=data,
        )
        return True
    except Exception as e:
        logger.error(f"Failed to set LED color: {e}")
        return False


def fade_test(lin_dev, nad, duration_per_color=5.0):
    """
    Fade through Red, Green, and Blue colors.

    Args:
        lin_dev: LinDevice22 instance
        nad: Node address
        duration_per_color: How long to fade each color (seconds)
    """
    colors = [
        ("Red", 255, 0, 0),
        ("Green", 0, 255, 0),
        ("Blue", 0, 0, 255),
    ]

    steps = 50  # Number of fade steps
    delay = duration_per_color / steps

    for color_name, r_max, g_max, b_max in colors:
        logger.info(f"Fading {color_name}...")

        # Fade in
        for step in range(steps + 1):
            progress = step / steps
            # Use sine wave for smoother fade
            brightness = math.sin(progress * math.pi / 2)

            red = int(r_max * brightness)
            green = int(g_max * brightness)
            blue = int(b_max * brightness)
            intensity = int(100 * brightness)

            set_led_color(lin_dev, nad, red, green, blue, intensity)
            time.sleep(delay)

        # Fade out
        for step in range(steps, -1, -1):
            progress = step / steps
            brightness = math.sin(progress * math.pi / 2)

            red = int(r_max * brightness)
            green = int(g_max * brightness)
            blue = int(b_max * brightness)
            intensity = int(100 * brightness)

            set_led_color(lin_dev, nad, red, green, blue, intensity)
            time.sleep(delay)


def main():
    parser = argparse.ArgumentParser(description='LIN LED Control Test')
    parser.add_argument('--host', default=MUM_HOST,
                       help=f'MUM IP address (default: {MUM_HOST})')
    parser.add_argument('--nad', type=lambda x: int(x,0), default=LED_DEFAULT_NAD,
                       help=f'Node address to control (default: 0x{LED_DEFAULT_NAD:02X})')
    parser.add_argument('--cycles', type=int, default=3,
                       help='Number of fade cycles (default: 3)')
    parser.add_argument('--duration', type=float, default=3.0,
                       help='Duration per color in seconds (default: 3.0)')

    args = parser.parse_args()

    try:
        logger.info(f"Connecting to MUM at {args.host}...")

        # Setup MUM and LIN
        mum = MelexisUniversalMaster()
        mum.open_all(args.host)

        power_control = mum.get_device(MUM_POWER_DEVICE)
        linmaster = mum.get_device(MUM_LIN_DEVICE)

        linmaster.setup()

        lin_bus = LinBusManager(linmaster)
        lin_dev = LinDevice22(lin_bus)
        lin_dev.baudrate = LIN_BAUDRATE
        lin_dev.nad = args.nad

        power_control.power_up()
        time.sleep(0.5)

        logger.info("MUM connected and LIN bus ready")
        logger.info("=" * 70)

        # Read current NAD
        logger.info("Reading current NAD from ALM_Status...")
        current_nad, status_data = read_alm_status(lin_dev)

        if current_nad is not None:
            data_hex = ' '.join(f'{b:02X}' for b in status_data)
            logger.info(f"Current NAD: 0x{current_nad:02X}")
            logger.info(f"Full status data: {data_hex}")
        else:
            logger.warning("Could not read NAD, using command-line NAD")
            current_nad = args.nad

        logger.info("=" * 70)
        logger.info(f"LED FADE TEST")
        logger.info(f"Controlling NAD: 0x{current_nad:02X}")
        logger.info(f"LIDFrom: 0x{current_nad:02X}, LIDTo: 0x{current_nad:02X}")
        logger.info(f"Fade cycles: {args.cycles}")
        logger.info(f"Duration per color: {args.duration}s")
        logger.info("=" * 70)

        # Turn LED off initially
        logger.info("Turning LED off...")
        set_led_color(lin_dev, current_nad, 0, 0, 0, 0)
        time.sleep(1.0)

        # Run fade test
        for cycle in range(1, args.cycles + 1):
            logger.info(f"\nCycle {cycle}/{args.cycles}")
            fade_test(lin_dev, current_nad, args.duration)

            if cycle < args.cycles:
                logger.info("Pausing between cycles...")
                time.sleep(1.0)

        # Turn LED off at the end
        logger.info("\nTurning LED off...")
        set_led_color(lin_dev, current_nad, 0, 0, 0, 0)

        logger.info("=" * 70)
        logger.info("✓ LED TEST COMPLETED")
        logger.info("=" * 70)

        logger.info("Tearing down...")
        linmaster.teardown()
        logger.info("Done (ECU still powered)")

    except KeyboardInterrupt:
        logger.info("")
        logger.info("Interrupted by user")
        logger.info("Turning LED off...")
        try:
            set_led_color(lin_dev, args.nad, 0, 0, 0, 0)
            linmaster.teardown()
        except:
            pass
    except Exception as e:
        logger.error(f"Error: {e}", exc_info=True)


if __name__ == "__main__":
    main()
