yaes/src/YaesuController.zig

const std = @import("std");

const lj = @import("./labjack.zig");
const Config = @import("./Config.zig");
const config = Config.global;

const log = std.log.scoped(.yaesu_controller);

const YaesuController = @This();

control_thread: std.Thread,
lock: *std.Thread.Mutex,
controller: *const Controller,

pub const AzEl = struct {
    azimuth: f64,
    elevation: f64,
};

pub const CalibrationRoutine = enum {
    feedback,
    orientation,
};

pub fn calibrate(allocator: std.mem.Allocator, routine: CalibrationRoutine) !void {
    const controller = try YaesuController.init(allocator);
    defer {
        controller.quit();
        controller.control_thread.join();
    }

    switch (routine) {
        .feedback => try controller.calibrate_feedback(),
        .orientation => try controller.calibrate_orientation(),
    }
}

pub fn init(allocator: std.mem.Allocator) !YaesuController {
    const controller = try allocator.create(Controller);
    errdefer allocator.destroy(controller);
    controller.* = try Controller.init(allocator);
    errdefer controller.deinit(allocator);
    // do this in the main thread so we can throw the error about it synchronously.
    try controller.connectLabjack();

    return .{
        .control_thread = try std.Thread.spawn(.{}, runController, .{controller}),
        .lock = &controller.lock,
        .controller = controller,
    };
}

fn inRange(request: f64, comptime dof: enum { azimuth, elevation }) bool {
    return switch (dof) {
        // zig fmt: off
        .azimuth => request >= (
                  config.labjack.feedback_calibration.azimuth.minimum.angle
                + config.controller.angle_offset.azimuth
            ) and request <= (
                  config.labjack.feedback_calibration.azimuth.maximum.angle
                + config.controller.angle_offset.azimuth
        ),
        .elevation => request >= (
                  config.labjack.feedback_calibration.elevation.minimum.angle
                + config.controller.angle_offset.elevation
            ) and request <= (
                  config.labjack.feedback_calibration.elevation.maximum.angle
                + config.controller.angle_offset.elevation
        ),
        // zig fmt: on
    };
}

pub fn setTarget(self: YaesuController, target: AzEl) error{OutOfRange}!void {
    self.lock.lock();
    defer self.lock.unlock();

    const masked_target: AzEl = .{
        .azimuth = target.azimuth,
        .elevation = @min(
            @max(target.elevation, config.controller.elevation_mask),
            180.0 - config.controller.elevation_mask,
        ),
    };

    if (!inRange(masked_target.azimuth, .azimuth) or !inRange(masked_target.elevation, .elevation))
        return error.OutOfRange;

    const controller = @constCast(self.controller);
    controller.target = masked_target;
    controller.requested_state = .running;
}

pub fn currentPosition(self: YaesuController) AzEl {
    self.lock.lock();
    defer self.lock.unlock();

    return self.controller.position;
}

pub fn waitForUpdate(self: YaesuController) AzEl {
    const controller = @constCast(self.controller);

    self.lock.lock();
    defer self.lock.unlock();

    controller.condition.wait(self.lock);

    return controller.position;
}

pub fn quit(self: YaesuController) void {
    self.lock.lock();
    defer self.lock.unlock();

    const controller = @constCast(self.controller);
    controller.requested_state = .stopped;
}

pub fn stop(self: YaesuController) void {
    self.lock.lock();
    defer self.lock.unlock();

    const controller = @constCast(self.controller);
    controller.target = controller.position;
    controller.requested_state = .idle;
}

pub fn startPark(self: YaesuController) void {
    self.setTarget(config.controller.parking_posture) catch unreachable;
}

fn calibrate_feedback(self: YaesuController) !void {
    _ = self;
    log.err("this isn't implemented yet, sorry.", .{});
    return error.NotImplemented;
}

fn calibrate_orientation(self: YaesuController) !void {
    _ = self;
    log.err("this isn't implemented yet, sorry.", .{});
    return error.NotImplemented;
}

