/*
 *  remotectl.cpp
 *
 *  Remote control server.
 *
 *  Joris van Rantwijk 2024
 */

#include <ctype.h>
#include <getopt.h>
#include <limits.h>
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <algorithm>
#include <chrono>
#include <fstream>
#include <functional>
#include <istream>
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <boost/asio.hpp>

#include "puzzlefw.hpp"
#include "logging.hpp"
#include "interrupt_manager.hpp"
#include "data_server.hpp"
#include "subproc.hpp"
#include "version.hpp"

using namespace puzzlefw;
namespace asio = boost::asio;


/* ********  Utility functions  ******** */

/** Return true if the string ends with the suffix. */
bool str_ends_with(const std::string& value, const std::string& suffix)
{
    return (value.size() >= suffix.size()) &&
        std::equal(value.end() - suffix.size(), value.end(), suffix.begin());
}


/** Return lower-case copy of string. */
std::string str_to_lower(const std::string& value)
{
    std::string result = value;
    for (char& c : result) {
        c = tolower(c);
    }
    return result;
}


/** String formatting. */
std::string str_format(const char *format, ...)
    __attribute__ ((format (printf, 1, 2)));

std::string str_format(const char *format, ...)
{
    va_list ap;
    va_start(ap, format);

    std::string result(800, ' ');
    size_t n = vsnprintf(result.data(), result.size(), format, ap);
    result.resize(n);

    va_end(ap);
    return result;
}


/** Convert string to unsigned integer. */
bool parse_uint(const std::string& s, unsigned int& v)
{
    if (s.empty()) {
        return false;
    }
    size_t pos = 0;
    unsigned long t = std::stoul(s, &pos, 10);
    if (pos != s.size()) {
        return false;
    }
    if (t > UINT_MAX) {
        return false;
    }
    v = t;
    return true;
}


/** Convert string to floating point number. */
bool parse_float(const std::string& s, double& v)
{
    if (s.empty()) {
        return false;
    }
    size_t pos = 0;
    v = std::stod(s, &pos);
    return (pos == s.size());
}


/* ********  Network configuration  ******** */

enum NetworkConfigMode { NETCFG_INVALID = 0, NETCFG_DHCP, NETCFG_STATIC };

struct NetworkConfig {
    NetworkConfigMode mode;
    asio::ip::address_v4 ipaddr;
    asio::ip::address_v4 netmask;
    asio::ip::address_v4 gateway;
    NetworkConfig() : mode(NETCFG_INVALID) { }
};


/** Read network configuration from file. */
bool read_network_config(const std::string& filename, NetworkConfig& ipcfg)
{
    ipcfg = NetworkConfig();

    std::ifstream is(filename);
    if (!is) {
        log(LOG_ERROR, "Can not read %s", filename.c_str());
        return false;
    }

    std::string line;

    while (std::getline(is, line)) {

        size_t p = line.find_last_not_of(" \t\n\v\f\r");
        if (p != line.npos) {
            line.erase(p + 1);
        }

        p = line.find('=');
        if (p == line.npos) {
            continue;
        }

        std::string label = line.substr(0, p);
        std::string value = line.substr(p + 1);

        boost::system::error_code ec{};

        if (label == "MODE") {
            if (value == "dhcp") {
                ipcfg.mode = NETCFG_DHCP;
            }
            if (value == "static") {
                ipcfg.mode = NETCFG_STATIC;
            }
        }
        if (label == "IPADDR" && (! value.empty())) {
            ipcfg.ipaddr = asio::ip::make_address_v4(value, ec);
        }
        if (label == "NETMASK" && (! value.empty())) {
            ipcfg.netmask = asio::ip::make_address_v4(value, ec);
        }
        if (label == "GATEWAY" && (! value.empty())) {
            ipcfg.gateway = asio::ip::make_address_v4(value, ec);
        }

        if (ec) {
            return false;
        }
    }

    return ((ipcfg.mode == NETCFG_DHCP)
            || (ipcfg.mode == NETCFG_STATIC
                && (! ipcfg.ipaddr.is_unspecified())
                && (! ipcfg.netmask.is_unspecified())));
}


/** Parse network configuration arguments. */
bool parse_network_config(const std::vector<std::string>& args,
                          NetworkConfig& ipcfg,
                          std::string& errmsg)
{
    ipcfg = NetworkConfig{};
    errmsg = std::string();

    if (args.size() == 1 && str_to_lower(args[0]) == "dhcp") {
        ipcfg.mode = NETCFG_DHCP;
        return true;
    }

    if ((args.size() == 3 || args.size() == 4)
            && str_to_lower(args[0]) == "static") {
        ipcfg.mode = NETCFG_STATIC;

        boost::system::error_code ec{};
        ipcfg.ipaddr = asio::ip::make_address_v4(args[1], ec);
        if (ec || ipcfg.ipaddr.is_unspecified()) {
            errmsg = "Invalid IP address";
            return false;
        }

        ipcfg.netmask = asio::ip::make_address_v4(args[2], ec);
        if (ec || ipcfg.netmask.is_unspecified()) {
            errmsg = "Invalid netmask";
            return false;
        }

        // Check that netmask describes a valid prefix.
        unsigned int mask = ipcfg.netmask.to_uint();
        if (mask & ((~mask) >> 1)) {
            errmsg = "Invalid netmask";
            return false;
        }

        // Optional gateway address.
        if (args.size() == 4) {
            ipcfg.gateway = asio::ip::make_address_v4(args[3], ec);
            if (ec) {
                errmsg = "Invalid gateway";
                return false;
            }
        }

        return true;
    }

    errmsg = "Invalid address mode";
    return false;
}


/* ********  Calibration  ******** */

enum RangeSpec {
    RANGE_NONE = 0,
    RANGE_LO = 1,
    RANGE_HI = 2
};

struct ChannelCalibration {
    RangeSpec range_spec;
    double offset_lo;
    double offset_hi;
    double gain_lo;
    double gain_hi;
};

struct Calibration {
    ChannelCalibration channel_cal[4];
};


/** Read calibration from file. */
void read_calibration_file(const std::string& filename, Calibration& cal)
{
    // Set defaults in case calibration is missing or incomplete.
    for (unsigned int channel = 0; channel < 4; channel++) {
        cal.channel_cal[channel].range_spec = RANGE_LO;
        cal.channel_cal[channel].offset_lo = 8192;
        cal.channel_cal[channel].offset_hi = 8192;
        cal.channel_cal[channel].gain_lo = -8191;
        cal.channel_cal[channel].gain_hi = -409;
    }

    std::ifstream is(filename);
    if (!is) {
        log(LOG_ERROR, "Can not read %s", filename.c_str());
        return;
    }

    std::string line;
    while (std::getline(is, line)) {

        size_t p = line.find_last_not_of(" \t\n\v\f\r");
        if (p != line.npos) {
            line.erase(p + 1);
        }

        p = line.find('=');
        if (p == line.npos) {
            continue;
        }

        std::string label = line.substr(0, p);
        std::string value = line.substr(p + 1);

        if (label.size() < 4
                || label[0] != 'C'
                || label[1] != 'H'
                || label[2] < '1'
                || label[2] > '4'
                || label[3] != '_') {
            continue;
        }

        unsigned int channel = label[2] - '1';
        label.erase(0, 4);

        if (label == "RANGE") {
            if (value == "LO") {
                cal.channel_cal[channel].range_spec = RANGE_LO;
            }
            if (value == "HI") {
                cal.channel_cal[channel].range_spec = RANGE_HI;
            }
        }
        if (label == "OFFSET_LO") {
            parse_float(value, cal.channel_cal[channel].offset_lo);
        }
        if (label == "OFFSET_HI") {
            parse_float(value, cal.channel_cal[channel].offset_hi);
        }
        if (label == "GAIN_LO") {
            parse_float(value, cal.channel_cal[channel].gain_lo);
        }
        if (label == "GAIN_HI") {
            parse_float(value, cal.channel_cal[channel].gain_hi);
        }
    }
}


