# Gate

// Gate / garage door controller.
//
// Control modes (config control_mode):
//   0 - Direct motor control: open/close outputs are level signals driving the motor
//   1 - Single control output, controller cycles open/stop/close/stop on each pulse (3F)
//   2 - Single control output, controller toggles open/close on each pulse (no stop)
//   3 - Separate pulse outputs for open and close; the controller function behind each
//       output is configured with open_output_function / close_output_function:
//       0 - Open (Ot), 1 - Close (ZA), 2 - Open/Close (OZ), 3 - Open/Stop/Close (3F),
//       4 - Partial open (U), 5 - Partial open/Close (PC)
//
// gate_state output values:
//   0 - closed, 1 - opening, 2 - open, 3 - closing, 4 - stopped midway, 5 - partially open
//
// desired_action state values:
//   0 - none, 1 - open, 2 - close, 3 - stop,
//   4 - partial open through a controller channel (dedicated partial pulse, or a
//       close pulse on a closed gate for the Partial open/Close function),
//   5 - raw control pulse (hardware toggle pass-through)
//
// Position is simulated from opening/closing durations on a 100ms tick and corrected
// by the is_open / is_closed limit switch inputs when they are connected.
//
// motor_lock_duration is the time the motor needs to reverse direction. In direct mode it
// keeps the motor off for that time (motor protection); in the non-direct modes the
// controller owns the motor, so the lock instead holds the simulated position for the
// reversal window so the position estimate accounts for the time a reversal takes.
//
// Movement is only believed for as long as the gate shows signs of moving. With limit
// switches connected, a movement that never clears the switch it started from, and a
// simulated travel that ends without the far switch confirming it, are both given up on
// after movement_timeout and the state is resynchronised with the switches. Without that
// a controller refusing a pulse, a stalled motor or a switch that merely flickered would
// leave the block believing the gate moves for good, and every further command would be
// discarded as already satisfied.

channel = input::channel()
value = input::value()

fn ticks_for(seconds) {
    t = math::round((seconds * 1000) / 100)
    if (t < 1) {
        t = 1
    }
    return t
}

// Position margin (percent) within which a limit-switch re-assert right after the
// gate departs that limit is treated as contact bounce rather than a real arrival.
// Outside this margin the sensor is always trusted, so remote / external movement
// that drives the gate back to a limit is honored.
fn sensor_bounce_margin() {
    return 5
}

// Counts the ticks in which the gate showed no sign of moving and reports when
// movement_timeout has passed without any progress.
fn no_progress_timeout() {
    t = state::get("stall_ticks") + 1
    if (t >= ticks_for(config::get("movement_timeout"))) {
        state::set("stall_ticks", 0)
        return true
    }
    state::set("stall_ticks", t)
    return false
}

fn set_gate_state(s) {
    output::set("gate_state", s)
}

fn apply_stopped_state() {
    if (state::get("partial_pending")) {
        state::set("partial_pending", false)
        set_gate_state(5)
        return
    }
    pos = state::get("position")
    if (pos >= 100) {
        set_gate_state(2)
    } else if (pos <= 0) {
        set_gate_state(0)
    } else {
        set_gate_state(4)
    }
}

fn set_dir(d) {
    prev_dir = state::get("last_dir")
    state::set("moving_dir", d)
    state::set("stall_ticks", 0)
    state::set("external_move", false)
    output::set("moving", d != 0)
    if (d != 0) {
        // A direction reversal costs the motor lock time regardless of the control mode.
        // In direct mode the motor is physically held off for that time (handled in
        // process_desired). In the non-direct modes the controller owns the motor, so the
        // pulse is sent right away, but the gate still needs that time to reverse - hold the
        // simulated position for the remaining lock window so the estimate stays accurate.
        if (config::get("control_mode") != 0 and prev_dir != d) {
            lock_ms = math::round(config::get("motor_lock_duration") * 1000)
            since = time::uptime() - state::get("last_movement_ts")
            if (since < lock_ms) {
                lt = math::round((lock_ms - since) / 100)
                if (lt > 0) {
                    state::set("move_lock_ticks", lt)
                }
            }
        }
        state::set("last_dir", d)
        state::set("last_movement_ts", time::uptime())
        if (d == 1) {
            set_gate_state(1)
        } else {
            set_gate_state(3)
        }
    } else {
        state::set("move_lock_ticks", 0)
    }
}

