uca-kiro/src/uca-kiro-camera.c

/* Copyright (C) 2011, 2012 Matthias Vogelgesang <matthias.vogelgesang@kit.edu>
   (Karlsruhe Institute of Technology)

   This library is free software; you can redistribute it and/or modify it
   under the terms of the GNU Lesser General Public License as published by the
   Free Software Foundation; either version 2.1 of the License, or (at your
   option) any later version.

   This library is distributed in the hope that it will be useful, but WITHOUT
   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
   FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
   details.

   You should have received a copy of the GNU Lesser General Public License along
   with this library; if not, write to the Free Software Foundation, Inc., 51
   Franklin St, Fifth Floor, Boston, MA 02110, USA */


#include <gmodule.h>
#include <gio/gio.h>
#include <string.h>
#include <math.h>
#include <kiro/kiro-messenger.h>
#include "uca-kiro-camera.h"


#define UCA_KIRO_CAMERA_GET_PRIVATE(obj) (G_TYPE_INSTANCE_GET_PRIVATE ((obj), UCA_TYPE_KIRO_CAMERA, UcaKiroCameraPrivate))

static void uca_kiro_initable_iface_init (GInitableIface *iface);
GError *initable_iface_error = NULL;

G_DEFINE_TYPE_WITH_CODE (UcaKiroCamera, uca_kiro_camera, UCA_TYPE_CAMERA,
                         G_IMPLEMENT_INTERFACE (G_TYPE_INITABLE,
                                                uca_kiro_initable_iface_init))


/**
 * UcaCameraError:
   @UCA_KIRO_CAMERA_ERROR_MISSING_ADDRESS: No KIRO address ('kiro://<IP>:<PORT>') property was supplied during camera creation
   @UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT: KIRO address has the wrong format. Expected: 'kiro://<IP>:<PORT>'
   @UCA_KIRO_CAMERA_ERROR_KIRO_CONNECTION_FAILED: Failed to establish a KIRO connection to the given TANGO server
 */
GQuark uca_kiro_camera_error_quark()
{
    return g_quark_from_static_string ("uca-kiro-camera-error-quark");
}


enum {
    PROP_KIRO_ADDRESS = N_BASE_PROPERTIES,
    PROP_KIRO_REMOTE_NAME,
    N_PROPERTIES
};

static const gint kiro_overrideables[] = {
    PROP_NAME,
    0,
};

static GParamSpec *kiro_properties[N_PROPERTIES] = { NULL, };

//+1 to make index and property ID identical
static guint64 kiro_scalar_prop_buffers[N_PROPERTIES] = { 0, };

typedef struct {
    GParamSpec *pspec;
    guint32 remote_id;
    guint32 local_id;
    guint64 buffer;
} KiroDynamicScalarProperty;


struct _UcaKiroCameraPrivate {
    guint8 *dummy_data;
    guint current_frame;
    gchar *kiro_address_string;
    gchar *kiro_address;
    gchar *kiro_port;
    gchar *remote_name;
    GList *property_install_list;
    KiroDynamicScalarProperty *kiro_dynamic_scalar_properties;

    gboolean thread_running;
    gboolean kiro_connected;
    gboolean construction_error;

    GThread *grab_thread;
    KiroMessenger *messenger;
    gulong peer_rank;
    KiroRequest *rec_request;

    guint roi_height;
    guint roi_width;
    guint bytes_per_pixel;
};


static gpointer
kiro_grab_func(gpointer data)
{
    /* UcaKiroCamera *kiro_camera = UCA_KIRO_CAMERA (data); */
    /* g_return_val_if_fail (UCA_IS_KIRO_CAMERA (kiro_camera), NULL); */

    /* UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (kiro_camera); */
    /* UcaCamera *camera = UCA_CAMERA (kiro_camera); */
    /* gdouble fps; */
    /* g_object_get (G_OBJECT (data), "frames-per-second", &fps, NULL); */
    /* const gulong sleep_time = (gulong) G_USEC_PER_SEC / fps; */

    /* while (priv->thread_running) { */
    /*     camera->grab_func (NULL, camera->user_data); */
    /*     g_usleep (sleep_time); */
    /* } */

    return NULL;
}