/** Write calibration to file. */
bool write_calibration_file(const std::string& filename, const Calibration& cal)
{
    std::ofstream os(filename);
    if (!os) {
        log(LOG_ERROR, "Can not write %s", filename.c_str());
        return false;
    }

    for (unsigned int channel = 0; channel < 4; channel++) {
        std::string line;
        line = str_format("CH%u_RANGE=%s\n", channel + 1,
            (cal.channel_cal[channel].range_spec == RANGE_HI) ? "HI" : "LO");
        os << line;

        line = str_format("CH%u_OFFSET_LO=%.6f\n", channel + 1,
            cal.channel_cal[channel].offset_lo);
        os << line;

        line = str_format("CH%u_OFFSET_HI=%.6f\n", channel + 1,
            cal.channel_cal[channel].offset_hi);
        os << line;

        line = str_format("CH%u_GAIN_LO=%.6f\n", channel + 1,
            cal.channel_cal[channel].gain_lo);
        os << line;

        line = str_format("CH%u_GAIN_HI=%.6f\n", channel + 1,
            cal.channel_cal[channel].gain_hi);
        os << line;
    }

    if (!os) {
        log(LOG_ERROR, "Error while writing %s", filename.c_str());
        return false;
    }

    return true;
}


/* ********  Run subprograms  ******** */

/** Run script to activate or save network configuration. */
bool run_ipcfg_script(const NetworkConfig& ipcfg, bool save)
{
    std::vector<std::string> args;
    args.push_back(save ? "save" : "config");
    if (ipcfg.mode == NETCFG_STATIC) {
        args.push_back("--mode");
        args.push_back("static");
        args.push_back("--ipaddr");
        args.push_back(ipcfg.ipaddr.to_string());
        args.push_back("--netmask");
        args.push_back(ipcfg.netmask.to_string());
        args.push_back("--gateway");
        if (ipcfg.gateway.is_unspecified()) {
            args.push_back("");
        } else {
            args.push_back(ipcfg.gateway.to_string());
        }
    } else {
        args.push_back("--mode");
        args.push_back("dhcp");
    }
    int status = run_subprocess("/opt/puzzlefw/bin/puzzle-ipcfg", args);
    return (status == 0);
}


/** Run script to copy calibration file to SD card. */
bool run_calibration_script()
{
    std::vector<std::string> args = { "save" };
    int status = run_subprocess("/opt/puzzlefw/bin/puzzle-calibration", args);
    return (status == 0);
}


/* ********  Temperature  ******** */

/** Read FPGA temperature. */
bool read_fpga_temperature(double& temp)
{
    const std::string xadc_dir =
        "/sys/devices/soc0/axi/f8007100.adc/iio:device0";

    std::ifstream is(xadc_dir + "/in_temp0_raw");
    double temp_raw;
    is >> temp_raw;
    if (!is) {
        return false;
    }

    is = std::ifstream(xadc_dir + "/in_temp0_offset");
    double temp_offset;
    is >> temp_offset;
    if (!is) {
        return false;
    }

    is = std::ifstream(xadc_dir + "/in_temp0_scale");
    double temp_scale;
    is >> temp_scale;
    if (!is) {
        return false;
    }

    temp = (temp_raw + temp_offset) * temp_scale / 1000.0;
    return true;
}


/* ********  class CommandHandler  ******** */

// Forward declaration.
class ControlServer;


/**
 * The CommandHandler handles commands from remote clients.
 */
class CommandHandler
{
public:
    // IDN response fields.
    static constexpr const char * IDN_MANUFACTURER = "Jigsaw";
    static constexpr const char * IDN_MODEL = "PuzzleFw";

    // Configuration files.
    static constexpr const char * CFG_FILE_CALIBRATION =
        "/var/lib/puzzlefw/cfg/calibration.conf";
    static constexpr const char * CFG_FILE_NETWORK_SAVED =
        "/var/lib/puzzlefw/cfg/network.conf";
    static constexpr const char * CFG_FILE_NETWORK_ACTIVE =
        "/var/lib/puzzlefw/cfg/network_active.conf";

    enum ExitStatus {
        EXIT_ERROR = 1,
        EXIT_HALT = 10,
        EXIT_REBOOT = 11
    };

    struct CommandEnvironment {
        int channel;
        RangeSpec range_spec;
        bool raw_flag;
        bool saved_flag;
    };

    /** Constructor. */
    CommandHandler(asio::io_context& io,
                   PuzzleFwDevice& device,
                   const std::string& serial_number)
      : m_io(io)
      , m_strand(asio::make_strand(io))
      , m_device(device)
      , m_serial_number(serial_number)
      , m_control_server(nullptr)
      , m_shutting_down(false)
      , m_exit_status(EXIT_ERROR)
    { }

    // Delete copy constructor and assignment operator.
    CommandHandler(const CommandHandler&) = delete;
    CommandHandler& operator=(const CommandHandler&) = delete;

    /** Register the control server with the command handler. */
    void set_control_server(ControlServer& control_server)
    {
        m_control_server = &control_server;
    }

    /** Register a data server with the command handler. */
    void add_data_server(DataServer& data_server)
    {
        m_data_servers.push_back(&data_server);
    }

    /** Return the exit status of the command handler. */
    ExitStatus exit_status() const
    {
        return m_exit_status;
    }

    /** Return the Asio strand that runs all handlers for this object. */
    asio::strand<asio::io_context::executor_type> get_executor()
    {
        return m_strand;
    }

    /**
     * Reset non-persistent settings to power-on defaults.
     *
     * This resets all settings except for
     *  - saved calibration
     *  - active network configuration
     *  - saved network configuration.
     *
     * The calibration is reset to the saved calibration.
     */
    void reset()
    {
        read_calibration_file(CFG_FILE_CALIBRATION, m_calibration);
        m_device.set_adc_simulation_enabled(false);
        m_device.set_digital_simulation_enabled(false);
        m_device.set_trigger_mode(TRIG_NONE);
        m_device.set_trigger_ext_channel(0);
        m_device.set_trigger_ext_falling(false);
        m_device.set_trigger_delay(0);
        m_device.set_4channel_mode(false);
        m_device.set_decimation_factor(125);
        m_device.set_averaging_enabled(true);
        m_device.set_shift_steps(0);
        m_device.set_record_length(1024);
        m_device.set_acquisition_enabled(true);
        m_device.set_timetagger_event_mask(0);
    }

