extern crate cursive;
extern crate drs_0x01;
extern crate serialport;
#[macro_use]
extern crate clap;

mod cli;
mod ui;

use cursive::CursiveExt;

use cli::{parse_args, Message};
use ui::init;

use drs_0x01::addr::{ReadableRamAddr, WritableEEPAddr};
use drs_0x01::builder::HerkulexMessage;
use drs_0x01::reader::{ACKReader, Command};
use drs_0x01::{Rotation, Servo};

use serialport::{open_with_settings, SerialPort, SerialPortSettings};

use std::io;
use std::io::Read;
use std::{thread, time};

fn show_send_message<S: Into<String>>(what: S, message: &HerkulexMessage) {
    println!("{}", what.into());
    println!("Sending : {:#x?}", message);
}

fn main() {
    include_str!("../Cargo.toml");
    let app = parse_args();
    let matches = app.get_matches();

    if matches.is_present("gui") {
        let mut id = None;
        if let Some(val) = matches.value_of("id") {
            // We can unwrap here because we provided a validator to clap
            id = Some(val.parse::<u8>().unwrap());
        }
        let mut ui = init(id);
        ui.run();
    } else if matches.is_present("completions") {
        let sub_matches = matches.subcommand_matches("completions").unwrap();
        let shell = sub_matches.value_of("SHELL").unwrap();
        parse_args().gen_completions_to(
            "herkulex_communicator",
            shell.parse().unwrap(),
            &mut io::stdout(),
        );
    } else if matches.is_present("message") {
        let matches = matches.subcommand_matches("message").unwrap();
        // We can unwrap here because all those flags are mandatory
        let mut connection = {
            let mut r = SerialPortSettings::default();
            r.baud_rate = 115_200;
            // We can unwrap here because we verified that the connection was valid during argument parsing.
            open_with_settings(matches.value_of("connection").unwrap(), &r).unwrap()
        };
        // The id is verified during argument parsing so it is safe to unwrap
        let id = matches.value_of("id").unwrap().parse::<u8>().unwrap();
        let message = value_t_or_exit!(matches, "command", Message);
        match message {
            Message::SetPosition => {
                // Same as id : we can unwrap
                let pos = matches
                    .value_of("desired_position")
                    .unwrap()
                    .parse::<u16>()
                    .unwrap();
                let message = Servo::new(id).set_position(pos);
                show_send_message(
                    format!(
                        "Asking the servomotor {} to got to the absolute position :{}.",
                        id, pos
                    ),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::SetSpeed => {
                // Same as id : we can unwrap
                let speed = matches
                    .value_of("desired_speed")
                    .unwrap()
                    .parse::<u16>()
                    .unwrap();
                let message = Servo::new(id).set_speed(speed, Rotation::Clockwise);
                show_send_message(
                    format!(
                        "Asking the servomotor {} to set its speed to : {}.",
                        id, speed
                    ),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::Reboot => {
                let message = Servo::new(id).reboot();
                show_send_message(format!("Asking the servomotor {} to reboot.", id), &message);
                send_message(&mut connection, message);
            }
            Message::EnableTorque => {
                let message = Servo::new(id).enable_torque();
                show_send_message(
                    format!("Asking the servomotor {} to enable its torque.", id),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::DisableTorque => {
                let message = Servo::new(id).disable_torque();
                show_send_message(
                    format!("Asking the servomotor {} to disable its torque.", id),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::ClearErrors => {
                let message = Servo::new(id).clear_errors();
                show_send_message(
                    format!("Asking the servomotor {} to clear its error register.", id),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::SetId => {
                let new_id = matches.value_of("set_id").unwrap().parse::<u8>().unwrap();
                let message = Servo::new(id).eep_write(WritableEEPAddr::ID(new_id));
                show_send_message(
                    format!(
                        "Asking the servomotor {} to change his id to : {}.",
                        id, new_id
                    ),
                    &message,
                );
                send_message(&mut connection, message);
            }
            Message::ForceSetId => {
                let new_id = matches.value_of("set_id").unwrap().parse::<u8>().unwrap();
                for i in 0..=253 {
                    println!(
                        "Asking the servomotor {} to change his id to : {}.",
                        i, new_id
                    );
                    let message = Servo::new(i).eep_write(WritableEEPAddr::ID(new_id));
                    send_message(&mut connection, message);
                }
            }

            Message::GetId => {
                let message = Servo::new(id).ram_request(ReadableRamAddr::ID);
                send_message(&mut connection, message);
                let mut reader = ACKReader::new();
                for b in connection.bytes() {
                    if let Ok(b) = b {
                        reader.parse(&[b]);
                        if let Some(p) = reader.pop_ack_packet() {
                            if let Command::RamRead { data } = p.cmd {
                                if let ReadableRamAddr::ID = data.addr {
                                    println!("The ID of the servomotor is {}", data.data[0]);
                                }
                            }
                        }
                    } else {
                        break;
                    }
                }
            }
            Message::GetPosition => {
                let message = Servo::new(id).ram_request(ReadableRamAddr::AbsolutePosition);
                send_message(&mut connection, message);
                let mut reader = ACKReader::new();
                for b in connection.bytes() {
                    if let Ok(b) = b {
                        reader.parse(&[b]);
                        if let Some(p) = reader.pop_ack_packet() {
                            if let Command::RamRead { data } = p.cmd {
                                if let ReadableRamAddr::AbsolutePosition = data.addr {
                                    if data.data_len == 2 {
                                        println!(
                                            "The absolute position of the servomotor is {}",
                                            (data.data[0] as u32 + ((data.data[1] as u32) << 8))
                                        );
                                    }
                                }
                            }
                        }
                    } else {
                        break;
                    }
                }
            }

            Message::GetTorque => {
                let message = Servo::new(id).ram_request(ReadableRamAddr::PWM);
                send_message(&mut connection, message);
                let mut reader = ACKReader::new();
                for b in connection.bytes() {
                    if let Ok(b) = b {
                        reader.parse(&[b]);
                        if let Some(p) = reader.pop_ack_packet() {
                            if let Command::RamRead { data } = p.cmd {
                                if let ReadableRamAddr::PWM = data.addr {
                                    if data.data_len == 2 {
                                        println!(
                                            "The absolute position of the servomotor is {}",
                                            (data.data[0] as u32 + ((data.data[1] as u32) << 8))
                                        );
                                    }
                                }
                            }
                        }
                    } else {
                        break;
                    }
                }
            }
        }
    }
}

fn send_message(connection: &mut Box<dyn SerialPort>, message: HerkulexMessage) {
    match connection.write(&message) {
        Ok(_) => {}
        Err(e) => panic!("Failed to send message : {}", e),
    }
    connection.flush().expect("Failed to flush");
    let ten_millis = time::Duration::from_millis(1);
    thread::sleep(ten_millis);
}