static void
uca_kiro_camera_start_recording(UcaCamera *camera, GError **error)
{
    /* gboolean transfer_async = FALSE; */
    /* UcaKiroCameraPrivate *priv; */
    /* g_return_if_fail(UCA_IS_KIRO_CAMERA (camera)); */

    /* priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */
    /* g_object_get (G_OBJECT(camera), */
    /*         "transfer-asynchronously", &transfer_async, */
    /*         NULL); */

    /* //'Cache' ROI settings from TANGO World */
    /* g_object_get (G_OBJECT(camera), */
    /*         "roi-width", &priv->roi_width, */
    /*         NULL); */
    /* g_object_get (G_OBJECT(camera), */
    /*         "roi-height", &priv->roi_height, */
    /*         NULL); */

    /* size_t bits = 0; */
    /* g_object_get (G_OBJECT(camera), */
    /*         "sensor-bitdepth", &bits, */
    /*         NULL); */

    /* priv->bytes_per_pixel = 1; */
    /* if (bits > 8) priv->bytes_per_pixel++; */
    /* if (bits > 16) priv->bytes_per_pixel++; */
    /* if (bits > 24) priv->bytes_per_pixel++; */

    /* Tango::DevState state; */
    /* g_object_get (G_OBJECT(camera), */
    /*         "State", &state, */
    /*         NULL); */
    /* try { */
    /*     if (Tango::DevState::STANDBY == state) */
    /*         priv->tango_device->command_inout ("StartRecording"); */
    /* } */
    /* catch (Tango::DevFailed &e) { */
    /*     g_warning ("Failed to execute 'StartRecording' on the remote camera due to a TANGO exception.\n"); */
    /*     g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_TANGO_EXCEPTION_OCCURED, */
    /*                  "A TANGO exception was raised: '%s'", (const char *)e.errors[0].desc); */
    /*     return; */
    /* } */


    /* /* */
    /*  * In case asynchronous transfer is requested, we start a new thread that */
    /*  * invokes the grab callback, otherwise nothing will be done here. */
    /*  *1/ */
    /* if (transfer_async) { */
    /*     GError *tmp_error = NULL; */
    /*     priv->thread_running = TRUE; */
    /*     priv->grab_thread = g_thread_create (kiro_grab_func, camera, TRUE, &tmp_error); */

    /*     if (tmp_error != NULL) { */
    /*         priv->thread_running = FALSE; */
    /*         g_propagate_error (error, tmp_error); */
    /*         try { */
    /*             priv->tango_device->command_inout ("StopRecording"); */
    /*         } */
    /*         catch (Tango::DevFailed &e) { */
    /*             g_warning ("Failed to execute 'StopRecording' on the remote camera due to a TANGO exception: '%s'\n", (const char *)e.errors[0].desc); */
    /*         } */
    /*     } */
    /* } */

    /* kiro_sb_thaw (priv->receive_buffer); */
}

static void
uca_kiro_camera_stop_recording(UcaCamera *camera, GError **error)
{
    /* g_return_if_fail(UCA_IS_KIRO_CAMERA (camera)); */
    /* UcaKiroCameraPrivate *priv; */
    /* priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */

    /* Tango::DevState state; */
    /* g_object_get (G_OBJECT(camera), */
    /*         "State", &state, */
    /*         NULL); */

    /* try { */
    /*     if (Tango::DevState::RUNNING == state) */
    /*         priv->tango_device->command_inout ("StopRecording"); */
    /* } */
    /* catch (Tango::DevFailed &e) { */
    /*     g_warning ("Failed to execute 'StopRecording' on the remote camera due to a TANGO exception.\n"); */
    /*     g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_TANGO_EXCEPTION_OCCURED, */
    /*                  "A TANGO exception was raised: '%s'", (const char *)e.errors[0].desc); */
    /* } */

    /* gboolean transfer_async = FALSE; */
    /* g_object_get(G_OBJECT (camera), */
    /*         "transfer-asynchronously", &transfer_async, */
    /*         NULL); */

    /* if (transfer_async) { */
    /*     priv->thread_running = FALSE; */
    /*     g_thread_join (priv->grab_thread); */
    /* } */

    /* kiro_sb_freeze (priv->receive_buffer); */
    /* g_free (priv->dummy_data); */
}

