# Motor axis

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

fn stop_motors() {
    callback::clear()
    if (output::get("forward") or output::get("reverse")) {
        state::set("was_forward", output::get("forward"))
        state::set("last_stop_time", time::uptime())
    }
    output::set("forward", false)
    output::set("reverse", false)
    output::set("moving", false)
    output::set("rpm", 0)
    state::set("last_encoder_pulse_time", 0)
}

fn start_forward() {
    output::set("forward", true)
    output::set("reverse", false)
    output::set("moving", true)
}

fn start_reverse() {
    output::set("forward", false)
    output::set("reverse", true)
    output::set("moving", true)
}

fn set_position(pos) {
    output::set("position", pos)
    full = config::get("full_travel_steps")
    if (full > 0) {
        pct = pos * 100.0 / full
        if (pct < 0) {
            pct = 0
        }
        if (pct > 100) {
            pct = 100
        }
        output::set("position_pct", pct)
    }
}

fn set_rpm(pulses_per_second) {
    spr = config::get("steps_per_revolution")
    if (spr > 0) {
        output::set("rpm", pulses_per_second * 60.0 / spr)
    }
}

extern fn onStep(v) {
    start_time = state::get("move_start_time")
    start_pos = state::get("move_start_position")
    target = state::get("move_target")
    is_forward = output::get("forward")

    pulse_ms = 0
    if (is_forward) {
        pulse_ms = math::round(config::get("pulse_time_forward") * 1000)
    } else {
        pulse_ms = math::round(config::get("pulse_time_reverse") * 1000)
    }

    total_steps = target - start_pos
    if (total_steps < 0) {
        total_steps = 0 - total_steps
    }
    total_ms = total_steps * pulse_ms
    elapsed = time::uptime() - start_time

    if (elapsed >= total_ms) {
        set_position(target)
        stop_motors()
        output::set("at_target", true)
        return
    }

    steps_float = elapsed / pulse_ms
    steps_int = math::round(steps_float)
    if (steps_int * pulse_ms > elapsed) {
        steps_int = steps_int - 1
    }

    position = start_pos
    if (is_forward) {
        position = start_pos + steps_int
        if (position > target) {
            position = target
        }
    } else {
        position = start_pos - steps_int
        if (position < target) {
            position = target
        }
    }

    set_position(position)

    remaining_ms = total_ms - elapsed
    if (remaining_ms <= 100) {
        callback::set(math::round(remaining_ms), "onStep", 0)
    } else {
        callback::set(100, "onStep", 0)
    }
}

extern fn onFreqFinish(v) {
    target = state::get("move_target")
    set_position(target)
    stop_motors()
    output::set("at_target", true)
}

fn init_freq_state() {
    state::set("last_freq_time", time::uptime())
    state::set("avg_pulse_time_ms", 0)
    state::set("freq_sample_count", 0)
    state::set("freq_finishing", false)
    state::set("fractional_pulses", 0)
}

fn evaluate_target_now(target, position) {
    tolerance = config::get("position_tolerance")
    diff = target - position

    control_mode = config::get("control_mode")

    if (diff > tolerance) {
        start_forward()
        output::set("at_target", false)
        if (control_mode == 1) {
            state::set("move_start_time", time::uptime())
            state::set("move_start_position", position)
            state::set("move_target", target)
            steps = math::round(diff)
            pulse_ms = math::round(config::get("pulse_time_forward") * 1000)
            set_rpm(1000.0 / pulse_ms)
            total_ms = steps * pulse_ms
            first_cb = 100
            if (total_ms < first_cb) {
                first_cb = total_ms
            }
            callback::set(first_cb, "onStep", 0)
        }
        if (control_mode == 2) {
            state::set("move_target", target)
            init_freq_state()
            pulse_ms = math::round(config::get("pulse_time_forward") * 1000)
            state::set("avg_pulse_time_ms", pulse_ms * 1.0)
            total_ms = math::round(diff) * pulse_ms
            callback::set(math::round(total_ms), "onFreqFinish", 0)
        }
    } else if (diff < (0 - tolerance)) {
        start_reverse()
        output::set("at_target", false)
        if (control_mode == 1) {
            state::set("move_start_time", time::uptime())
            state::set("move_start_position", position)
            state::set("move_target", target)
            steps = math::round(0 - diff)
            pulse_ms = math::round(config::get("pulse_time_reverse") * 1000)
            set_rpm(1000.0 / pulse_ms)
            total_ms = steps * pulse_ms
            first_cb = 100
            if (total_ms < first_cb) {
                first_cb = total_ms
            }
            callback::set(first_cb, "onStep", 0)
        }
        if (control_mode == 2) {
            state::set("move_target", target)
            init_freq_state()
            pulse_ms = math::round(config::get("pulse_time_reverse") * 1000)
            state::set("avg_pulse_time_ms", pulse_ms * 1.0)
            total_ms = math::round(0 - diff) * pulse_ms
            callback::set(math::round(total_ms), "onFreqFinish", 0)
        }
    } else {
        stop_motors()
        output::set("at_target", true)
    }
}