    /**
     * Handle a command.
     *
     * If a multi-threaded Asio event loop is running, this method may only
     * be called through the strand returned by "get_executor()".
     *
     * Returns:
     *   Response without line terminator,
     *   or an empty string if no response must be sent.
     */
    std::string handle_command(const std::string& command)
    {
        if (m_shutting_down) {
            // The server is shutting down.
            // Ignore new commands and return an empty string to indicate
            // that no response must be sent.
            return std::string();
        }

        // Split words.
        std::vector<std::string> tokens = parse_command(command);

        // Ignore empty command line without response.
        if (tokens.empty()) {
            return std::string();
        }

        // Convert command to lower case.
        std::string action = str_to_lower(tokens.front());

        CommandEnvironment env;

        // Extract channel specifier.
        env.channel = 0;
        if (action.size() > 7
                && action.compare(0, 6, "ain:ch") == 0
                && action[7] == ':') {
            char channel_digit = action[6];
            if (channel_digit < '1' || channel_digit > '4') {
                return err_unknown_command();
            }
            env.channel = channel_digit - '1';
            action[6] = 'N';  // mark channel index
        }

        // Extract range specifier.
        env.range_spec = RANGE_NONE;
        {
            size_t p = action.rfind(":lo");
            if (p != action.npos
                    && (p + 3 == action.size() || action[p+3] == '?')) {
                env.range_spec = RANGE_LO;
                action[p + 1] = 'R';  // mark range specifier
                action[p + 2] = 'R';
            } else {
                p = action.rfind(":hi");
                if (p != action.npos
                        && (p + 3 == action.size() || action[p+3] == '?')) {
                    env.range_spec = RANGE_HI;
                    action[p + 1] = 'R';  // mark range specifier
                    action[p + 2] = 'R';
                }
            }
        }

        // Detect :RAW flag.
        env.raw_flag = str_ends_with(action, ":raw?");

        // Detect :SAVED flag.
        env.saved_flag = str_ends_with(action, ":saved")
                         || str_ends_with(action, ":saved?");

        // Handle commands without argument.
        {
            const auto it = command_table_no_args.find(action);
            if (it != command_table_no_args.end()) {
                if (tokens.size() != 1) {
                    return err_unexpected_argument();
                }
                auto func = it->second;
                return (this->*func)(env);
            }
        }

        // Handle commands with one argument.
        {
            const auto it = command_table_one_arg.find(action);
            if (it != command_table_one_arg.end()) {
                if (tokens.size() < 2) {
                    return err_missing_argument();
                }
                if (tokens.size() > 2) {
                    return err_unexpected_argument();
                }
                auto func = it->second;
                return (this->*func)(env, tokens[1]);
            }
        }

        // Handle command IPCFG.
        if (action == "ipcfg" || action == "ipcfg:saved") {
            if (tokens.size() < 2) {
                return err_missing_argument();
            }
            if (tokens.size() > 5) {
                return err_unexpected_argument();
            }
            tokens.erase(tokens.begin());
            return cmd_ipcfg(env, tokens);
        }

        return err_unknown_command();
    }

private:
    /** Asynchronously stop control and/or data servers. */
    void stop_server(std::function<void()> handler);
    void stop_data_servers(unsigned int idx, std::function<void()> handler);

    /** Asynchronously start control and/or data servers. */
    void start_server();
    void start_data_servers(unsigned int idx);

    /** Convert a raw ADC sample to Volt. */
    double convert_sample_to_volt(unsigned int channel, unsigned int sample)
    {
        ChannelCalibration& cal = m_calibration.channel_cal[channel];
        RangeSpec range_spec = cal.range_spec;
        double offs = (range_spec == RANGE_HI) ? cal.offset_hi : cal.offset_lo;
        double gain = (range_spec == RANGE_HI) ? cal.gain_hi : cal.gain_lo;
        return (sample - offs) / gain;
    }

    std::string err_unknown_command() const
    {
        return "ERROR Unknown command";
    }

    std::string err_unexpected_argument() const
    {
        return "ERROR Unexpected argument";
    }

    std::string err_missing_argument() const
    {
        return "ERROR Missing argument";
    }

    std::string err_invalid_argument() const
    {
        return "ERROR Invalid argument";
    }

    /** Parse command to a list of white space separated tokens. */
    std::vector<std::string> parse_command(const std::string& command)
    {
        static constexpr std::string_view space_chars = " \t\n\v\f\r";
        std::vector<std::string> tokens;

        for (size_t p = 0; p < command.size(); ) {

            p = command.find_first_not_of(space_chars, p);
            if (p == command.npos) {
                break;
            }

            size_t q = command.find_first_of(space_chars, p);
            if (q == command.npos) {
                q = command.size();
            }
            tokens.emplace_back(command, p, q - p);

            p = q;
        }

        return tokens;
    }

    /** Handle command *IDN? */
    std::string qry_idn(CommandEnvironment env)
    {
        VersionInfo fw_version = m_device.get_version_info();
        return str_format("%s,%s,%s,FW-%d.%d/SW-%d.%d",
                          IDN_MANUFACTURER,
                          IDN_MODEL,
                          m_serial_number.c_str(),
                          fw_version.major_version,
                          fw_version.minor_version,
                          PUZZLEFW_SW_MAJOR,
                          PUZZLEFW_SW_MINOR);
    }

    /** Handle command TIMESTAMP? */
    std::string qry_timestamp(CommandEnvironment env)
    {
        uint64_t timestamp = m_device.get_timestamp();
        return std::to_string(timestamp);
    }

    /** Handle command AIN:CHANNELS:COUNT? */
    std::string qry_channels_count(CommandEnvironment env)
    {
        unsigned int n = m_device.get_analog_channel_count();
        return std::to_string(n);
    }

    /** Handle command AIN:CHANNELS:ACTIVE? */
    std::string qry_channels_active(CommandEnvironment env)
    {
        return m_device.is_4channel_mode() ? "4" : "2";
    }

    /** Handle command AIN:CHn:RANGE? */
    std::string qry_channel_range(CommandEnvironment env)
    {
        ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
        return (cal.range_spec == RANGE_HI) ? "HI" : "LO";
    }

    /** Handle command AIN:CHn:OFFS[:range]? */
    std::string qry_channel_offset(CommandEnvironment env)
    {
        ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
        RangeSpec range_spec = env.range_spec;
        if (range_spec == RANGE_NONE) {
            range_spec = cal.range_spec;
        }
        double offs = (range_spec == RANGE_HI) ? cal.offset_hi : cal.offset_lo;
        return str_format("%.6f", offs);
    }

    /** Handle command AIN:CHn:GAIN[:range]? */
    std::string qry_channel_gain(CommandEnvironment env)
    {
        ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
        RangeSpec range_spec = env.range_spec;
        if (range_spec == RANGE_NONE) {
            range_spec = cal.range_spec;
        }
        double gain = (range_spec == RANGE_HI) ? cal.gain_hi : cal.gain_lo;
        return str_format("%.6f", gain);
    }

