use std::cell::RefCell;
use std::rc::Rc;
use std::thread;
use std::time::Duration;

use command::{
    CommandGroup, CommandGroupConfig, CommandInfo, CommandRef, Scheduler as CommandScheduler,
};
use drive::{Drive, DriveRef};

mod cmds {
    use self::CommandResult::*;
    use command::CommandResult;
    use drive::DriveRef;

    pub struct DriveCommandConfig {
        pub drive: DriveRef,
    }

    pub struct DriveCommandState {
        iterations: u64,
    }

    impl Default for DriveCommandState {
        fn default() -> Self {
            DriveCommandState { iterations: 0 }
        }
    }

    command_impl! (
        DriveCommand<DriveCommandConfig, DriveCommandState>;
        self, ctx => {
            *ctx.state() = DriveCommandState::default();
        }; {
            let state = ctx.state();
            self.drive.borrow_mut().drive_powers(1.0, 1.0);
            state.iterations += 1;
            if state.iterations >= 10 {
                Done
            } else {
                Continue
            }
        }; {
            self.drive.borrow_mut().drive_powers(0.0, 0.0);
        }
    );

    pub struct LogCommandConfig;

    command_impl! {
        LogCmd<LogCommandConfig>; self => { info!("{}", "LogCmd") }
    }

    use command::Command;
    use std::cell::RefCell;
    use std::rc::Weak;
    pub struct StartCommandConfig(pub Weak<RefCell<Command>>);

    command_impl! {
        StartCommand<StartCommandConfig>; self, ctx => {
            let command = if let Some(command) = self.0.upgrade() {command} else {
                warn!("StartCommand({}) has undefined weak reference to command", ctx.name());
                return;
            };
            ctx.parent().start_command(command);
        }
    }
}

pub struct Robot {
    drive: DriveRef,
    command_scheduler: CommandScheduler,
}

const ITER_PERIOD: Duration = Duration::from_nanos(1_000_000_000 / 50);

impl Robot {
    pub fn new() -> Self {
        Robot {
            drive: Drive::new_ref(),
            command_scheduler: CommandScheduler::new(),
        }
    }

    pub fn run(mut self) {
        warn!("Starting RobotRS");

        let command = cmds::DriveCommand::new_ref(
            CommandInfo::new(0, "DriveCommand"),
            cmds::DriveCommandConfig {
                drive: self.drive.clone(),
            },
        );
        let log_command =
            cmds::LogCmd::new_ref(CommandInfo::new(2, "LogCommand"), cmds::LogCommandConfig);
        let start_command: Rc<_> = cmds::StartCommand::new_ref(
            CommandInfo::new(3, "StartCommand"),
            cmds::StartCommandConfig(std::rc::Weak::<RefCell<CommandGroup>>::new()),
        );
        let command_group = CommandGroup::new_ref(
            CommandInfo::new(1, "Group"),
            CommandGroupConfig::new(vec![command, log_command, start_command.clone()]),
        );
        start_command.borrow_mut().imp.0 = Rc::downgrade(&command_group) as std::rc::Weak<RefCell<dyn crate::command::Command>>;

        self.command_scheduler.start_command(command_group);

        loop {
            self.execute();
            thread::sleep(ITER_PERIOD);
        }
    }

    fn execute(&mut self) {
        self.command_scheduler.execute();
    }
}