fn direct_apply(d) {
    output::set("open", d == 1)
    output::set("close", d == -1)
}

fn pulse_output_name(out_id) {
    if (out_id == 1) {
        return "control"
    }
    if (out_id == 2) {
        return "open"
    }
    return "close"
}

fn cycle_state_name(out_id) {
    if (out_id == 1) {
        return "cycle_next"
    }
    if (out_id == 2) {
        return "cycle_next_op"
    }
    return "cycle_next_cl"
}

// Returns the pulse output (2=open, 3=close) whose controller function matches f,
// or 0 when none is configured. Used in separate pulses mode only.
fn channel_with_function(f) {
    if (config::get("open_output_function") == f) {
        return 2
    }
    if (config::get("close_output_function") == f) {
        return 3
    }
    return 0
}

// Mirror what the external controller does when it receives a pulse
// on a channel configured with the given function
fn apply_channel_function(out_id, f) {
    if (f == 0) { // open
        set_dir(1)
        return
    }
    if (f == 1) { // close
        set_dir(-1)
        return
    }
    if (f == 2) { // open/close toggle
        d = state::get("moving_dir")
        if (d != 0) {
            set_dir(0 - d)
        } else if (state::get("last_dir") == 1) {
            set_dir(-1)
        } else {
            set_dir(1)
        }
        return
    }
    if (f == 3) { // open/stop/close/stop cycle, tracked per channel
        a = state::get(cycle_state_name(out_id))
        nxt = a + 1
        if (nxt > 3) {
            nxt = 0
        }
        state::set(cycle_state_name(out_id), nxt)
        if (a == 0) {
            set_dir(1)
        } else if (a == 2) {
            set_dir(-1)
        } else {
            set_dir(0)
            apply_stopped_state()
        }
        return
    }
    if (f == 4) { // partial open - controller moves the gate to its partial position
        ppos = config::get("partial_open_position")
        pos = state::get("position")
        if (pos < ppos) {
            state::set("target", ppos)
            state::set("partial_pending", true)
            set_dir(1)
        } else if (pos > ppos) {
            state::set("target", ppos)
            state::set("partial_pending", true)
            set_dir(-1)
        } else {
            set_gate_state(5)
        }
        return
    }
    // f == 5: partial open / close
    if (state::get("position") <= 0) {
        // controller interprets a pulse on a closed gate as partial open
        state::set("target", config::get("partial_open_position"))
        state::set("partial_pending", true)
        set_dir(1)
    } else {
        set_dir(-1)
    }
}

fn apply_pulse_semantics(out_id) {
    mode = config::get("control_mode")
    if (mode == 1) { // single control output with open/stop/close cycle
        apply_channel_function(1, 3)
        return
    }
    if (mode == 2) { // single control output with open/close toggle
        apply_channel_function(1, 2)
        return
    }
    // separate pulses - behaviour depends on the configured channel function
    f = config::get("open_output_function")
    if (out_id == 3) {
        f = config::get("close_output_function")
    }
    apply_channel_function(out_id, f)
}

fn emit_pulse(out_id) {
    output::set(pulse_output_name(out_id), true)
    state::set("pulse_out", out_id)
    state::set("pulse_phase", 1)
    state::set("pulse_ticks", ticks_for(config::get("pulse_duration")))
    apply_pulse_semantics(out_id)
}