    /** Handle command AIN:CHn:SAMPLE[:RAW]? */
    std::string qry_channel_sample(CommandEnvironment env)
    {
        unsigned int sample = m_device.get_adc_sample(env.channel);
        if (env.raw_flag) {
            return std::to_string(sample);
        } else {
            double v = convert_sample_to_volt(env.channel, sample);
            return str_format("%.6f", v);
        }
    }

    /** Handle command AIN:CHn:MINMAX[:RAW]? */
    std::string qry_channel_minmax(CommandEnvironment env)
    {
        unsigned int min_sample, max_sample;
        m_device.get_adc_range(env.channel, min_sample, max_sample);
        if (env.raw_flag) {
            return std::to_string(min_sample) + " "
                + std::to_string(max_sample);
        } else {
            double vmin = convert_sample_to_volt(env.channel, min_sample);
            double vmax = convert_sample_to_volt(env.channel, max_sample);
            return str_format("%.6f %.6f", vmin, vmax);
        }
    }

    /** Handle command AIN:SRATE? */
    std::string qry_srate(CommandEnvironment env)
    {
        unsigned int divisor = m_device.get_decimation_factor();
        double srate = 125e6 / divisor;
        return str_format("%.3f", srate);
    }

    /** Handle command AIN:SRATE:DIVISOR? */
    std::string qry_srate_divisor(CommandEnvironment env)
    {
        unsigned int divisor = m_device.get_decimation_factor();
        return std::to_string(divisor);
    }

    /** Handle command AIN:SRATE:MODE? */
    std::string qry_srate_mode(CommandEnvironment env)
    {
        return m_device.is_averaging_enabled() ? "AVERAGE" : "DECIMATE";
    }

    /** Handle command AIN:SRATE:GAIN? */
    std::string qry_srate_gain(CommandEnvironment env)
    {
        double gain = 1.0;
        if (m_device.is_averaging_enabled()) {
            unsigned int divisor = m_device.get_decimation_factor();
            int shift_steps = m_device.get_shift_steps();
            gain = divisor / static_cast<double>(1 << shift_steps);
        }
        return str_format("%.8f", gain);
    }

    /** Handle command AIN:NSAMPLES? */
    std::string qry_nsamples(CommandEnvironment env)
    {
        unsigned int nsamples = m_device.get_record_length();
        return std::to_string(nsamples);
    }

    /** Handle command AIN:TRIGGER:MODE? */
    std::string qry_trigger_mode(CommandEnvironment env)
    {
        TriggerMode trigger_mode = m_device.get_trigger_mode();
        return trigger_mode_to_string(trigger_mode);
    }

    /** Handle command AIN:TRIGGER:EXT:CHANNEL? */
    std::string qry_trigger_ext_channel(CommandEnvironment env)
    {
        unsigned int n = m_device.get_trigger_ext_channel();
        return std::to_string(n);
    }

    /** Handle command AIN:TRIGGER:EXT:EDGE? */
    std::string qry_trigger_ext_edge(CommandEnvironment env)
    {
        return m_device.get_trigger_ext_falling() ? "FALLING" : "RISING";
    }

    /** Handle command AIN:TRIGGER:DELAY? */
    std::string qry_trigger_delay(CommandEnvironment env)
    {
        unsigned int n = m_device.get_trigger_delay();
        return std::to_string(n);
    }

    /** Handle command AIN:TRIGGER:STATUS? */
    std::string qry_trigger_status(CommandEnvironment env)
    {
        if (m_device.is_waiting_for_trigger()) {
            return "WAITING";
        } else {
            if (m_device.is_acquisition_enabled()) {
                return "BUSY";
            } else {
                return "IDLE";
            }
        }
    }

    /** Handle command TT:SAMPLE? */
    std::string qry_tt_sample(CommandEnvironment env)
    {
        unsigned int sample = m_device.get_digital_input_state();
        return str_format("%d %d %d %d",
                          (sample & 1),
                          ((sample >> 1) & 1),
                          ((sample >> 2) & 1),
                          ((sample >> 3) & 1));
    }

    /** Handle command TT:EVENT:MASK? */
    std::string qry_tt_event_mask(CommandEnvironment env)
    {
        unsigned int event_mask = m_device.get_timetagger_event_mask();
        return std::to_string(event_mask);
    }

    /** Handle command IPCFG[:SAVED]? */
    std::string qry_ipcfg(CommandEnvironment env)
    {
        const char *filename = env.saved_flag ? CFG_FILE_NETWORK_SAVED
                                              : CFG_FILE_NETWORK_ACTIVE;
        NetworkConfig ipcfg;
        if (! read_network_config(filename, ipcfg)) {
            return "ERROR Unconfigured";
        }

        std::string result;
        if (ipcfg.mode == NETCFG_DHCP) {
            return "DHCP";
        }
        if (ipcfg.mode == NETCFG_STATIC) {
            return "STATIC "
                    + ipcfg.ipaddr.to_string()
                    + " " + ipcfg.netmask.to_string()
                    + " " + ipcfg.gateway.to_string();
        }

        return "ERROR Unconfigured";
    }

    /** Handle command TEMP:FPGA? */
    std::string qry_temp_fpga(CommandEnvironment env)
    {
        double temp;
        if (! read_fpga_temperature(temp)) {
            return "ERROR Reading temperature failed";
        }
        return str_format("%.1f", temp);
    }

    /** Handle command RESET */
    std::string cmd_reset(CommandEnvironment env)
    {
        reset();
        return "OK";
    }

    /** Handle command HALT */
    std::string cmd_halt(CommandEnvironment env)
    {
        log(LOG_INFO, "Got command HALT");

        m_exit_status = EXIT_HALT;
        m_shutting_down = true;
        m_io.stop();

        // No response while shutting down.
        // Response delivery would not be reliable while the socket is closing.
        return std::string();
    }

    /** Handle command REBOOT */
    std::string cmd_reboot(CommandEnvironment env)
    {
        log(LOG_INFO, "Got command REBOOT");

        m_exit_status = EXIT_REBOOT;
        m_shutting_down = true;
        m_io.stop();

        // No response while shutting down.
        // Response delivery would not be reliable while the socket is closing.
        return std::string();
    }

    /** Handle command AIN:MINMAX:CLEAR */
    std::string cmd_minmax_clear(CommandEnvironment env)
    {
        m_device.clear_adc_range();
        return "OK";
    }

    /** Handle command TT:MARK */
    std::string cmd_tt_mark(CommandEnvironment env)
    {
        m_device.timetagger_mark();
        return "OK";
    }

    /** Handle command AIN:CHANNELS:ACTIVE */
    std::string cmd_channels_active(CommandEnvironment env,
                                    const std::string& arg)
    {
        unsigned int n;
        if (! parse_uint(arg, n)) {
            return err_invalid_argument();
        }
        if (n != 2 && n != 4) {
            return err_invalid_argument();
        }

        // Reduce sample rate if necessary.
        if (n == 4) {
            unsigned int min_divisor = min_srate_divisor(
                m_device.get_trigger_mode() == TRIG_AUTO,
                true);
            unsigned int divisor = m_device.get_decimation_factor();
            if (divisor < min_divisor) {
                m_device.set_decimation_factor(min_divisor);
            }
        }

        m_device.set_4channel_mode(n == 4);
        return "OK";
    }