extern fn onMotorLock(v) {
    evaluate_target_now(state::get("lock_target"), state::get("lock_position"))
}

fn evaluate_target(target, position) {
    callback::clear()
    lock_ms = math::round(config::get("motor_lock_duration") * 1000)

    if (lock_ms <= 0) {
        evaluate_target_now(target, position)
        return
    }

    tolerance = config::get("position_tolerance")
    diff = target - position
    going_forward = diff > tolerance
    going_reverse = diff < (0 - tolerance)

    if (!going_forward and !going_reverse) {
        evaluate_target_now(target, position)
        return
    }

    direction_changed = false
    if (going_forward and !state::get("was_forward")) {
        direction_changed = true
    }
    if (going_reverse and state::get("was_forward")) {
        direction_changed = true
    }

    if (!direction_changed) {
        evaluate_target_now(target, position)
        return
    }

    elapsed_ms = time::uptime() - state::get("last_stop_time")
    if (elapsed_ms >= lock_ms) {
        evaluate_target_now(target, position)
        return
    }

    remaining_ms = lock_ms - elapsed_ms
    output::set("at_target", false)
    state::set("lock_target", target)
    state::set("lock_position", position)
    callback::set(math::round(remaining_ms), "onMotorLock", 0)
}

fn complete_homing() {
    stop_motors()
    set_position(0)
    output::set("homed", true)
    state::set("is_homing", false)

    if (state::get("move_after_home")) {
        state::set("move_after_home", false)
        pending_target = state::get("pending_target")
        evaluate_target(pending_target, 0)
    } else {
        output::set("at_target", true)
    }
}

fn start_homing_sequence() {
    callback::clear()
    output::set("homed", false)
    output::set("at_target", false)
    state::set("is_homing", true)

    if (input::get("home_switch")) {
        complete_homing()
    } else {
        home_direction = config::get("home_direction") // 0=Forward, 1=Reverse
        if (home_direction == 0) {
            start_forward()
        } else {
            start_reverse()
        }
    }
}

// Handle "stop" input first (dominant input)
if (channel == "stop") {
    prev_stop = state::get("prev_stop")

    if (value and !prev_stop) {
        stop_motors()
        state::set("is_homing", false)
    }

    state::set("prev_stop", value)
    return
}

// Check home_switch on every execution during homing (level-triggered)
// This ensures homing completes even if the switch was already active
if (state::get("is_homing") and input::get("home_switch")) {
    complete_homing()
    return
}

// Handle "home_switch" input - stop motor when home switch activates outside homing
if (channel == "home_switch") {
    if (value and output::get("moving")) {
        stop_motors()
        set_position(0)
        output::set("homed", true)
        output::set("at_target", true)
    }
    return
}

// Handle "home" input
if (channel == "home") {
    prev_home = state::get("prev_home")

    if (value and !prev_home) {
        start_homing_sequence()
    }

    state::set("prev_home", value)
    return
}

