use std::collections::VecDeque;
use std::time::{Duration, Instant};

use crate::bus::{CanBus, CanFrame, CanId};
use crate::error::{Error, Result};
use crate::protocol::{ControlMode, DEFAULT_MOTOR_ID};
use crate::protocols::robstride::{
    ROBSTRIDE_CONFIG_BASE_ID, ROBSTRIDE_MIT_BASE_ID, ROBSTRIDE_POSITION_BASE_ID,
    ROBSTRIDE_SPEED_BASE_ID, ROBSTRIDE_STATUS_BASE_ID,
};

pub const CAN_TRACE_CSV_HEADER: &str = "sequence,elapsed_us,frame_format,id,len,data_hex";

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CanFrameClass {
    CubeMarsMit { motor_id: u8 },
    CubeMarsMitHelper { motor_id: u8 },
    CubeMarsDirect { mode: ControlMode, motor_id: u8 },
    RobStrideMit { motor_id: u8 },
    RobStridePosition { motor_id: u8 },
    RobStrideSpeed { motor_id: u8 },
    RobStrideStatus { motor_id: u8 },
    RobStrideConfig { motor_id: u8 },
    Standard { id: u16 },
    Extended { id: u32 },
}

impl CanFrameClass {
    pub fn classify(frame: &CanFrame) -> Self {
        match frame.id() {
            CanId::Standard(id) => Self::classify_standard(id),
            CanId::Extended(id) => Self::classify_extended(id, frame.len()),
        }
    }

    pub fn label(self) -> String {
        match self {
            Self::CubeMarsMit { motor_id } => format!("CubeMars MIT motor=0x{motor_id:02X}"),
            Self::CubeMarsMitHelper { motor_id } => {
                format!("CubeMars helper motor=0x{motor_id:02X}")
            }
            Self::CubeMarsDirect { mode, motor_id } => {
                format!("CubeMars {:?} motor=0x{motor_id:02X}", mode)
            }
            Self::RobStrideMit { motor_id } => format!("RobStride MIT motor=0x{motor_id:02X}"),
            Self::RobStridePosition { motor_id } => {
                format!("RobStride position motor=0x{motor_id:02X}")
            }
            Self::RobStrideSpeed { motor_id } => {
                format!("RobStride speed motor=0x{motor_id:02X}")
            }
            Self::RobStrideStatus { motor_id } => {
                format!("RobStride status motor=0x{motor_id:02X}")
            }
            Self::RobStrideConfig { motor_id } => {
                format!("RobStride config motor=0x{motor_id:02X}")
            }
            Self::Standard { id } => format!("standard 0x{id:03X}"),
            Self::Extended { id } => format!("extended 0x{id:08X}"),
        }
    }

    const fn classify_standard(id: u16) -> Self {
        if id <= u8::MAX as u16 {
            Self::CubeMarsMit { motor_id: id as u8 }
        } else {
            Self::Standard { id }
        }
    }