    /** Handle command AIN:CHn:RANGE */
    std::string cmd_channel_range(CommandEnvironment env,
                                  const std::string& arg)
    {
        std::string range_name = str_to_lower(arg);

        if (range_name == "lo") {
            m_calibration.channel_cal[env.channel].range_spec = RANGE_LO;
        } else if (range_name == "hi") {
            m_calibration.channel_cal[env.channel].range_spec = RANGE_HI;
        } else {
            return err_invalid_argument();
        }

        return "OK";
    }

    /** Handle command AIN:CHn:OFFS[:range] */
    std::string cmd_channel_offset(CommandEnvironment env,
                                   const std::string& arg)
    {
        double offs;
        if ((! parse_float(arg, offs))
                || (offs < 0)
                || (offs > 16383)) {
            return err_invalid_argument();
        }

        ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
        RangeSpec range_spec = env.range_spec;
        if (range_spec == RANGE_NONE) {
            range_spec = cal.range_spec;
        }

        if (range_spec == RANGE_LO) {
            cal.offset_lo = offs;
        }
        if (range_spec == RANGE_HI) {
            cal.offset_hi = offs;
        }

        return "OK";
    }

    /** Handle command AIN:CHn:GAIN[:range] */
    std::string cmd_channel_gain(CommandEnvironment env,
                                 const std::string& arg)
    {
        double gain;
        if ((! parse_float(arg, gain))
                || (gain < -1e6)
                || (gain > 1e6)) {
            return err_invalid_argument();
        }

        ChannelCalibration& cal = m_calibration.channel_cal[env.channel];
        RangeSpec range_spec = env.range_spec;
        if (range_spec == RANGE_NONE) {
            range_spec = cal.range_spec;
        }

        if (range_spec == RANGE_LO) {
            cal.gain_lo = gain;
        }
        if (range_spec == RANGE_HI) {
            cal.gain_hi = gain;
        }

        return "OK";
    }

    /**
     * Return minimum sample rate divisor depending on trigger mode
     * and number of active channels.
     *
     * In auto-trigger mode, divisor must be at least 2.
     * In 4-channel mode, divisor must be at least 2 or 4 depending
     * on auto-trigger mode.
     */
    unsigned int min_srate_divisor(bool trig_auto, bool ch4)
    {
        unsigned int min_divisor = 1;
        if (trig_auto) {
            min_divisor *= 2;
        }
        if (ch4) {
            min_divisor += 2;
        }
        return min_divisor;
    }

    /** Commond handling for setting sample rate. */
    std::string set_srate_divisor(unsigned int divisor)
    {
        unsigned int min_divisor = min_srate_divisor(
            m_device.get_trigger_mode() == TRIG_AUTO,
            m_device.is_4channel_mode());

        if ((divisor < min_divisor)
                || (divisor > PuzzleFwDevice::MAX_DECIMATION_FACTOR)) {
            return err_invalid_argument();
        }

        m_device.set_decimation_factor(divisor);

        if (m_device.is_averaging_enabled()) {
            // Adjust shift steps to avoid 24-bit overflow.
            int shift_steps = 0;
            while (divisor > (1UL << (10 + shift_steps))) {
                shift_steps++;
            }
            m_device.set_shift_steps(shift_steps);
        }

        return "OK";
    }

    /** Handle command AIN:SRATE */
    std::string cmd_srate(CommandEnvironment env,
                          const std::string& arg)
    {
        double v;
        if ((! parse_float(arg, v)) || (v < 1) || (v > 125e6)) {
            return err_invalid_argument();
        }

        unsigned int divisor = lrint(125e6 / v);
        return set_srate_divisor(divisor);
    }

    /** Handle command AIN:SRATE:DIVISOR */
    std::string cmd_srate_divisor(CommandEnvironment env,
                                  const std::string& arg)
    {
        unsigned int divisor;
        if (! parse_uint(arg, divisor)) {
            return err_invalid_argument();
        }

        return set_srate_divisor(divisor);
    }

    /** Handle command AIN:SRATE:MODE */
    std::string cmd_srate_mode(CommandEnvironment env,
                               const std::string& arg)
    {
        std::string srate_mode = str_to_lower(arg);

        if (srate_mode == "average") {

            // Adjust shift steps to avoid 24-bit overflow.
            unsigned int divisor = m_device.get_decimation_factor();
            int shift_steps = 0;
            while (divisor > (1UL << (10 + shift_steps))) {
                shift_steps++;
            }

            m_device.set_averaging_enabled(true);
            m_device.set_shift_steps(shift_steps);
        } else if (srate_mode == "decimate") {
            m_device.set_averaging_enabled(false);
            m_device.set_shift_steps(0);
        } else {
            return err_invalid_argument();
        }

        return "OK";
    }

    /** Handle command AIN:NSAMPLES */
    std::string cmd_nsamples(CommandEnvironment env,
                             const std::string& arg)
    {
        unsigned int n;
        if ((! parse_uint(arg, n))
                || (n < 1)
                || (n > PuzzleFwDevice::MAX_RECORD_LENGTH)) {
            return err_invalid_argument();
        }
        m_device.set_record_length(n);
        return "OK";
    }

    /** Handle command AIN:TRIGGER */
    std::string cmd_trigger(CommandEnvironment env)
    {
        m_device.trigger_force();
        return "OK";
    }

    /** Handle command AIN:TRIGGER:MODE */
    std::string cmd_trigger_mode(CommandEnvironment env,
                                 const std::string& arg)
    {
        std::string trigger_mode = str_to_lower(arg);

        if (trigger_mode == "none") {
            m_device.set_trigger_mode(TRIG_NONE);
        } else if (trigger_mode == "auto") {
            // Reduce sample rate if necessary.
            unsigned int min_divisor = min_srate_divisor(
                true,
                m_device.is_4channel_mode());
            unsigned int divisor = m_device.get_decimation_factor();
            if (divisor < min_divisor) {
                m_device.set_decimation_factor(min_divisor);
            }
            m_device.set_trigger_mode(TRIG_AUTO);
        } else if (trigger_mode == "external") {
            m_device.set_trigger_mode(TRIG_EXTERNAL);
        } else if (trigger_mode == "external_once") {
            m_device.set_trigger_mode(TRIG_EXTERNAL_ONCE);
        } else {
            return err_invalid_argument();
        }

        return "OK";
    }

    /** Handle command AIN:TRIGGER:EXT:CHANNEL */
    std::string cmd_trigger_ext_channel(CommandEnvironment env,
                                        const std::string& arg)
    {
        unsigned int n;
        if ((! parse_uint(arg, n)) || (n > 3)) {
            return err_invalid_argument();
        }
        m_device.set_trigger_ext_channel(n);
        return "OK";
    }