fn runController(controller: *Controller) void {
    controller.run() catch {
        log.err(
            "the rotator control loop has terminated unexpectedly!!!!",
            .{},
        );
    };
}

const FeedbackBuffer = struct {
    samples: []f64,
    index: usize = 0,

    fn initZero(allocator: std.mem.Allocator, samples: usize) !FeedbackBuffer {
        const buf = try allocator.alloc(f64, samples * 2);
        @memset(buf, 0);
        return .{ .samples = buf };
    }

    fn deinit(self: FeedbackBuffer, allocator: std.mem.Allocator) void {
        allocator.free(self.samples);
    }

    fn push(self: *FeedbackBuffer, sample: [2]lj.AnalogReadResult) void {
        const halfpoint = @divExact(self.samples.len, 2);
        defer self.index = (self.index + 1) % halfpoint;

        self.samples[self.index] = sample[0].voltage;
        self.samples[self.index + halfpoint] = sample[1].voltage;
    }

    inline fn mean(data: []f64) f64 {
        var accum: f64 = 0;
        for (data) |pt| {
            accum += pt;
        }
        return accum / @as(f64, @floatFromInt(data.len));
    }

    fn lerp(input: f64, cal_points: Config.MinMax) f64 {
        return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
    }

    fn get(self: FeedbackBuffer) AzEl {
        const halfpoint = @divExact(self.samples.len, 2);

        return .{
            .azimuth = lerp(
                mean(self.samples[0..halfpoint]),
                config.labjack.feedback_calibration.azimuth,
            ) + config.controller.angle_offset.azimuth,
            .elevation = lerp(
                mean(self.samples[halfpoint..]),
                config.labjack.feedback_calibration.elevation,
            ) + config.controller.angle_offset.elevation,
        };
    }

    fn getRaw(self: FeedbackBuffer) AzEl {
        const halfpoint = @divExact(self.samples.len, 2);
        return .{
            .azimuth = mean(self.samples[0..halfpoint]),
            .elevation = mean(self.samples[halfpoint..]),
        };
    }
};