// Handle "encoder" input
if (channel == "encoder") {
    if (config::get("control_mode") != 0) {
        return
    }

    prev_encoder = state::get("prev_encoder")

    if (value and !prev_encoder) {
        // Rising edge on encoder - count if motor is driving and not homing
        is_homing = state::get("is_homing")
        is_forward = output::get("forward")
        is_reverse = output::get("reverse")

        if (!is_homing and (is_forward or is_reverse)) {
            position = output::get("position")

            if (is_forward) {
                position = position + 1
            } else {
                position = position - 1
            }

            set_position(position)

            enc_now = time::uptime()
            last_enc = state::get("last_encoder_pulse_time")
            if (last_enc > 0) {
                enc_elapsed = enc_now - last_enc
                if (enc_elapsed > 0) {
                    set_rpm(1000.0 / enc_elapsed)
                }
            }
            state::set("last_encoder_pulse_time", enc_now)

            // Check if at target
            target = input::get("target")
            tolerance = config::get("position_tolerance")
            diff = target - position

            if (diff >= (0 - tolerance) and diff <= tolerance) {
                stop_motors()
                output::set("at_target", true)
            }
        }
    }

    state::set("prev_encoder", value)
    return
}

// Handle "freq" input (Frequency control mode)
if (channel == "freq") {
    if (config::get("control_mode") != 2) {
        return
    }
    if (state::get("is_homing")) {
        return
    }
    if (state::get("freq_finishing")) {
        return
    }

    is_forward = output::get("forward")
    is_reverse = output::get("reverse")
    if (!is_forward and !is_reverse) {
        return
    }

    callback::clear()

    now = time::uptime()
    last_time = state::get("last_freq_time")
    elapsed_ms = now - last_time
    state::set("last_freq_time", now)

    if (value <= 0 or elapsed_ms <= 0) {
        return
    }

    pulse_count = value * elapsed_ms / 1000.0 + state::get("fractional_pulses")
    whole_steps = math::round(pulse_count)
    if (pulse_count < whole_steps) {
        whole_steps = whole_steps - 1.0
    }
    state::set("fractional_pulses", pulse_count - whole_steps)

    pulse_time = 1000.0 / value
    set_rpm(value)
    sample_count = state::get("freq_sample_count") + 1
    state::set("freq_sample_count", sample_count)
    avg_pt = state::get("avg_pulse_time_ms")
    config_pt = config::get("pulse_time_forward") * 1000.0
    if (is_reverse) {
        config_pt = config::get("pulse_time_reverse") * 1000.0
    }
    if (pulse_time > config_pt / 2.0 and pulse_time < config_pt * 2.0) {
        avg_pt = avg_pt + (pulse_time - avg_pt) / (sample_count * 1.0)
        state::set("avg_pulse_time_ms", avg_pt)
    }

    position = output::get("position")
    target = state::get("move_target")

    if (is_forward) {
        position = position + whole_steps
        if (position >= target) {
            position = target
        }
    } else {
        position = position - whole_steps
        if (position <= target) {
            position = target
        }
    }
    set_position(position)

    remaining = target - position
    if (remaining < 0) {
        remaining = 0 - remaining
    }
    tolerance = config::get("position_tolerance")

    if (remaining <= tolerance) {
        stop_motors()
        output::set("at_target", true)
        return
    }

    remaining_time_ms = remaining * avg_pt
    callback::set(math::round(remaining_time_ms), "onFreqFinish", 0)
    if (remaining_time_ms < elapsed_ms) {
        state::set("freq_finishing", true)
    }

    return
}

// Handle "target" input
if (channel == "target") {
    is_homing = state::get("is_homing")
    home_before_move = input::get("home_before_move")

    if (config::get("control_mode") != 0 and output::get("moving")) {
        stop_motors()
        state::set("is_homing", false)
        is_homing = false
    }

    if (home_before_move) {
        if (is_homing) {
            state::set("pending_target", value)
            state::set("move_after_home", true)
            return
        }

        if (output::get("moving")) {
            stop_motors()
        }

        if (input::get("home_switch")) {
            set_position(0)
            output::set("homed", true)
            evaluate_target(value, 0)
        } else {
            state::set("pending_target", value)
            state::set("move_after_home", true)
            start_homing_sequence()
        }
    } else {
        homed = output::get("homed")
        if (homed and !is_homing) {
            position = output::get("position")
            evaluate_target(value, position)
        }
    }

    return
}