fn process_desired() {
    desired = state::get("desired_action")
    if (desired == 0) {
        return
    }
    mode = config::get("control_mode")
    d = state::get("moving_dir")

    if (desired == 5) { // hardware toggle pass-through
        state::set("desired_action", 0)
        if (mode == 1 or mode == 2) {
            emit_pulse(1)
            return
        }
        ch = channel_with_function(3)
        if (ch == 0) {
            ch = channel_with_function(2)
        }
        if (ch != 0) {
            emit_pulse(ch)
        }
        return
    }
    if (desired == 4) { // partial open through a controller channel
        state::set("desired_action", 0)
        ch = channel_with_function(4)
        if (ch == 0) {
            ch = channel_with_function(5)
        }
        if (ch != 0) {
            emit_pulse(ch)
        }
        return
    }

    // already satisfied?
    if (desired == 1 and (d == 1 or state::get("position") >= 100)) {
        state::set("desired_action", 0)
        return
    }
    if (desired == 2 and (d == -1 or state::get("position") <= 0)) {
        state::set("desired_action", 0)
        return
    }
    if (desired == 3 and d == 0) {
        state::set("desired_action", 0)
        return
    }

    // safety sensors
    if (desired == 1 and input::get("prevent_open")) {
        state::set("desired_action", 0)
        return
    }
    if (desired == 2 and input::get("prevent_close")) {
        state::set("desired_action", 0)
        return
    }

    if (mode == 0) { // direct motor control
        if (desired == 3) {
            set_dir(0)
            direct_apply(0)
            apply_stopped_state()
            state::set("desired_action", 0)
            return
        }

        new_dir = 1
        if (desired == 2) {
            new_dir = -1
        }

        if (d != 0) { // direction change - stop the motor first
            set_dir(0)
            direct_apply(0)
            apply_stopped_state()
        }

        lock_ms = math::round(config::get("motor_lock_duration") * 1000)
        since = time::uptime() - state::get("last_movement_ts")
        if (since < lock_ms and state::get("last_dir") != new_dir) {
            // wait for the motor lock to release, then retry (desired_action is kept)
            lock_ticks = math::round((lock_ms - since) / 100)
            if (lock_ticks < 1) {
                lock_ticks = 1
            }
            state::set("pulse_phase", 2)
            state::set("pulse_ticks", lock_ticks)
            return
        }

        set_dir(new_dir)
        direct_apply(new_dir)
        state::set("desired_action", 0)
        return
    }

    if (mode == 1) {
        // keep pulsing through the controller cycle until the desired action is applied
        emit_pulse(1)
        return
    }

    if (mode == 2) { // open/close toggle - stop is not available
        if (desired == 3) {
            state::set("desired_action", 0)
            return
        }
        emit_pulse(1)
        return
    }

    // separate pulses - pick a channel that can perform the desired action
    if (desired == 3) {
        ch = channel_with_function(3) // only an open/stop/close channel can stop
        if (ch == 0) {
            state::set("desired_action", 0)
            return
        }
        emit_pulse(ch)
        return
    }
    if (desired == 1) {
        ch = channel_with_function(0)
        if (ch == 0) {
            ch = channel_with_function(3)
        }
        if (ch == 0) {
            ch = channel_with_function(2)
        }
        if (ch == 0) {
            state::set("desired_action", 0)
            return
        }
        emit_pulse(ch)
        return
    }
    ch = channel_with_function(1)
    if (ch == 0) {
        ch = channel_with_function(3)
    }
    if (ch == 0) {
        ch = channel_with_function(2)
    }
    if (ch == 0) {
        ch = channel_with_function(5)
    }
    if (ch == 0) {
        state::set("desired_action", 0)
        return
    }
    emit_pulse(ch)
}

fn ensure_tick() {
    if (state::get("tick_active")) {
        return
    }
    if (state::get("pulse_phase") == 0 and state::get("moving_dir") == 0 and state::get("desired_action") == 0) {
        return
    }
    state::set("tick_active", true)
    callback::set(99, "onTick") // 99 instead of 100 to leave room for callback scheduling
}

fn command(action) {
    if (action != 4) { // any new command cancels a pending partial-open target
        state::set("target", -1)
        state::set("partial_pending", false)
        state::set("target_stop_needed", false)
        state::set("partial_after_close", false)
    }
    state::set("desired_action", action)
    if (state::get("pulse_phase") == 0) {
        process_desired()
    }
    ensure_tick()
}

