use socketcan::CANFrame;

use canstream::*;

use std::collections::{HashMap, VecDeque};

use std::cmp::min;

const DEFAULT_BLOCKS: u8 = 16;

struct IncomingPacket {
    id: u16,
    remaining_bytes: u16,
    content: Vec<u8>,
    block_size: u8,
    blocks_until: u8,
    next_counter: u8,
}

impl IncomingPacket {
    fn new(frame: &CANFrame) -> IncomingPacket {
        let data: &[u8] = frame.data();
        let mut datavec = Vec::new();

        for v in 2..8 {
            datavec.push(data[v]);
        }

        IncomingPacket {
            id: frame.id() as u16,
            remaining_bytes: ((((data[0] as u16) & 0x0f) << 8) | (data[1] as u16)) - 6,
            content: datavec,
            block_size: DEFAULT_BLOCKS,
            blocks_until: 0,
            next_counter: 1,
        }
    }

    fn process_more(&mut self, frame: &CANFrame) -> Result<bool, ()> {
        let data: &[u8] = frame.data();
        if (data[0] & 0x0f) != self.next_counter {
            return Err(());
        }
        let provided = min(7, self.remaining_bytes);
        for v in 1..(provided + 1) {
            self.content.push(data[v as usize]);
        }
        self.remaining_bytes -= provided;
        if self.block_size != 0 {
            self.blocks_until -= 1;
        }
        self.next_counter = (self.next_counter + 1) & 0xF;

        Ok(self.block_size != 0 && self.blocks_until == 0)
    }
}

pub struct ISOTP {
    incoming: HashMap<u16, IncomingPacket>,
    outgoing: VecDeque<CANFrame>,
}

impl ISOTP {
    pub fn new() -> ISOTP {
        ISOTP {
            incoming: HashMap::new(),
            outgoing: VecDeque::new(),
        }
    }

    pub fn get_outgoing(&mut self) -> Option<CANFrame> {
        self.outgoing.pop_front()
    }

    pub fn handle_frame(&mut self, frame: &CANFrame) -> Option<(u16, Vec<u8>)> {
        let id = frame.id() as u16;
        let data = frame.data();
        match data[0] >> 4 {
            0 => {
                println!("SingleFrame");
                // Received a SingleFrame frame, return a Vec of the
                // requisite number of bytes...
                let len = (data[0] & 0xf) as usize;
                if len < 1 || len > 7 {
                    return None;
                }
                let sliced = &data[1..len + 1];
                return Some((frame.id() as u16, Vec::from(sliced)));
            }
            1 => {
                println!("FirstFrame");
                // Received a FirstFrame frame, we need to cancel any
                // ongoing receive for this ID
                if self.incoming.contains_key(&id) {
                    println!("Removed partial");
                    self.incoming.remove(&id);
                }
                // Now we need to construct an incoming packet
                let mut incoming = IncomingPacket::new(frame);
                // And we need to queue a flow control for it.
                self.queue_flow(&mut incoming);
                // We insert this into the hash
                self.incoming.insert(id, incoming);
                // Finally we return None since we've not completed reception
                return None;
            }
            2 => {
                println!("ConsecutiveFrame");
                // Received a ConsecutiveFrame frame, we need to find the
                // incoming which we want, and manipulate it with the incoming
                // frame and hopefully we either succeed in completing packet
                // or we have to send a flow.  Failing both of those, we have
                // to abort the incoming packet.
                let mut ret = None;
                let mut do_flow = false;
                let mut do_delete = false;
                if let Some(packet) = self.incoming.get_mut(&id) {
                    println!("Found packet");
                    if let Ok(flow) = packet.process_more(frame) {
                        println!("Processed a frame, no error");
                        if packet.remaining_bytes == 0 {
                            // We have received the packet fully
                            ret = Some((id, packet.content.clone()));
                            do_delete = true;
                        } else if flow {
                            // We probably need to send a flow packet
                            do_flow = true;
                        } else {
                            // We're just processing, return None for now
                            return None;
                        }
                    } else {
                        println!("Error processing frame");
                        do_delete = true;
                    }
                } else {
                    println!("Didn't find a packet");
                }
                if do_flow {
                    let mut packet = self.incoming.remove(&id).unwrap();
                    self.queue_flow(&mut packet);
                    self.incoming.insert(id, packet);
                }
                if do_delete {
                    self.incoming.remove(&id);
                }
                return ret;
            }
            3 => {
                println!("Oh gods, a flow control frame!");
            }
            _ => {}
        }
        None
    }

    fn queue_flow(&mut self, packet: &mut IncomingPacket) {
        // We queue a flow control packet here
        let frame = CANFrame::new(
            (packet.id as u32) - 8,
            &[0x30, packet.block_size, 0x00],
            false,
            false,
        ).unwrap();
        self.outgoing.push_back(frame);
        packet.blocks_until = packet.block_size;
    }
}