const std = @import("std");

const microzig = @import("microzig");
const rp2xxx = microzig.hal;
const peripherals = microzig.chip.peripherals;

const board = @import("board.zig");
const DCC = @import("dcc.zig");
const main = @import("main.zig");
const motor = @import("motor.zig");

// XXX: hardcoded!
pub const PID_INTERRUPT_MS = 1000;

// TODO: use fixed point arithmetics here!
const VariableType = f32;

// PID State variables
var err_sum: VariableType = 0;
var err_last: VariableType = 0;

sp: DCC.Speed = .stop,

kp: VariableType,
ki: VariableType,
kd: VariableType,

fwd_emc_offset: u12 = 0, // ADC is 12 bits
bwd_emc_offset: u12 = 0, // ADC is 12 bits

// TODO: make an initWithInterrupt function to set up periodic interrupts

pub fn init(
    comptime b: board.BoardConfig,
    interrupt: u10,
    kp: VariableType,
    ki: VariableType,
    kd: VariableType,
) @This() {
    return @This(){
        .kp = kp,
        .ki = ki * @as(VariableType, @floatFromInt(interrupt)),
        .kd = kd / @as(VariableType, @floatFromInt(interrupt)),
        .fwd_emc_offset = motor.measureEMCOffset(b.md, main.pins.m1emf) catch 0,
        .bwd_emc_offset = motor.measureEMCOffset(b.md, main.pins.m2emf) catch 0,
    };
}

pub fn compute(self: @This(), input: VariableType) VariableType {
    // Calculate errors
    const err: VariableType = @as(VariableType, @floatFromInt(@intFromEnum(self.sp))) - input;
    err_sum += err;
    const err_diff: VariableType = err - err_last;

    // Calculate PID output
    const output: VariableType = self.kp * err + self.ki * err_sum + self.kd * err_diff;

    // Update state
    err_last = err;

    // re-enable interrupt
    // setAlarm(PID_INTERRUPT_MS);

    return output;
}

// XXX: make this non-public!
pub fn setAlarm(ms: u32) void {
    const current = rp2xxx.time.get_time_since_boot();
    const target = current.add_duration(microzig.drivers.time.Duration.from_ms(ms));

    peripherals.TIMER.ALARM0.write_raw(@truncate(@intFromEnum(target)));
}