fn reach_open_end() {
    state::set("position", 100.0)
    output::set("position", 100)
    set_dir(0)
    // the gate sits at the open limit, so opening was the last travel that happened,
    // whatever direction the block believed in before
    state::set("last_dir", 1)
    if (config::get("control_mode") == 0) {
        direct_apply(0)
    }
    // limit reached - a cycling controller continues with the closing direction
    state::set("cycle_next", 2)
    state::set("cycle_next_op", 2)
    state::set("cycle_next_cl", 2)
    state::set("target", -1)
    state::set("partial_pending", false)
    state::set("target_stop_needed", false)
    state::set("partial_after_close", false)
    set_gate_state(2)
}

fn reach_closed_end() {
    state::set("position", 0.0)
    output::set("position", 0)
    set_dir(0)
    state::set("last_dir", -1)
    if (config::get("control_mode") == 0) {
        direct_apply(0)
    }
    // limit reached - a cycling controller continues with the opening direction
    state::set("cycle_next", 0)
    state::set("cycle_next_op", 0)
    state::set("cycle_next_cl", 0)
    state::set("target", -1)
    state::set("partial_pending", false)
    state::set("target_stop_needed", false)
    set_gate_state(0)
    // if a two-phase partial open was waiting for the gate to be fully closed,
    // now issue the partial open pulse (PC channel interprets it as partial open)
    if (state::get("partial_after_close")) {
        state::set("partial_after_close", false)
        command(4)
    }
}

extern fn onTick() {
    // pulse / pause / motor lock phases
    phase = state::get("pulse_phase")
    if (phase == 1) {
        t = state::get("pulse_ticks") - 1
        if (t <= 0) {
            output::set(pulse_output_name(state::get("pulse_out")), false)
            state::set("pulse_out", 0)
            state::set("pulse_phase", 2)
            state::set("pulse_ticks", ticks_for(config::get("pulse_pause")))
        } else {
            state::set("pulse_ticks", t)
        }
    } else if (phase == 2) {
        t = state::get("pulse_ticks") - 1
        if (t <= 0) {
            state::set("pulse_phase", 0)
            state::set("pulse_ticks", 0)
            process_desired()
        } else {
            state::set("pulse_ticks", t)
        }
    }

    // movement simulation; position is frozen while a stop pulse is pending
    // (the gate is about to be stopped, waiting only for the pulse machinery)
    d = state::get("moving_dir")
    // reversal dead-time: in the non-direct modes the position is held while the motor
    // lock is still elapsing, because the gate cannot have changed direction yet
    move_lock = state::get("move_lock_ticks")
    // in sensor mode, position does not advance while the departure sensor is still
    // asserted: the gate may not have physically left the limit yet (e.g. power
    // failure, stalled motor) and the sensor is the authoritative source. That wait is
    // bounded by movement_timeout below, or a gate that never departs would keep the
    // block believing it moves forever.
    sensor_holds_pos = false
    if (config::get("use_sensors")) {
        if (d == 1 and input::get("is_closed")) {
            sensor_holds_pos = true
        }
        if (d == -1 and input::get("is_open")) {
            sensor_holds_pos = true
        }
    }
    if (move_lock > 0 and d != 0) {
        state::set("move_lock_ticks", move_lock - 1)
    } else if (d != 0 and state::get("desired_action") != 3 and sensor_holds_pos) {
        // The gate is still on the limit switch it was told to leave. Give it
        // movement_timeout to get going, then take it that the movement never started
        // and park the gate back on that limit, so the state stops claiming a movement
        // that is not happening and the next command is not discarded as already
        // satisfied. If the gate does depart later, the released switch picks the
        // movement up again.
        if (no_progress_timeout()) {
            if (d == 1) {
                reach_closed_end()
            } else {
                reach_open_end()
            }
        }
    } else if (d != 0 and state::get("desired_action") != 3) {
        pos = state::get("position")
        if (d == 1) {
            pos = pos + (100 * 100) / (config::get("opening_duration") * 1000.0)
        } else {
            pos = pos - (100 * 100) / (config::get("closing_duration") * 1000.0)
        }

        if (d == 1 and pos >= 100) {
            if (config::get("use_sensors")) {
                // clamp for display; actual end-of-travel is confirmed by the is_open
                // sensor, or taken for granted once movement_timeout passes without it -
                // the sensor may not be wired on this side, or the gate may have stopped
                // just short of it
                state::set("position", 100.0)
                output::set("position", 100)
                state::set("last_movement_ts", time::uptime())
                if (no_progress_timeout()) {
                    reach_open_end()
                }
            } else {
                reach_open_end()
            }
        } else if (d == -1 and pos <= 0) {
            if (config::get("use_sensors")) {
                // clamp for display; actual end-of-travel is confirmed by the is_closed
                // sensor, or taken for granted once movement_timeout passes without it
                state::set("position", 0.0)
                output::set("position", 0)
                state::set("last_movement_ts", time::uptime())
                if (no_progress_timeout()) {
                    reach_closed_end()
                }
            } else {
                reach_closed_end()
            }
        } else {
            target = state::get("target")
            arrived = false
            if (target >= 0) {
                if (d == 1 and pos >= target) {
                    arrived = true
                }
                if (d == -1 and pos <= target) {
                    arrived = true
                }
            }

            if (arrived) {
                pos = target
            }
            state::set("position", pos)
            output::set("position", math::round(pos))
            state::set("last_movement_ts", time::uptime())
            state::set("stall_ticks", 0)

            if (arrived) {
                state::set("target", -1)
                mode = config::get("control_mode")
                stop_needed = state::get("target_stop_needed")
                state::set("target_stop_needed", false)
                if (mode == 1 or (mode == 3 and stop_needed)) {
                    // stop through a controller pulse;
                    // the partial state is applied together with the stop pulse
                    state::set("desired_action", 3)
                    if (state::get("pulse_phase") == 0) {
                        process_desired()
                    }
                } else {
                    // direct stop, or the controller stops on its own (partial open mode)
                    set_dir(0)
                    if (mode == 0) {
                        direct_apply(0)
                    }
                    apply_stopped_state()
                }
            }
        }
    }

    // keep ticking while there is anything to do
    if (state::get("pulse_phase") != 0 or state::get("moving_dir") != 0 or state::get("desired_action") != 0) {
        callback::set(99, "onTick")
    } else {
        state::set("tick_active", false)
    }
}

