use crate::util::*;

const CYCLE_MS: i32 = 5000;

#[derive(Debug, Default)]
pub struct LogicInputs {
    pub left_limit: bool,
    pub right_limit: bool,
    pub pressure_switch: bool,

    pub switch_run_continuous: bool,
    pub switch_run_once: bool,

    pub rio_online: bool,
}

#[derive(Debug, Default)]
pub struct LogicOutputs {
    pub cylinder_left: bool,
    pub cylinder_right: bool,

    pub signal_operate: bool,
    pub signal_ready: bool,
    pub signal_fault: bool,
    pub signal_alarm: bool,

    pub display: [String; 4],
}

#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)]
enum MachineState {
    #[default]
    Stopped,
    Idle,
    ExecuteContinuous,
    ExecuteOnce,
    Aborted,
    Completed,
}

impl MachineState {
    pub fn short_name(&self) -> &'static str {
        match self {
            MachineState::Stopped => "STOP",
            MachineState::Idle => "IDLE",
            MachineState::ExecuteContinuous => "RUN CONT",
            MachineState::ExecuteOnce => "RUN ONCE",
            MachineState::Aborted => "FAULT",
            MachineState::Completed => "DONE",
        }
    }
}

#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)]
enum CylinderState {
    #[default]
    Unknown,
    Left,
    Right,
}

#[derive(Debug, Default)]
pub struct Logic {
    inp: LogicInputs,
    out: LogicOutputs,

    counter: usize,
    running: bool,
    faulted: bool,
    pressure_ok: bool,
    cylinder_state: CylinderState,
    state: MachineState,

    t_alarm: PulseTimer,
    t_state: BaseTimer<MachineState>,
    t_cylinder: BaseTimer<CylinderState>,
}

impl Logic {
    pub fn new() -> Self {
        Self::default()
    }

    pub fn inputs_mut(&mut self) -> &mut LogicInputs {
        &mut self.inp
    }

    pub fn outputs(&self) -> &LogicOutputs {
        &self.out
    }
}