static void
uca_kiro_camera_trigger (UcaCamera *camera, GError **error)
{
}

static gboolean
uca_kiro_camera_grab (UcaCamera *camera, gpointer data, GError **error)
{
    /* g_return_val_if_fail (UCA_IS_KIRO_CAMERA (camera), FALSE); */

    /* UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */

    /* //This is a hack to make sure we actually wait for a new frame; */
    /* gpointer frame = kiro_sb_get_data_blocking (priv->receive_buffer); */

    /* kiro_sb_freeze (priv->receive_buffer); */
    /* //Element 0 might still be in the process of being written. */
    /* //Therefore, we take Element 1, to be sure this one is finished. */
    /* if (data) */
    /*     g_memmove (data, frame, priv->roi_width * priv->roi_height * priv->bytes_per_pixel); */
    /* kiro_sb_thaw (priv->receive_buffer); */

    return TRUE;
}


static gboolean
kiro_address_decode (const gchar *addr_in, gchar **addr, gchar **port, GError **error)
{
    if (!g_str_has_prefix (addr_in, "kiro://")) {
        g_set_error_literal (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT,
                             "Address does not use 'kiro://' scheme.");
        return FALSE;
    }

    /* Pitfall: kiro will silently accept hostnames like kiro://localhost:5555
     * but not bind to it as it treats it like an interface name (like eth0).
     * We have to use IP addresses instead of DNS names.
     */
    gchar *host = g_strdup (&addr_in[7]);

    if (!g_ascii_isdigit (host[0]) && host[0] != '*')
        g_debug ("Treating address %s as interface device name. Use IP address if supplying a host was intended.", host);

    gchar **split = g_strsplit (host, ":", 2);

    if (!g_ascii_isdigit (*split[1])) {
            g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT,
                         "Address '%s' has wrong format", addr_in);
            g_strfreev (split);
            g_free (host);
            return FALSE;
    }

    *addr = g_strdup (split[0]);
    *port = g_strdup (split[1]);

    g_strfreev (split);
    g_free (host);
    return TRUE;
}


static void
null_callback (gpointer unused)
{
    (void)unused;
}




void
receive_handler (KiroRequest *request, gpointer user_data)
{
    UcaKiroCamera *cam = (UcaKiroCamera *)user_data;
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (cam);

    KiroMessage *msg = request->message;

    if (msg->msg == KIROCS_INSTALL) {
        PropertyRequisition *req = (PropertyRequisition *)msg->payload;
        g_debug ("Got requisition for a non-base property '%s' of type '%s' with ID %u", req->name, g_type_name (req->value_type),
                req->id);
        priv->property_install_list = g_list_append (priv->property_install_list, msg->payload);
        msg->payload = NULL;
        goto done;
    }

    if (msg->msg == KIROCS_READY) {
        g_debug ("Interface Setup Done.");
        priv->kiro_connected = TRUE;
        goto done;
    }

    if (msg->msg == KIROCS_UPDATE) {
        PropUpdate *update = (PropUpdate *)request->message->payload;
        const gchar *name = NULL;
        gpointer buffer = NULL;

        if (update->scalar == TRUE) {
            PropUpdateScalar *scalar_update = (PropUpdateScalar *)request->message->payload;

            if (update->id >= N_BASE_PROPERTIES) {
                name = priv->kiro_dynamic_scalar_properties[update->id - N_BASE_PROPERTIES].pspec->name;
                buffer = &priv->kiro_dynamic_scalar_properties[update->id - N_BASE_PROPERTIES].buffer;
            }
            else {
                name = uca_camera_props[update->id];
                buffer = &kiro_scalar_prop_buffers[update->id];
            }

            memcpy (buffer, &scalar_update->prop_raw, sizeof (guint64));
        }

        g_debug ("Peer informed us about an update of property '%s' (ID: %u)", name, update->id);
        goto done;
    }

    g_message ("Message Type '%u' is unhandled.", msg->msg);

done:
    if (msg->payload)
        g_free (msg->payload);
    g_free (msg);
    kiro_messenger_receive (priv->messenger, request);
}