    /** Handle command AIN:TRIGGER:EXT:EDGE */
    std::string cmd_trigger_ext_edge(CommandEnvironment env,
                                     const std::string& arg)
    {
        std::string edge = str_to_lower(arg);
        if (edge == "rising") {
            m_device.set_trigger_ext_falling(false);
        } else if (edge == "falling") {
            m_device.set_trigger_ext_falling(true);
        } else {
            return err_invalid_argument();
        }
        return "OK";
    }

    /** Handle command AIN:TRIGGER:DELAY */
    std::string cmd_trigger_delay(CommandEnvironment env,
                                  const std::string& arg)
    {
        unsigned int n;
        if ((! parse_uint(arg, n))
                || (n > PuzzleFwDevice::MAX_TRIGGER_DELAY)) {
            return err_invalid_argument();
        }
        m_device.set_trigger_delay(n);
        return "OK";
    }

    /** Handle command TT:EVENT:MASK */
    std::string cmd_tt_event_mask(CommandEnvironment env,
                                  const std::string& arg)
    {
        unsigned int n;
        if ((! parse_uint(arg, n)) || (n > 255)) {
            return err_invalid_argument();
        }
        m_device.set_timetagger_event_mask(n);
        return "OK";
    }

    /** Handle command AIN:MINMAX:CLEAR */
    std::string cmd_cal_save(CommandEnvironment env)
    {
        std::string filename = std::string(CFG_FILE_CALIBRATION) + ".new";
        if (! write_calibration_file(filename, m_calibration)) {
            return "ERROR Can not write calibration";
        }

        if (! run_calibration_script()) {
            return "ERROR Saving calibration failed";
        }

        return "OK";
    }

    /** Handle command IPCFG */
    std::string cmd_ipcfg(CommandEnvironment env,
                          const std::vector<std::string>& args)
    {
        NetworkConfig ipcfg;
        std::string errmsg;
        if (! parse_network_config(args, ipcfg, errmsg)) {
            return "ERROR " + errmsg;
        }

        if (env.saved_flag) {

            if (! run_ipcfg_script(ipcfg, true)) {
                return "ERROR Saving network configuration failed";
            }

            return "OK";

        } else {

            // Shut down server sockets; then change IP address and resume.
            m_shutting_down = true;
            stop_server(
                [this,ipcfg]() {
                    run_ipcfg_script(ipcfg, false);
                    m_shutting_down = false;
                    start_server();
                });

            // No response while shutting down.
            // Response delivery would not be reliable while the socket is closing.
            return std::string();
        }
    }

    static const inline std::map<
        std::string,
        std::string (CommandHandler::*)(CommandEnvironment)>
            command_table_no_args = {
        { "*idn?",                  &CommandHandler::qry_idn },
        { "timestamp?",             &CommandHandler::qry_timestamp },
        { "ain:channels:count?",    &CommandHandler::qry_channels_count },
        { "ain:channels:active?",   &CommandHandler::qry_channels_active },
        { "ain:chN:range?",         &CommandHandler::qry_channel_range },
        { "ain:chN:offset?",        &CommandHandler::qry_channel_offset },
        { "ain:chN:offset:RR?",     &CommandHandler::qry_channel_offset },
        { "ain:chN:gain?",          &CommandHandler::qry_channel_gain },
        { "ain:chN:gain:RR?",       &CommandHandler::qry_channel_gain },
        { "ain:chN:sample?",        &CommandHandler::qry_channel_sample },
        { "ain:chN:sample:raw?",    &CommandHandler::qry_channel_sample },
        { "ain:chN:minmax?",        &CommandHandler::qry_channel_minmax },
        { "ain:chN:minmax:raw?",    &CommandHandler::qry_channel_minmax },
        { "ain:srate?",             &CommandHandler::qry_srate },
        { "ain:srate:divisor?",     &CommandHandler::qry_srate_divisor },
        { "ain:srate:mode?",        &CommandHandler::qry_srate_mode },
        { "ain:srate:gain?",        &CommandHandler::qry_srate_gain },
        { "ain:nsamples?",          &CommandHandler::qry_nsamples },
        { "ain:trigger:mode?",      &CommandHandler::qry_trigger_mode },
        { "ain:trigger:ext:channel?",
                                    &CommandHandler::qry_trigger_ext_channel },
        { "ain:trigger:ext:edge?",  &CommandHandler::qry_trigger_ext_edge },
        { "ain:trigger:delay?",     &CommandHandler::qry_trigger_delay },
        { "ain:trigger:status?",    &CommandHandler::qry_trigger_status },
        { "tt:sample?",             &CommandHandler::qry_tt_sample },
        { "tt:event:mask?",         &CommandHandler::qry_tt_event_mask },
        { "ipcfg?",                 &CommandHandler::qry_ipcfg },
        { "ipcfg:saved?",           &CommandHandler::qry_ipcfg },
        { "temp:fpga?",             &CommandHandler::qry_temp_fpga },
        { "reset",                  &CommandHandler::cmd_reset },
        { "halt",                   &CommandHandler::cmd_halt },
        { "reboot",                 &CommandHandler::cmd_reboot },
        { "ain:cal:save",           &CommandHandler::cmd_cal_save },
        { "ain:minmax:clear",       &CommandHandler::cmd_minmax_clear },
        { "ain:trigger",            &CommandHandler::cmd_trigger },
        { "tt:mark",                &CommandHandler::cmd_tt_mark }
    };

    static const inline std::map<
        std::string,
        std::string (CommandHandler::*)(CommandEnvironment, const std::string&)>
            command_table_one_arg = {
        { "ain:channels:active",    &CommandHandler::cmd_channels_active },
        { "ain:chN:range",          &CommandHandler::cmd_channel_range },
        { "ain:chN:offset",         &CommandHandler::cmd_channel_offset },
        { "ain:chN:offset:RR",      &CommandHandler::cmd_channel_offset },
        { "ain:chN:gain",           &CommandHandler::cmd_channel_gain },
        { "ain:chN:gain:RR",        &CommandHandler::cmd_channel_gain },
        { "ain:srate",              &CommandHandler::cmd_srate },
        { "ain:srate:divisor",      &CommandHandler::cmd_srate_divisor },
        { "ain:srate:mode",         &CommandHandler::cmd_srate_mode },
        { "ain:nsamples",           &CommandHandler::cmd_nsamples },
        { "ain:trigger:mode",       &CommandHandler::cmd_trigger_mode },
        { "ain:trigger:ext:channel", &CommandHandler::cmd_trigger_ext_channel },
        { "ain:trigger:ext:edge",   &CommandHandler::cmd_trigger_ext_edge },
        { "ain:trigger:delay",      &CommandHandler::cmd_trigger_delay },
        { "tt:event:mask",          &CommandHandler::cmd_tt_event_mask }
    };

    asio::io_context&           m_io;
    asio::strand<asio::io_context::executor_type> m_strand;
    PuzzleFwDevice&             m_device;
    std::string                 m_serial_number;
    ControlServer*              m_control_server;
    std::vector<DataServer*>    m_data_servers;
    Calibration                 m_calibration;
    bool                        m_shutting_down;
    ExitStatus                  m_exit_status;
};


/* ********  class ControlServer  ******** */

/**
 * Manage TCP connections for remote control.
 */
