tao-fake-mirror-server.c

// tao-fake-mirror-server.c -
//
// Implementation of a fake remote deformable mirror server.
//
//-----------------------------------------------------------------------------
//
// This file if part of TAO real-time software licensed under the MIT license
// (https://git-cral.univ-lyon1.fr/tao/tao-rt).
//
// Copyright (C) 2021-2022, Éric Thiébaut.

#include "tao-basics.h"
#include "tao-layouts.h"
#include "tao-remote-mirrors-private.h"
#include "tao-generic.h"
#include "tao-errors.h"

#include <math.h>
#include <string.h>

// Private data needed for the cleanup callback.
static tao_remote_mirror* dm = NULL;
static long* inds = NULL;
static uint8_t* msk = NULL;

// Send the requested command.
static tao_status on_send(
    tao_remote_mirror* dm,
    void* ctx)
{
    // Filter the requested commands to compute the actual commands.  Both are
    // relative to the reference commands.
    double* act_cmds = tao_remote_mirror_get_actual_commands(dm);
    const double* req_cmds = tao_remote_mirror_get_requested_commands(dm);
    const double* refs =  tao_remote_mirror_get_reference(dm);
    long nacts = tao_remote_mirror_get_nacts(dm);
    for (long i = 0; i < nacts; ++i) {
        double cmd = tao_clamp(req_cmds[i] + refs[i], -1.0, 1.0) - refs[i];
        act_cmds[i] = isfinite(cmd) ? cmd : 0.0;
    }
    return TAO_OK;
}

// Reset the deformable mirror.  This amounts to sending zero commands.
static tao_status on_reset(
    tao_remote_mirror* dm,
    void* ctx)
{
    double* req_cmds = tao_remote_mirror_get_requested_commands(dm);
    long nacts = tao_remote_mirror_get_nacts(dm);
    for (long i = 0; i < nacts; ++i) {
        req_cmds[i] = 0.0;
    }
    return on_send(dm, ctx);
}

// Release resources.  This function is automatically called on normal exit.
static void cleanup(void)
{
    if (dm != NULL) {
        if (dm->base.state != TAO_STATE_UNREACHABLE) {
            // FIXME: should lock
            dm->base.state = TAO_STATE_UNREACHABLE;
        }
        if (dm->base.command != TAO_COMMAND_NONE) {
            // FIXME: should lock
            dm->base.command = TAO_COMMAND_NONE;
        }
        tao_remote_mirror_detach(dm);
        dm = NULL;
    }
    if (msk != NULL) {
        tao_free(msk);
        msk = NULL;
    }
    if (inds != NULL) {
        tao_free(inds);
        inds = NULL;
    }
}

int main(
    int argc,
    char* argv[])
{
    // Determine program name.
    const char* progname = tao_basename(argv[0]);

    // Install function to free all allocated resources.
    if (atexit(cleanup) != 0) {
        fprintf(stderr, "%s: failed to install cleanup handler\n",
                progname);
        return EXIT_FAILURE;
    }

    // Parse arguments.
    char const* ident = NULL;
    bool debug = false;
    long nbufs = 10000;
    long nacts = 97;
    unsigned int orient = 0;
    unsigned int perms = 0077;
    const char* usage = "Usage: %s [OPTIONS ...] [--] NAME\n";
    bool opt = true;
    for (int iarg = 1; iarg < argc; ++iarg) {
        char dummy;
        if (opt) {
            // Argument may be an option.
            if (argv[iarg][0] != '-') {
                opt = false;
            } else if (argv[iarg][1] == '-' && argv[iarg][2] == '\0') {
                opt = false;
                continue;
            }
        }
        if (!opt) {
            // Positional argument.
            if (ident == NULL) {
                ident = argv[iarg];
                continue;
            }
            fprintf(stderr, "%s: too many arguments\n", progname);
        bad_usage:
            fprintf(stderr, usage, progname);
            return EXIT_FAILURE;
        }
        // Argument is an option.
        if (strcmp(argv[iarg], "-h") == 0
            || strcmp(argv[iarg], "-help") == 0
            || strcmp(argv[iarg], "--help") == 0) {
            printf(usage, progname);
            printf("\n");
            printf("Arguments:\n");
            printf("  NAME               Name of server.\n");
            printf("\n");
            printf("Options:\n");
            printf("  -orient BITS       Orientation of layout [%u].\n",
                   orient);
            printf("  -nacts NACTS       Number of actuators [%ld].\n",
                   nacts);
            printf("  -nbufs NBUFS       Number of frame buffers [%ld].\n",
                   nbufs);
            printf("  -perms BITS        Bitwise mask of permissions [0%o].\n",
                   perms);
            printf("  -debug             Debug mode [%s].\n",
                   (debug ? "true" : "false"));
            printf("  -h, -help, --help  Print this help.\n");
            return EXIT_SUCCESS;
        }
        if (strcmp(argv[iarg], "-orient") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%iarg %c", (int*)&orient, &dummy) != 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
            }
            orient &= 5;
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-nacts") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%ld %c", &nacts, &dummy) != 1
                || nacts < 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
            }
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-nbufs") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%ld %c", &nbufs, &dummy) != 1
                || nbufs < 2) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
            }
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-perms") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%iarg %c", (int*)&perms, &dummy) != 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
            }
            perms &= 0777;
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-debug") == 0) {
            debug = true;
            continue;
        }
        fprintf(stderr, "%s: unknown option %s\n", progname, argv[iarg]);
        return EXIT_FAILURE;
    }
    if (ident == NULL) {
        fprintf(stderr, "%s: missing server name\n", progname);
        goto bad_usage;
    }

    // Build mirror mask and layout indices.
    long dim = lround(2*sqrt(nacts/M_PI));
    msk = tao_malloc(dim*dim*sizeof(*msk));
    if (msk == NULL) {
        fprintf(stderr, "%s: cannot allocate mirror mask of size %ld×%ld "
                "for %ld actuators\n", progname, dim, dim, nacts);
        return EXIT_FAILURE;
    }
    inds = tao_malloc(dim*dim*sizeof(*inds));
    if (inds == NULL) {
        fprintf(stderr, "%s: cannot allocate array for layout indices\n",
                progname);
        return EXIT_FAILURE;
    }
    if (tao_layout_mask_instanciate(msk, dim, dim, nacts, inds) == NULL) {
        fprintf(stderr, "%s: failed to instantiate mirror mask\n",
                progname);
        return EXIT_FAILURE;
    }
    nacts = tao_indexed_layout_build(inds, msk, dim, dim, orient);
    if (nacts < 1) {
        fprintf(stderr, "%s: failed to build layout indices\n", progname);
        tao_report_error();
        return EXIT_FAILURE;
    }

    // Allocate the remote mirror instance.
    dm = tao_remote_mirror_create(ident, nbufs, inds, dim, dim, perms);
    if (dm == NULL) {
        fprintf(stderr, "%s: failed to create remote mirror instance\n",
                progname);
        tao_report_error();
        return EXIT_FAILURE;
    }

    // Run loop (on entry of the loop we own the lock on the remote mirror
    // instance).
    tao_remote_mirror_operations ops = {
        .on_send = on_send,
        .on_reset = on_reset,
        .name = ident,
        .debug = debug
    };
    tao_status status = tao_remote_mirror_run_loop(dm, &ops, NULL);
    if (status != TAO_OK) {
        tao_report_error();
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}