static void
uca_kiro_camera_clone_interface(UcaKiroCamera *kiro_camera)
{
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (kiro_camera);
    UcaKiroCameraClass *klass = UCA_KIRO_CAMERA_GET_CLASS (kiro_camera);
    GObjectClass *gobject_class = G_OBJECT_CLASS (klass);

    if (priv->messenger) {
        kiro_messenger_free (priv->messenger);
        priv->messenger = kiro_messenger_new ();
    }

    priv->rec_request = g_malloc0 (sizeof (KiroRequest));
    priv->rec_request->id = 0;
    priv->rec_request->callback = (KiroMessageCallbackFunc) receive_handler;
    priv->rec_request->user_data = (gpointer) kiro_camera;
    kiro_messenger_receive (priv->messenger, priv->rec_request);

    priv->kiro_connected = FALSE;
    kiro_messenger_connect (priv->messenger, priv->kiro_address, priv->kiro_port, &priv->peer_rank, &initable_iface_error);
    if (initable_iface_error) {
        priv->construction_error = TRUE;
        g_free (priv->rec_request);
        return;
    }

    //Wait until the remote side has given us the "READY" signal
    while (!priv->kiro_connected) {};

    guint count = g_list_length (priv->property_install_list);
    if (count > 0) {
        g_debug ("Registering buffers for %u dyanmic properties", count);
        priv->kiro_dynamic_scalar_properties = g_malloc0 (count * sizeof (KiroDynamicScalarProperty));
        GList *curr = g_list_first (priv->property_install_list);
        guint idx = 0;
        while (curr) {
            PropertyRequisition *req = (PropertyRequisition *)curr->data;
            g_debug ("Registering dynamic property '%s'", req->name);
            GParamSpec *pspec = g_param_spec_boolean (req->name, "Remote Property",
                                                      "Remote Property", TRUE, G_PARAM_READWRITE);
            guint local_id = N_PROPERTIES + idx;
            g_object_class_install_property (gobject_class, local_id, pspec);

            priv->kiro_dynamic_scalar_properties[idx].local_id = local_id;
            priv->kiro_dynamic_scalar_properties[idx].remote_id = req->id;
            priv->kiro_dynamic_scalar_properties[idx].pspec = pspec;

            KiroMessage message;
            message.msg = KIROCS_FETCH;
            message.size = strlen (req->name) + 1; //Don't forget the NULL-byte
            message.payload = &(req->name);

            g_debug ("Sending request to fetch value of property '%s'", req->name);
            GError *error = NULL;
            kiro_messenger_send_blocking (priv->messenger, &message, priv->peer_rank, &error);
            if (error) {
                g_error ("Oh shit! (%s)", error->message);
                g_error_free (error);
            }

            idx++;
            curr = g_list_next (curr);
        }
        g_list_free (priv->property_install_list);
    }



    if (priv->construction_error) {
        kiro_messenger_stop (priv->messenger);

        //TODO
        //Maybe set an error?
    }
}