    const fn classify_extended(id: u32, len: usize) -> Self {
        let high = (id >> 8) & 0xFF;
        let low = id & 0xFF;

        if len == 8 {
            if id >= ROBSTRIDE_MIT_BASE_ID && id <= ROBSTRIDE_MIT_BASE_ID + u8::MAX as u32 {
                return Self::RobStrideMit {
                    motor_id: (id - ROBSTRIDE_MIT_BASE_ID) as u8,
                };
            }
            if id >= ROBSTRIDE_POSITION_BASE_ID && id <= ROBSTRIDE_POSITION_BASE_ID + u8::MAX as u32
            {
                return Self::RobStridePosition {
                    motor_id: (id - ROBSTRIDE_POSITION_BASE_ID) as u8,
                };
            }
            if id >= ROBSTRIDE_SPEED_BASE_ID && id <= ROBSTRIDE_SPEED_BASE_ID + u8::MAX as u32 {
                return Self::RobStrideSpeed {
                    motor_id: (id - ROBSTRIDE_SPEED_BASE_ID) as u8,
                };
            }
            if id >= ROBSTRIDE_STATUS_BASE_ID && id <= ROBSTRIDE_STATUS_BASE_ID + u8::MAX as u32 {
                return Self::RobStrideStatus {
                    motor_id: (id - ROBSTRIDE_STATUS_BASE_ID) as u8,
                };
            }
            if id >= ROBSTRIDE_CONFIG_BASE_ID && id <= ROBSTRIDE_CONFIG_BASE_ID + u8::MAX as u32 {
                return Self::RobStrideConfig {
                    motor_id: (id - ROBSTRIDE_CONFIG_BASE_ID) as u8,
                };
            }
        }

        if high == ControlMode::DutyCycle as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::DutyCycle,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::CurrentLoop as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::CurrentLoop,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::CurrentBrake as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::CurrentBrake,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::VelocityLoop as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::VelocityLoop,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::PositionLoop as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::PositionLoop,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::SetOrigin as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::SetOrigin,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::PositionVelocity as u32 {
            return Self::CubeMarsDirect {
                mode: ControlMode::PositionVelocity,
                motor_id: low as u8,
            };
        }
        if high == ControlMode::MitMode as u32 {
            return Self::CubeMarsMit {
                motor_id: low as u8,
            };
        }

        if id <= u8::MAX as u32 {
            return Self::CubeMarsMitHelper { motor_id: id as u8 };
        }

        Self::Extended { id }
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct FormattedCanFrame {
    pub id: String,
    pub data: String,
    pub ascii: String,
    pub len: usize,
    pub class: String,
}

impl FormattedCanFrame {
    pub fn from_frame(frame: &CanFrame) -> Self {
        let id = match frame.id() {
            CanId::Standard(id) => format!("{:03X}", id),
            CanId::Extended(id) => format!("{:08X}", id),
        };
        let data = frame
            .data()
            .iter()
            .map(|byte| format!("{byte:02X}"))
            .collect::<Vec<_>>()
            .join(" ");
        let ascii = frame
            .data()
            .iter()
            .map(|byte| {
                if byte.is_ascii_graphic() || *byte == b' ' {
                    char::from(*byte)
                } else {
                    '.'
                }
            })
            .collect();

        Self {
            id,
            data,
            ascii,
            len: frame.len(),
            class: CanFrameClass::classify(frame).label(),
        }
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct CanFrameRecord {
    pub sequence: u64,
    pub elapsed: Duration,
    pub frame: CanFrame,
    pub class: CanFrameClass,
}

impl CanFrameRecord {
    pub fn formatted(&self) -> FormattedCanFrame {
        FormattedCanFrame::from_frame(&self.frame)
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct CanTraceRecord {
    pub sequence: u64,
    pub elapsed: Duration,
    pub frame: CanFrame,
}

impl CanTraceRecord {
    pub fn from_record(record: &CanFrameRecord) -> Self {
        Self {
            sequence: record.sequence,
            elapsed: record.elapsed,
            frame: record.frame.clone(),
        }
    }

    pub fn formatted(&self) -> FormattedCanFrame {
        FormattedCanFrame::from_frame(&self.frame)
    }

    pub fn to_csv_row(&self) -> String {
        let (frame_format, id) = match self.frame.id() {
            CanId::Standard(id) => ("S", format!("{id:03X}")),
            CanId::Extended(id) => ("E", format!("{id:08X}")),
        };

        format!(
            "{},{},{},{},{},{}",
            self.sequence,
            duration_micros(self.elapsed),
            frame_format,
            id,
            self.frame.len(),
            data_hex(self.frame.data())
        )
    }
}

#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct CanMonitorStats {
    pub frames_seen: u64,
    pub frames_retained: usize,
    pub frames_dropped: u64,
    pub standard_frames: u64,
    pub extended_frames: u64,
    pub bytes_seen: u64,
}

#[derive(Debug, Clone)]
pub struct CanMonitor {
    capacity: usize,
    started_at: Instant,
    next_sequence: u64,
    records: VecDeque<CanFrameRecord>,
    stats: CanMonitorStats,
}

impl CanMonitor {
    pub fn new(capacity: usize) -> Self {
        Self {
            capacity: capacity.max(1),
            started_at: Instant::now(),
            next_sequence: 0,
            records: VecDeque::with_capacity(capacity.max(1)),
            stats: CanMonitorStats::default(),
        }
    }

    pub const fn capacity(&self) -> usize {
        self.capacity
    }

    pub fn records(&self) -> &VecDeque<CanFrameRecord> {
        &self.records
    }

    pub const fn stats(&self) -> CanMonitorStats {
        self.stats
    }

    pub fn clear(&mut self) {
        self.records.clear();
        self.stats = CanMonitorStats::default();
        self.next_sequence = 0;
        self.started_at = Instant::now();
    }

    pub fn observe(&mut self, frame: CanFrame) -> &CanFrameRecord {
        let elapsed = self.started_at.elapsed();
        let class = CanFrameClass::classify(&frame);

        if self.records.len() == self.capacity {
            self.records.pop_front();
            self.stats.frames_dropped = self.stats.frames_dropped.saturating_add(1);
        }

        self.stats.frames_seen = self.stats.frames_seen.saturating_add(1);
        self.stats.frames_retained = self.records.len() + 1;
        self.stats.bytes_seen = self.stats.bytes_seen.saturating_add(frame.len() as u64);
        match frame.id() {
            CanId::Standard(_) => {
                self.stats.standard_frames = self.stats.standard_frames.saturating_add(1);
            }
            CanId::Extended(_) => {
                self.stats.extended_frames = self.stats.extended_frames.saturating_add(1);
            }
        }

        self.records.push_back(CanFrameRecord {
            sequence: self.next_sequence,
            elapsed,
            frame,
            class,
        });
        self.next_sequence = self.next_sequence.wrapping_add(1);
        self.records.back().expect("record was just pushed")
    }

    pub fn poll_once<B>(
        &mut self,
        bus: &mut B,
        timeout: Duration,
    ) -> Result<Option<&CanFrameRecord>>
    where
        B: CanBus,
    {
        match bus.receive(timeout)? {
            Some(frame) => Ok(Some(self.observe(frame))),
            None => Ok(None),
        }
    }

    pub fn poll_available<B>(
        &mut self,
        bus: &mut B,
        timeout: Duration,
        max_frames: usize,
    ) -> Result<usize>
    where
        B: CanBus,
    {
        let mut received = 0;

        for _ in 0..max_frames {
            let timeout = if received == 0 {
                timeout
            } else {
                Duration::ZERO
            };

            if self.poll_once(bus, timeout)?.is_some() {
                received += 1;
            } else {
                break;
            }
        }

        Ok(received)
    }

    pub fn rows(&self) -> Vec<String> {
        self.records
            .iter()
            .rev()
            .map(|record| {
                let formatted = record.formatted();
                format!(
                    "#{:<5} {:>8.3}s {:<8} len={} {:<23} {}",
                    record.sequence,
                    record.elapsed.as_secs_f64(),
                    formatted.id,
                    formatted.len,
                    formatted.data,
                    record.class.label()
                )
            })
            .collect()
    }

    pub fn trace_records(&self) -> Vec<CanTraceRecord> {
        self.records
            .iter()
            .map(CanTraceRecord::from_record)
            .collect()
    }

    pub fn to_trace_csv(&self) -> String {
        let mut csv = String::from(CAN_TRACE_CSV_HEADER);
        csv.push('\n');

        for record in &self.records {
            csv.push_str(&CanTraceRecord::from_record(record).to_csv_row());
            csv.push('\n');
        }

        csv
    }
}

impl Default for CanMonitor {
    fn default() -> Self {
        Self::new(256)
    }
}

pub fn sample_can_monitor() -> Result<CanMonitor> {
    let mut monitor = CanMonitor::new(32);
    monitor.observe(CanFrame::new(
        ControlMode::MitMode.extended_id(DEFAULT_MOTOR_ID)?,
        &[0x80, 0x00, 0x7F, 0xF0, 0x00, 0x00, 0x00, 0x00],
    )?);
    monitor.observe(CanFrame::new_padded(
        CanId::extended(ROBSTRIDE_STATUS_BASE_ID + 1)?,
        &[0xE2, 0x04, 0x00, 0x00, 0x10, 0x00, 0x2A, 0x00],
    )?);
    monitor.observe(CanFrame::new(CanId::standard(0x123)?, b"kcan")?);
    Ok(monitor)
}

pub fn parse_can_trace_csv(input: &str) -> Result<Vec<CanTraceRecord>> {
    let mut records = Vec::new();

    for (index, raw_line) in input.lines().enumerate() {
        let line_number = index + 1;
        let line = raw_line.trim();

        if line.is_empty() || line == CAN_TRACE_CSV_HEADER {
            continue;
        }

        let fields = line.split(',').map(str::trim).collect::<Vec<_>>();
        if fields.len() != 6 {
            return Err(trace_error(
                line_number,
                format!("expected 6 columns, got {}", fields.len()),
            ));
        }

        let sequence = parse_u64_field(fields[0], line_number, "sequence")?;
        let elapsed_us = parse_u64_field(fields[1], line_number, "elapsed_us")?;
        let id = parse_can_trace_id(fields[2], fields[3], line_number)?;
        let len = parse_usize_field(fields[4], line_number, "len")?;
        let data = parse_trace_data(fields[5], len, line_number)?;
        let frame = CanFrame::new(id, &data)?;

        records.push(CanTraceRecord {
            sequence,
            elapsed: Duration::from_micros(elapsed_us),
            frame,
        });
    }

    Ok(records)
}

pub fn replay_can_trace<B>(bus: &mut B, trace: &[CanTraceRecord]) -> Result<usize>
where
    B: CanBus,
{
    for record in trace {
        bus.send(&record.frame)?;
    }

    Ok(trace.len())
}

fn duration_micros(duration: Duration) -> u64 {
    let micros = duration.as_micros();
    if micros > u64::MAX as u128 {
        u64::MAX
    } else {
        micros as u64
    }
}

fn data_hex(data: &[u8]) -> String {
    data.iter()
        .map(|byte| format!("{byte:02X}"))
        .collect::<Vec<_>>()
        .join("")
}

fn parse_can_trace_id(frame_format: &str, id: &str, line_number: usize) -> Result<CanId> {
    let raw_id = parse_hex_u32_field(id, line_number, "id")?;

    match frame_format {
        "S" | "s" => {
            if raw_id > u16::MAX as u32 {
                return Err(trace_error(
                    line_number,
                    format!("standard id 0x{raw_id:X} exceeds 11-bit range"),
                ));
            }

            CanId::standard(raw_id as u16).map_err(|err| match err {
                Error::InvalidCanId(_) => trace_error(
                    line_number,
                    format!("standard id 0x{raw_id:X} exceeds 11-bit range"),
                ),
                other => other,
            })
        }
        "E" | "e" => CanId::extended(raw_id).map_err(|err| match err {
            Error::InvalidCanId(_) => trace_error(
                line_number,
                format!("extended id 0x{raw_id:X} exceeds 29-bit range"),
            ),
            other => other,
        }),
        other => Err(trace_error(
            line_number,
            format!("frame_format must be S or E, got {other:?}"),
        )),
    }
}

fn parse_trace_data(data_hex: &str, len: usize, line_number: usize) -> Result<Vec<u8>> {
    if len > crate::bus::MAX_CAN_DATA_LEN {
        return Err(trace_error(
            line_number,
            format!("len {len} exceeds {} bytes", crate::bus::MAX_CAN_DATA_LEN),
        ));
    }

    if data_hex.len() != len * 2 {
        return Err(trace_error(
            line_number,
            format!(
                "data_hex length {} does not match len {len}",
                data_hex.len()
            ),
        ));
    }

    let mut data = Vec::with_capacity(len);
    for index in 0..len {
        let start = index * 2;
        let end = start + 2;
        let byte = u8::from_str_radix(&data_hex[start..end], 16).map_err(|err| {
            trace_error(
                line_number,
                format!("invalid data byte {:?}: {err}", &data_hex[start..end]),
            )
        })?;
        data.push(byte);
    }

    Ok(data)
}

fn parse_u64_field(value: &str, line_number: usize, field: &str) -> Result<u64> {
    value.parse::<u64>().map_err(|err| {
        trace_error(
            line_number,
            format!("invalid {field} value {value:?}: {err}"),
        )
    })
}

fn parse_usize_field(value: &str, line_number: usize, field: &str) -> Result<usize> {
    value.parse::<usize>().map_err(|err| {
        trace_error(
            line_number,
            format!("invalid {field} value {value:?}: {err}"),
        )
    })
}

fn parse_hex_u32_field(value: &str, line_number: usize, field: &str) -> Result<u32> {
    let value = value
        .strip_prefix("0x")
        .or_else(|| value.strip_prefix("0X"))
        .unwrap_or(value);

    u32::from_str_radix(value, 16).map_err(|err| {
        trace_error(
            line_number,
            format!("invalid {field} hex value {value:?}: {err}"),
        )
    })
}

fn trace_error(line: usize, reason: String) -> Error {
    Error::InvalidTraceLine { line, reason }
}