// ---- limit switches ----

if (channel == "is_open") {
    if (value) {
        // The sensor is authoritative (e.g. a remote may have driven the gate here),
        // so honor it - except for contact bounce in the instant after the gate leaves
        // the open limit: that shows up as a re-assert while closing and still within a
        // small position margin of the open limit. A re-assert during a movement that was
        // only assumed from this switch opening is never bounce - it means the gate never
        // left the limit, so that assumed movement has to go.
        bounce = state::get("moving_dir") == -1 and state::get("position") > 100 - sensor_bounce_margin()
        if (!bounce or state::get("external_move")) {
            reach_open_end()
        }
    } else if (state::get("moving_dir") == 0 and state::get("position") >= 100) {
        // gate left the open limit without a command - assume external closing
        set_dir(-1)
        state::set("external_move", true)
        ensure_tick()
    }
    return
}

if (channel == "is_closed") {
    if (value) {
        // The sensor is authoritative (e.g. a remote may have driven the gate here),
        // so honor it - except for contact bounce in the instant after the gate leaves
        // the closed limit: that shows up as a re-assert while opening and still within a
        // small position margin of the closed limit. A re-assert during a movement that
        // was only assumed from this switch opening is never bounce - it means the gate
        // never left the limit, so that assumed movement has to go.
        bounce = state::get("moving_dir") == 1 and state::get("position") < sensor_bounce_margin()
        if (!bounce or state::get("external_move")) {
            reach_closed_end()
        }
    } else if (state::get("moving_dir") == 0 and state::get("position") <= 0) {
        // gate left the closed limit without a command - assume external opening
        set_dir(1)
        state::set("external_move", true)
        ensure_tick()
    }
    return
}

// ---- safety inputs ----