class ControlServer
{
public:

    /** Constructor. */
    ControlServer(asio::io_context& io,
                  CommandHandler& command_handler,
                  uint16_t tcp_port)
      : m_strand(asio::make_strand(io))
      , m_acceptor(m_strand)
      , m_command_handler(command_handler)
      , m_tcp_port(tcp_port)
    { }

    // Delete copy constructor and assignment operator.
    ControlServer(const ControlServer&) = delete;
    ControlServer& operator=(const ControlServer&) = delete;

    /** Return the Asio strand that runs all handlers for this object. */
    asio::strand<asio::io_context::executor_type> get_executor()
    {
        return m_strand;
    }

    /**
     * Start the server.
     *
     * If a multi-threaded Asio event loop is running, this method may only
     * be called through the strand returned by "get_executor()".
     */
    void start_server()
    {
        // If the server is already open, close and re-open it.
        if (m_acceptor.is_open()) {
            m_acceptor.close();
        }

        // Drop all connections.
        for (auto& conn : m_connections) {
            if (conn->sock.is_open()) {
                conn->sock.close();
            }
        }
        m_connections.clear();

        // Open IPv6 TCP socket.
        m_acceptor.open(asio::ip::tcp::v6());

        // Disable IPV6_V6ONLY to allow IPv4 connections.
        m_acceptor.set_option(asio::ip::v6_only(false));

        // Enable SO_REUSEADDR.
        m_acceptor.set_option(asio::socket_base::reuse_address(true));

        // Bind to the TCP port on all interfaces.
        asio::ip::tcp::endpoint addr(asio::ip::address_v6::any(), m_tcp_port);
        m_acceptor.bind(addr);

        // Start listening for connections.
        m_acceptor.listen();

        // Asynchronously accept connections.
        m_acceptor.async_accept(
            [this](auto ec, auto s){ handle_accept(ec, std::move(s)); });

        log(LOG_INFO, "Ready for TCP connections to port %d", m_tcp_port);
    }

    /**
     * Stop the server and close all connections.
     *
     * If a multi-threaded Asio event loop is running, this method may only
     * be called through the strand returned by "get_executor()".
     */
    void stop_server()
    {
        // Stop accepting connections.
        if (m_acceptor.is_open()) {
            log(LOG_INFO, "Closing TCP server on port %d", m_tcp_port);
            m_acceptor.close();
        }

        // Drop all connections.
        for (auto& conn : m_connections) {
            if (conn->sock.is_open()) {
                conn->sock.close();
            }
        }
        m_connections.clear();
    }

private:
    /**
     * Each active TCP connection is represented by an instance of Connection.
     */
    struct Connection {
        asio::ip::tcp::socket sock;
        asio::streambuf recv_buf;
        std::string send_buf;

        Connection(asio::ip::tcp::socket&& s)
          : sock(std::move(s)), recv_buf(4096)
        { }
    };

    /** Accept completion handler. */
    void handle_accept(boost::system::error_code error,
                       asio::ip::tcp::socket sock)
    {
        if (error) {
            // Ignore error due to cancellation.
            if (error == asio::error::operation_aborted) {
                return;
            }

            // Certain errors can be triggered by network conditions.
            // In these cases, we should retry the accept call.
            if (error == asio::error::broken_pipe
                    || error == asio::error::connection_aborted
                    || error == asio::error::connection_reset
                    || error == asio::error::host_unreachable
                    || error == asio::error::network_down
                    || error == asio::error::network_reset
                    || error == asio::error::network_unreachable
                    || error == asio::error::timed_out) {

                log(LOG_ERROR,
                    "Accept failed for port %d (%s), retrying",
                    m_tcp_port,
                    error.message().c_str());

                // Retry accept call.
                if (m_acceptor.is_open()) {
                    m_acceptor.async_accept(
                        [this](auto& ec, auto s) {
                            handle_accept(ec, std::move(s));
                        });
                }

                return;
            }

            // Raise exception on unexpected error.
            throw std::system_error(error);
        }

        if (! m_acceptor.is_open()) {
            // Oops we were not supposed to accept new connections.
            // Apparently this connection sneaked right through before
            // closing the acceptor socket.
            // Drop the new connection.
            log(LOG_INFO, "Dropping new connection to port %d", m_tcp_port);
            sock.close();
            return;
        }

        if (m_acceptor.is_open()) {
            // Continue accepting connections.
            m_acceptor.async_accept(
                [this](auto ec, auto s){ handle_accept(ec, std::move(s)); });
        }

        log(LOG_INFO, "New connection to port %d", m_tcp_port);

        // Create Connection instance.
        std::shared_ptr<Connection> conn =
            std::make_shared<Connection>(std::move(sock));
        m_connections.push_back(conn);

        // Iniate receive opereration.
        receive_command(conn);
    }

    /** Receive completion handler. */
    void handle_receive(std::shared_ptr<Connection> conn,
                        boost::system::error_code error,
                        size_t len)
    {
        if (! conn->sock.is_open()) {
            // This connection is already closed. Ignore further events.
            return;
        }

        if (error) {
            // Report error.
            if (error == asio::error::eof) {
                log(LOG_INFO,
                    "Connection to port %d closed by remote", m_tcp_port);
            } else {
                log(LOG_ERROR,
                    "Receive failed on port %d (%s), closing connection",
                    m_tcp_port,
                    error.message().c_str());
            }

            // Close this connection.
            conn->sock.close();
            return;
        }

        // Extract a command up to newline.
        std::istream is(&conn->recv_buf);
        std::string command;
        std::getline(is, command, '\n');

        // Handle command.
        process_command(conn, command);
    }

    /** Send completion handler. */
    void handle_send(std::shared_ptr<Connection> conn,
                     boost::system::error_code error,
                     size_t len)
    {
        if (! conn->sock.is_open()) {
            // This connection is already closed. Ignore further events.
            return;
        }

        if (error) {
            // Report error.
            if (error == asio::error::broken_pipe
                    || error == asio::error::connection_reset) {
                log(LOG_INFO,
                    "Connection to port %d closed by remote", m_tcp_port);
            } else {
                log(LOG_ERROR,
                    "Send failed on port %d (%s), closing connection",
                    m_tcp_port,
                    error.message().c_str());
            }

            // Close the connection.
            conn->sock.close();
            return;
        }

        // Discard send buffer.
        assert(len == conn->send_buf.size());
        conn->send_buf.clear();

        // Iniiate receive operation for next command.
        receive_command(conn);
    }

    /** Iniate asynchronous receiving of a command from the connection. */
    void receive_command(std::shared_ptr<Connection> conn)
    {
        if (! conn->sock.is_open()) {
            // The connection is already closed. Don't mess with it.
            return;
        }

        // Initiate receive operation.
        asio::async_read_until(conn->sock, conn->recv_buf, '\n',
            [this,conn](auto ec, size_t n) { handle_receive(conn, ec, n); });
    }