const Controller = struct {
    target: AzEl,
    position: AzEl,
    feedback_buffer: FeedbackBuffer,

    current_state: ControllerState,
    requested_state: ControllerState,

    labjack: lj.Labjack,

    lock: std.Thread.Mutex = .{},
    condition: std.Thread.Condition = .{},

    const ControllerState = enum {
        initializing,
        idle,
        calibration,
        running,
        stopped,
    };

    fn init(allocator: std.mem.Allocator) !Controller {
        return .{
            .target = .{ .azimuth = 0, .elevation = 0 },
            .position = .{ .azimuth = 0, .elevation = 0 },
            .feedback_buffer = try FeedbackBuffer.initZero(allocator, config.controller.feedback_window_samples),
            .current_state = .stopped,
            .requested_state = .idle,
            .labjack = switch (config.labjack.device) {
                .autodetect => lj.Labjack.autodetect(),
                .serial_number => |sn| lj.Labjack.with_serial_number(sn),
            },
        };
    }

    fn deinit(self: Controller, allocator: std.mem.Allocator) void {
        self.feedback_buffer.deinit(allocator);
    }

    fn connectLabjack(self: *Controller) !void {
        const info = try self.labjack.connect();
        try self.labjack.setAllDigitalOutputLow();
        self.labjack.id = info.local_id;
    }

    fn lerpOne(input: f64, cal_points: Config.MinMax) f64 {
        return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
    }

    fn lerpAndOffsetAngles(input: [2]lj.AnalogReadResult) AzEl {
        return .{
            .azimuth = lerpOne(
                input[0].voltage,
                config.labjack.feedback_calibration.azimuth,
            ) + config.controller.angle_offset.azimuth,
            .elevation = lerpOne(
                input[1].voltage,
                config.labjack.feedback_calibration.elevation,
            ) + config.controller.angle_offset.elevation,
        };
    }

    const Sign = enum {
        negative,
        zero,
        positive,

        pub fn symbol(self: Sign) u21 {
            return switch (self) {
                .negative => '-',
                .zero => '×',
                .positive => '+',
            };
        }
    };

    fn signDeadzone(offset: f64, deadzone: f64) Sign {
        return if (@abs(offset) < deadzone)
            .zero
        else if (offset < 0)
            .negative
        else
            .positive;
    }

    fn updateFeedback(self: *Controller) !void {
        const inputs = .{
            config.controller.azimuth_input,
            config.controller.elevation_input,
        };

        const raw = try self.labjack.readAnalogWriteDigital(
            2,
            inputs,
            null,
            true,
        );

        self.feedback_buffer.push(raw);
    }

    fn drive(self: *const Controller, pos_error: AzEl) !void {
        const azsign = signDeadzone(
            pos_error.azimuth,
            config.controller.angle_tolerance.azimuth,
        );

        const elsign = signDeadzone(
            pos_error.elevation,
            config.controller.angle_tolerance.elevation,
        );

        var drive_signal: [4]bool = .{false} ** 4;
        drive_signal[config.controller.azimuth_outputs.increase.io] = azsign == .positive;
        drive_signal[config.controller.azimuth_outputs.decrease.io] = azsign == .negative;
        drive_signal[config.controller.elevation_outputs.increase.io] = elsign == .positive;
        drive_signal[config.controller.elevation_outputs.decrease.io] = elsign == .negative;

        const raw = self.feedback_buffer.getRaw();

        log.info(
            // -180.1 is 6 chars. -5.20 is 5 chars
            "az: {d: >6.1}° ({d: >5.2} V) Δ {d: >6.1}° => {u}, el: {d: >6.1}° ({d: >5.2} V) Δ {d: >6.1}° => {u}",
            .{
                self.position.azimuth,
                raw.azimuth,
                pos_error.azimuth,
                azsign.symbol(),
                self.position.elevation,
                raw.elevation,
                pos_error.elevation,
                elsign.symbol(),
            },
        );

        try self.labjack.writeIoLines(drive_signal);
    }

    fn setPosition(self: *Controller, position: AzEl) void {
        self.position = position;
        self.condition.broadcast();
    }

    fn run(self: *Controller) !void {
        self.current_state = .initializing;

        var timer: LoopTimer = .{ .interval_ns = config.controller.loop_interval_ns };

        while (timer.mark()) : (timer.sleep()) {
            self.updateFeedback() catch {
                self.lock.lock();
                defer self.lock.unlock();

                self.current_state = .stopped;
                continue;
            };

            self.lock.lock();
            defer self.lock.unlock();

            self.setPosition(self.feedback_buffer.get());

            switch (self.current_state) {
                .initializing, .idle => {
                    self.current_state = self.requested_state;
                },
                .calibration => {
                    self.lock.lock();
                    defer self.lock.unlock();

                    // run calibration routine. psych, this does nothing. gottem
                    self.current_state = .idle;
                    self.requested_state = self.current_state;
                },
                .running => {
                    const pos_error: AzEl = blk: {
                        self.lock.lock();
                        defer self.lock.unlock();

                        break :blk .{
                            .azimuth = self.target.azimuth - self.position.azimuth,
                            .elevation = self.target.elevation - self.position.elevation,
                        };
                    };

                    self.drive(pos_error) catch {
                        self.lock.lock();
                        defer self.lock.unlock();

                        self.current_state = .stopped;
                        continue;
                    };
                },
                .stopped => {
                    // attempt to reset the drive outputs
                    try self.labjack.writeIoLines(.{false} ** 4);
                    break;
                },
            }
        }
    }
};

pub const LoopTimer = struct {
    interval_ns: u64,

    start: i128 = 0,

    pub fn mark(self: *LoopTimer) bool {
        self.start = std.time.nanoTimestamp();
        return true;
    }

    pub fn sleep(self: *LoopTimer) void {
        const now = std.time.nanoTimestamp();
        const elapsed: u64 = @intCast(now - self.start);

        std.time.sleep(self.interval_ns - elapsed);
    }
};