path: root/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, };

struct _UcaKiroCameraPrivate {
    guint8 *dummy_data;
    guint current_frame;
    gchar *kiro_address_string;
    gchar *kiro_address;
    gchar *kiro_port;
    gchar *remote_name;
    GParamSpec **kiro_dynamic_attributes;

    gboolean thread_running;
    gboolean kiro_connected;
    gboolean construction_error;

    GThread *grab_thread;
    KiroMessenger *messenger;
    gulong peer_rank;

    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 KiroContinueFlag
receive_handler (KiroMessageStatus *status, gpointer user_data)
{
    UcaKiroCamera *cam = (UcaKiroCamera *)user_data;
    UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (cam);
    
    KiroMessage *msg = status->message;

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

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


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->kiro_connected = FALSE;

    kiro_messenger_add_receive_callback (priv->messenger, receive_handler, kiro_camera);
    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;
        kiro_messenger_remove_receive_callback (priv->messenger);
        return;
    }

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

    if (priv->construction_error) {
        //something went wrong. Tear down the connection.
        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;
            }

            GError *error = NULL;

            GVariant *tmp = variant_from_scalar (value);
            gsize data_size = g_variant_get_size (tmp);

            PropUpdate *test = g_malloc0 (sizeof (PropUpdate) + data_size);
            test->id = property_id_from_name (pspec->name);
            test->type[0] = gtype_to_gvariant_class (pspec->value_type);
            test->size = data_size;
            g_variant_store (tmp, test->val);
            g_variant_unref (tmp);

            KiroMessage message;
            message.peer_rank = priv->peer_rank;
            message.msg = KIROCS_UPDATE;
            message.payload = test;
            message.size = sizeof (PropUpdate) + data_size;

            kiro_messenger_send_blocking (priv->messenger, &message, &error);
            if (error) {
                g_free (test);
                g_error ("Oh shit! (%s)", error->message);
            }

            g_free (test);
    }
}


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:
            //try_handle_read_tango_property (object, property_id, value, pspec);
            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 Server and TANGO 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 remot camera",
                "Name of the camera plugin that is loaded on the KIRO remote site",
                "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->kiro_address = g_strdup ("NONE");
    self->priv->kiro_port = g_strdup ("NONE");
    self->priv->remote_name = g_strdup ("NONE");
    self->priv->construction_error = FALSE;
    self->priv->kiro_dynamic_attributes = 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;
}