static void
uca_kiro_camera_set_property(GObject *object, guint property_id, const GValue *value, GParamSpec *pspec)
{
    g_return_if_fail(UCA_IS_KIRO_CAMERA (object));
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (object);

    switch (property_id) {
        case PROP_KIRO_ADDRESS:
            if (priv->kiro_address_string)
                g_free (priv->kiro_address_string);
            priv->kiro_address_string  = g_value_dup_string (value);
            break;
        default:
            g_debug ("Updating %s.", pspec->name);

            if (!priv->kiro_connected) {
                g_warning ("Trying to modify a property before a connection to the remote camera was established.");
                G_OBJECT_WARN_INVALID_PROPERTY_ID(object, property_id, pspec);
                return;
            }

            if (property_id >= N_PROPERTIES) {
                g_debug ("Non-Base-Property ID %u (Index: %u)", property_id, property_id - N_PROPERTIES);
                g_value_write_to_raw_data (value, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer));
            }
            else
                g_value_write_to_raw_data (value, &kiro_scalar_prop_buffers[property_id]);

            KiroMessage message;
            message.msg = KIROCS_UPDATE;
            message.size = sizeof (PropUpdate) + sizeof (guint64);
            message.payload = g_malloc0 (message.size);
            gpointer data_pointer = message.payload + sizeof (PropUpdate);

            PropUpdate *update = (PropUpdate *)message.payload;
            update->size = 1;
            update->scalar = TRUE;

            if (property_id > N_BASE_PROPERTIES) {
                update->id = priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].remote_id;
                memcpy (data_pointer, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer), sizeof (guint64));
            }
            else {
                update->id = property_id;
                memcpy (data_pointer, &kiro_scalar_prop_buffers[property_id], sizeof (guint64));
            }


            GError *error = NULL;
            kiro_messenger_send_blocking (priv->messenger, &message, priv->peer_rank, &error);
            if (error) {
                g_error ("Oh shit! (%s)", error->message);
                g_error_free (error);
            }
            g_free (message.payload);
    }
}


static void
uca_kiro_camera_get_property(GObject *object, guint property_id, GValue *value, GParamSpec *pspec)
{
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (object);

    switch (property_id) {
        case PROP_NAME:
            g_value_set_string (value, "KIRO camera");
            break;
        case PROP_KIRO_ADDRESS:
            g_value_set_string (value, priv->kiro_address_string);
            break;
        case PROP_KIRO_REMOTE_NAME:
            g_value_set_string (value, priv->remote_name);
            break;
        default:
            //TODO:
            //Handle non-scalar types specifically
            if (property_id <= N_BASE_PROPERTIES) {
                g_value_set_from_raw_data (value, &kiro_scalar_prop_buffers[property_id]);
            }
            else {
                g_value_set_from_raw_data (value, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer));
            }
            break;
    }
}

static void
uca_kiro_camera_finalize(GObject *object)
{
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE(object);

    if (priv->thread_running) {
        priv->thread_running = FALSE;
        g_thread_join (priv->grab_thread);
    }

    if (priv->messenger) {
        kiro_messenger_free (priv->messenger);
        priv->messenger = NULL;
    }
    priv->kiro_connected = FALSE;

    if (priv->dummy_data) {
        g_free (priv->dummy_data);
        priv->dummy_data = NULL;
    }

    g_free (priv->kiro_address_string);
    g_free (priv->kiro_address);
    g_free (priv->kiro_port);

    G_OBJECT_CLASS (uca_kiro_camera_parent_class)->finalize(object);
}

static gboolean
ufo_kiro_camera_initable_init (GInitable *initable,
                               GCancellable *cancellable,
                               GError **error)
{
    g_return_val_if_fail (UCA_IS_KIRO_CAMERA (initable), FALSE);

    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (UCA_KIRO_CAMERA (initable));
    if(priv->construction_error) {
        g_propagate_error (error, initable_iface_error);
        return FALSE;
    }

    return TRUE;
}

static void
uca_kiro_initable_iface_init (GInitableIface *iface)
{
    iface->init = ufo_kiro_camera_initable_init;
}