impl Logic {
    pub fn run(&mut self, now: profirust::time::Instant) {
        self.out.signal_fault = matches!(self.state, MachineState::Stopped | MachineState::Aborted);
        self.out.signal_ready = matches!(self.state, MachineState::Idle | MachineState::Completed);
        self.out.signal_operate = matches!(
            self.state,
            MachineState::ExecuteContinuous | MachineState::ExecuteOnce
        );
        self.out.signal_alarm = self
            .t_alarm
            .run(
                now,
                matches!(self.state, MachineState::Aborted | MachineState::Completed),
                500.millis(),
            )
            .done;

        self.t_state.run(now, self.state);
        self.state = match self.state {
            MachineState::Stopped if !self.inp.pressure_switch => MachineState::Stopped,
            _ if !self.inp.pressure_switch => MachineState::Aborted,

            // Reset aborted state when no run mode is requested
            MachineState::Aborted
                if !self.inp.switch_run_once && !self.inp.switch_run_continuous =>
            {
                MachineState::Stopped
            }

            // Go to idle state once pressure is okay
            MachineState::Stopped
                if self.inp.pressure_switch
                    && !self.inp.switch_run_once
                    && !self.inp.switch_run_continuous =>
            {
                log::info!("Machine ready to start.");
                MachineState::Idle
            }

            // Start from idle state when one of the modes is requested (with 2 seconds delay)
            MachineState::Idle
                if self.t_state.timer(now, 2.secs()) && self.inp.switch_run_continuous =>
            {
                log::info!("Starting continuous test...");
                MachineState::ExecuteContinuous
            }
            MachineState::Idle if self.t_state.timer(now, 2.secs()) && self.inp.switch_run_once => {
                log::info!("Starting single test...");
                MachineState::ExecuteOnce
            }

            // End condition
            MachineState::ExecuteContinuous
                if self.inp.left_limit || self.inp.right_limit =>
            {
                log::info!("Completed continuous test. Failure after {} cycles.", self.counter);
                MachineState::Completed
            }
            MachineState::ExecuteOnce
                if self.inp.left_limit || self.inp.right_limit =>
            {
                log::info!("Completed single test. Failure after {}.", self.t_cylinder.timer_value(now));
                MachineState::Completed
            }

            // End on user request
            MachineState::ExecuteContinuous if !self.inp.switch_run_continuous => {
                log::info!("Aborted continuous test by user request (after {} cycles).", self.counter);
                MachineState::Idle
            }
            MachineState::ExecuteOnce
                if self.t_state.timer(now, 2.secs()) && self.inp.switch_run_once =>
            {
                log::info!("Aborted single test by user request (after {}).", self.t_cylinder.timer_value(now));
                MachineState::Idle
            }

            // Reset condition
            MachineState::Completed
                if !self.inp.switch_run_continuous && !self.inp.switch_run_once =>
            {
                MachineState::Idle
            }

            // Stay in current state if when no transition is active
            state => state,
        };

        if !matches!(self.state, MachineState::ExecuteContinuous) {
            self.counter = 0;
        }

        // Cylinder state update
        self.t_cylinder.run(now, self.cylinder_state);
        match self.state {
            MachineState::ExecuteContinuous => {
                self.cylinder_state = match self.cylinder_state {
                    CylinderState::Unknown => CylinderState::Left,
                    CylinderState::Left if self.t_cylinder.timer(now, CYCLE_MS.millis()) => {
                        self.counter += 1;
                        log::debug!("Cycle counter: {}", self.counter);
                        CylinderState::Right
                    }
                    CylinderState::Right if self.t_cylinder.timer(now, CYCLE_MS.millis()) => {
                        self.counter += 1;
                        log::debug!("Cycle counter: {}", self.counter);
                        CylinderState::Left
                    }
                    s => s,
                };
            }
            MachineState::ExecuteOnce => {
                self.cylinder_state = match self.cylinder_state {
                    CylinderState::Unknown => CylinderState::Left,
                    CylinderState::Left if self.t_cylinder.timer(now, 1.secs()) => {
                        log::debug!("Completed preparation, starting test run...");
                        CylinderState::Right
                    }
                    s => s,
                }
            }
            _ => {
                self.cylinder_state = CylinderState::Unknown;
            }
        }

        // Only move pneumatic cylinder in execute states
        if matches!(
            self.state,
            MachineState::ExecuteOnce | MachineState::ExecuteContinuous
        ) {
            self.out.cylinder_left = self.cylinder_state == CylinderState::Left;
            self.out.cylinder_right = self.cylinder_state == CylinderState::Right;
        } else {
            self.out.cylinder_left = false;
            self.out.cylinder_right = false;
        }

        // Display update
        self.out.display[0] = format!("{:^20}", "torque tester");
        let msg = match self.state {
            MachineState::Aborted | MachineState::Stopped | MachineState::Completed
                if self.inp.switch_run_once =>
            {
                "turn off run once"
            }
            MachineState::Aborted | MachineState::Stopped | MachineState::Completed
                if self.inp.switch_run_continuous =>
            {
                "turn off run cont."
            }
            MachineState::Stopped if !self.inp.pressure_switch => "no pressure",
            MachineState::Idle => "ready to start",
            _ => "",
        };
        self.out.display[1] = format!("{}: {}", self.state.short_name(), msg);

        match self.state {
            MachineState::ExecuteContinuous => {
                self.out.display[3] = format!("Cycles: {:6}", self.counter);
            }
            MachineState::ExecuteOnce => {
                if self.cylinder_state == CylinderState::Right {
                    self.out.display[3] = format!("Time: {}", self.t_cylinder.timer_value(now));
                } else {
                    self.out.display[3] = format!("Preparing...");
                }
            }
            // Keep last message visible
            _ => (),
        }
    }
}