    /** Process a command received through a connection. */
    void process_command(std::shared_ptr<Connection> conn,
                         const std::string& command)
    {
        // Post an event to the strand of the command handler.
        asio::post(
            m_command_handler.get_executor(),
            [this,conn,command]() {
                // This code runs in the command handler strand.
                // Tell the command handler to run the command.
                auto response = m_command_handler.handle_command(command);

                // Post the response back to our own strand.
                asio::post(
                    m_strand,
                    [this,conn,response]() {
                        // This code runs in our own strand.
                        // Handle the response.
                        process_response(conn, response);
                    });
            });
    }

    /** Initiate asynchronous sending of a response to the connection. */
    void process_response(std::shared_ptr<Connection> conn,
                          const std::string& response)
    {
        if (! conn->sock.is_open()) {
            // The connection is already closed. Don't mess with it.
            return;
        }

        if (response.empty()) {
            // Empty response string means don't send a response.
            // Initiate a new receive operation.
            receive_command(conn);
            return;
        }

        // Check that the send buffer is empty.
        assert(conn->send_buf.empty());

        // Put the response message into the send buffer.
        conn->send_buf = response;
        conn->send_buf.push_back('\n');

        // Start asynchronous send.
        asio::async_write(conn->sock, asio::buffer(conn->send_buf),
            [this,conn](auto ec, size_t n) { handle_send(conn, ec, n); });
    }

    asio::strand<asio::io_context::executor_type> m_strand;
    asio::ip::tcp::acceptor m_acceptor;
    CommandHandler&         m_command_handler;
    const uint16_t          m_tcp_port;
    std::vector<std::shared_ptr<Connection>> m_connections;
};


/* ******** Methods for class CommandHandler ******** */

void CommandHandler::stop_server(std::function<void()> handler)
{
    asio::post(m_control_server->get_executor(),
        [this,handler]() {
            m_control_server->stop_server();
            asio::post(m_strand,
                [this,handler]() {
                    stop_data_servers(0, handler);
                });
        });
}

void CommandHandler::stop_data_servers(unsigned int idx,
                                       std::function<void()> handler)
{
    if (idx < m_data_servers.size()) {
        asio::post(m_data_servers[idx]->get_executor(),
            [this,idx,handler]() {
                m_data_servers[idx]->stop_server();
                asio::post(m_strand,
                    [this,idx,handler]() {
                        stop_data_servers(idx + 1, handler);
                    });
            });
    } else {
        handler();
    }
}

void CommandHandler::start_server()
{
    asio::post(m_control_server->get_executor(),
        [this]() {
            m_control_server->start_server();
            asio::post(m_strand,
                [this]() {
                    start_data_servers(0);
                });
        });
}

void CommandHandler::start_data_servers(unsigned int idx)
{
    if (idx < m_data_servers.size()) {
        asio::post(m_data_servers[idx]->get_executor(),
            [this,idx]() {
                m_data_servers[idx]->start_server();
                asio::post(m_strand,
                    [this,idx]() {
                        start_data_servers(idx + 1);
                    });
            });
    }
}


/* ******** Main program ******** */

/** Run remote control server. */
int run_remote_control_server(
    puzzlefw::PuzzleFwDevice& device,
    const std::string& serial_number)
{
    namespace asio = boost::asio;
    using namespace puzzlefw;

    asio::io_context io;

    // Catch Ctrl-C for controlled shut down.
    asio::signal_set signals(io, SIGINT);
    signals.async_wait(
        [&io](auto& ec, int sig) {
            log(LOG_INFO, "Got SIGINT, stopping server");
            io.stop();
        });

    // Reserve 3/4 of the DMA buffer for analog acquisition data.
    // Reserve 1/4 of the DMA buffer for timetagger data.
    size_t acq_buf_size = 3 * 4096 * (device.dma_buffer_size() / 4096 / 4);
    size_t timetagger_buf_size = device.dma_buffer_size() - acq_buf_size;

    DmaWriteStream acq_stream(
        device,
        DmaWriteStream::DMA_ACQ,
        0,
        acq_buf_size);
    DmaWriteStream timetagger_stream(
        device,
        DmaWriteStream::DMA_TT,
        acq_buf_size,
        acq_buf_size + timetagger_buf_size);

    DataServer acq_server(io, acq_stream, 5001);
    DataServer timetagger_server(io, timetagger_stream, 5002);

    InterruptManager interrupt_manager(io, device);
    interrupt_manager.add_callback(
        [&acq_server](){ acq_server.handle_interrupt(); });
    interrupt_manager.add_callback(
        [&timetagger_server](){ timetagger_server.handle_interrupt(); });

    DmaErrorMonitor dma_error_monitor(
        io,
        device,
        std::chrono::milliseconds(100));

    CommandHandler command_handler(io, device, serial_number);
    ControlServer control_server(io, command_handler, 5025);

    command_handler.set_control_server(control_server);
    command_handler.add_data_server(acq_server);
    command_handler.add_data_server(timetagger_server);

    // Restore firmware status on exit from this function.
    struct ScopeGuard {
        PuzzleFwDevice& m_device;
        ScopeGuard(PuzzleFwDevice& device) : m_device(device) { }
        ~ScopeGuard() {
            m_device.set_dma_enabled(false);
            m_device.set_acquisition_enabled(false);
        }
    } scope_guard(device);

    // Reset instrument.
    command_handler.reset();

    // Clear DMA errors, then enable DMA engine.
    device.clear_dma_errors();
    device.set_dma_enabled(true);

    // Enable TCP servers.
    control_server.start_server();
    acq_server.start_server();
    timetagger_server.start_server();

    // We could start a few background threads, each calling io.run().
    // It may improve performance a little bit.
    // However, if there are synchronization bugs, it would cause weird issues
    // that are difficult to track down. So let's keep just one thread for now.

    log(LOG_INFO, "Running, press Ctrl-C to stop");
    io.run();

    return command_handler.exit_status();
}


int main(int argc, char **argv)
{
    static const struct option options[] = {
        {"help",              0, 0, 'h'},
        {"serialnr",          1, 0, 1 },
        {nullptr,             0, 0, 0}
    };

    const char *usage_text = "Usage: %s --serialnr SNR\n";

    std::string serial_number = "0";

    while (1) {
        int c = getopt_long(argc, argv, "h", options, nullptr);
        if (c == -1) {
            break;
        }

        switch (c) {
            case 'h':
                printf(usage_text, argv[0]);
                return 0;
            case 1:
                serial_number = optarg;
                break;
            default:
                fprintf(stderr, usage_text, argv[0]);
                return 1;
        }
    }

    if (optind < argc) {
        fprintf(stderr, "ERROR: Unexpected positional argument\n");
        fprintf(stderr, usage_text, argv[0]);
        return 1;
    }

    try {
        PuzzleFwDevice device;

        VersionInfo version = device.get_version_info();
        printf("Detected PuzzleFW firmware version %d.%d\n",
               version.major_version, version.minor_version);
        printf("    %u analog input channels\n",
               device.get_analog_channel_count());
        printf("    DMA buffer size: %zu bytes\n",
               device.dma_buffer_size());

        return run_remote_control_server(device, serial_number);

    } catch (std::exception& e) {
        fprintf(stderr, "ERROR: %s\n", e.what());
        return 1;
    }
}

/* end */