static void
uca_kiro_camera_constructed (GObject *object)
{
    //Initialization for the KIRO Messenger and interface cloning is moved
    //here and done early!
    //We want to add dynamic properties and it is too late to do so in the
    //real initable part. Therefore, we do it here and 'remember' any errors
    //that occur and check them later in the initable part.

    UcaKiroCamera *self = UCA_KIRO_CAMERA (object);
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (self);
    priv->construction_error = FALSE;

    GValue address = G_VALUE_INIT;
    g_value_init(&address, G_TYPE_STRING);
    uca_kiro_camera_get_property (object, PROP_KIRO_ADDRESS, &address, NULL);

    const gchar *addrstring = g_value_get_string (&address);
    gint address_not_none = g_strcmp0(addrstring, "NONE");

    if (0 == address_not_none) {
        g_warning ("kiro-address was not set! Can not connect to server...\n");
        priv->construction_error = TRUE;
        g_set_error (&initable_iface_error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_MISSING_ADDRESS,
             "'kiro-address' property was not set during construction.");
    }
    else {
        if (kiro_address_decode (addrstring, &priv->kiro_address, &priv->kiro_port, &initable_iface_error)) {
            uca_kiro_camera_clone_interface (self);
        }
        else {
            priv->construction_error = TRUE;
        }
    }

    G_OBJECT_CLASS (uca_kiro_camera_parent_class)->constructed(object);
}



static void
uca_kiro_camera_class_init(UcaKiroCameraClass *klass)
{
    GObjectClass *gobject_class = G_OBJECT_CLASS( klass);
    gobject_class->set_property = uca_kiro_camera_set_property;
    gobject_class->get_property = uca_kiro_camera_get_property;
    gobject_class->finalize = uca_kiro_camera_finalize;
    gobject_class->constructed = uca_kiro_camera_constructed;

    UcaCameraClass *camera_class = UCA_CAMERA_CLASS (klass);
    camera_class->start_recording = uca_kiro_camera_start_recording;
    camera_class->stop_recording = uca_kiro_camera_stop_recording;
    camera_class->grab = uca_kiro_camera_grab;
    camera_class->trigger = uca_kiro_camera_trigger;

    for (guint i = 0; i < N_BASE_PROPERTIES; i++)
        g_object_class_override_property (gobject_class, i, uca_camera_props[i]);

    kiro_properties[PROP_KIRO_ADDRESS] =
        g_param_spec_string("kiro-address",
                "KIRO Server Address",
                "Address of the KIRO Server to grab images from",
                "NONE",
                G_PARAM_READWRITE | G_PARAM_CONSTRUCT);

    kiro_properties[PROP_KIRO_REMOTE_NAME] =
        g_param_spec_string("remote-name",
                "Name of the remote camera",
                "Name of the camera plugin that is loaded on the KIRO remote side",
                "NONE",
                G_PARAM_READABLE);

    for (guint id = N_BASE_PROPERTIES; id < N_PROPERTIES; id++)
        g_object_class_install_property (gobject_class, id, kiro_properties[id]);

    g_type_class_add_private (klass, sizeof(UcaKiroCameraPrivate));
}

static void
uca_kiro_camera_init(UcaKiroCamera *self)
{
    self->priv = UCA_KIRO_CAMERA_GET_PRIVATE(self);
    self->priv->grab_thread = NULL;
    self->priv->current_frame = 0;
    self->priv->kiro_address_string = g_strdup ("NONE");
    self->priv->remote_name = g_strdup ("NONE");
    self->priv->construction_error = FALSE;
    self->priv->kiro_dynamic_scalar_properties = NULL;

    self->priv->messenger = kiro_messenger_new ();
    self->priv->peer_rank = 0;
}


G_MODULE_EXPORT GType
uca_camera_get_type (void)
{
    return UCA_TYPE_KIRO_CAMERA;
}