if (channel == "prevent_open") {
    if (value and state::get("moving_dir") == 1) {
        command(3)
    }
    return
}

if (channel == "prevent_close") {
    if (value and state::get("moving_dir") == -1) {
        command(3)
    }
    return
}

if (channel == "off") {
    if (value) {
        command(3)
    }
    return
}

// ---- command inputs - rising edge only ----

if (value == false) {
    return
}

if (input::get("off")) {
    return
}

if (channel == "open") {
    if (input::get("prevent_open")) {
        return
    }
    command(1)
    return
}

if (channel == "close") {
    if (input::get("prevent_close")) {
        return
    }
    command(2)
    return
}

if (channel == "stop") {
    command(3)
    return
}

if (channel == "toggle") {
    mode = config::get("control_mode")
    if (mode == 1 or mode == 2) {
        // forward directly as a hardware control pulse - the controller owns the cycle
        command(5)
        return
    }
    if (mode == 3) {
        if (channel_with_function(3) != 0 or channel_with_function(2) != 0) {
            // a cycling controller channel owns the open/stop/close sequence
            command(5)
            return
        }
        // no cycling channel - reverse while moving, otherwise alternate direction
        d = state::get("moving_dir")
        towards_close = false
        if (d == 1) {
            towards_close = true
        }
        if (d == 0 and state::get("last_dir") == 1) {
            towards_close = true
        }
        if (towards_close) {
            if (input::get("prevent_close")) {
                return
            }
            command(2)
        } else {
            if (input::get("prevent_open")) {
                return
            }
            command(1)
        }
        return
    }
    d = state::get("moving_dir")
    if (d != 0) {
        command(3)
    } else if (state::get("last_dir") == 1) {
        if (input::get("prevent_close")) {
            return
        }
        command(2)
    } else {
        if (input::get("prevent_open")) {
            return
        }
        command(1)
    }
    return
}

if (channel == "partial_open") {
    mode = config::get("control_mode")
    pos = state::get("position")
    ppos = config::get("partial_open_position")

    if (mode == 2) {
        // open/close toggle controller cannot stop mid-travel
        return
    }

    if (mode == 3) {
        if (math::abs(pos - ppos) < 1) {
            set_gate_state(5)
            return
        }
        if (channel_with_function(4) != 0) {
            // dedicated partial open channel - the controller handles it
            command(4)
            return
        }
        if (channel_with_function(5) != 0) {
            if (pos <= 0 or input::get("is_closed")) {
                // at closed position (by estimate or sensor);
                // correct position estimate if sensor disagrees with it
                if (pos > 0 and input::get("is_closed")) {
                    state::set("position", 0.0)
                    output::set("position", 0)
                }
                // the PC pulse on a closed gate is interpreted as partial open
                command(4)
            } else {
            // close fully first; reach_closed_end will fire command(4) automatically
            if (input::get("prevent_close")) {
                return
            }
            command(2) // clears all flags, so partial_after_close must be set afterwards
            state::set("partial_after_close", true)
            }
            return
        }
        if (channel_with_function(3) != 0) {
            // stop at position using the open/stop/close channel
            if (pos < ppos) {
                if (input::get("prevent_open")) {
                    return
                }
                state::set("desired_action", 1)
            } else {
                if (input::get("prevent_close")) {
                    return
                }
                state::set("desired_action", 2)
            }
            state::set("target", ppos)
            state::set("partial_pending", true)
            state::set("target_stop_needed", true)
            if (state::get("pulse_phase") == 0) {
                process_desired()
            }
            ensure_tick()
        }
        return
    }

    if (math::abs(pos - ppos) < 1) {
        set_gate_state(5)
        return
    }

    if (pos < ppos) {
        if (input::get("prevent_open")) {
            return
        }
        state::set("desired_action", 1)
    } else {
        if (input::get("prevent_close")) {
            return
        }
        state::set("desired_action", 2)
    }
    state::set("target", ppos)
    state::set("partial_pending", true)
    if (state::get("pulse_phase") == 0) {
        process_desired()
    }
    ensure_tick()
    return
}