#include <camunits/plugin.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include <GL/gl.h>
#include <GL/glut.h>

#include <linuxcnc/config.h>
#include <linuxcnc/hal.h>
#include "hal_priv.h"

// This file demonstrates how to create a simple CamUnit plugin.  When
// implementing a custom MyUnit, replace "HalIOPlugin", "HAL_IO_PLUGIN",
// and "hal_io_plugin" with your own names.

typedef struct _HalIOPlugin HalIOPlugin;
typedef struct _HalIOPluginClass HalIOPluginClass;

// This macro is a type-safe alternative to "(HalIOPlugin*)obj"
// If obj is not a pointer to HalIOPlugin, then a warning message
// is printed to stderr, and the macro returns NULL.  It's reasonable to delete
// this macro if you don't need it.
#define HAL_IO_PLUGIN(obj)  (G_TYPE_CHECK_INSTANCE_CAST( (obj), \
        hal_io_plugin_get_type(), HalIOPlugin))

#define AXIS_MAX 9
static char h_comp_name[256] = {0};
static int  h_comp_id = -1;
static int  h_flag = 0;
static int  h_done = 0;
static int _glut_init = 0;

static hal_float_t *pin_axis[AXIS_MAX];
static hal_s32_t *pin_circle_x;
static hal_s32_t *pin_circle_y;
static hal_s32_t *pin_circle_r;

static void _hal_quit(int sig)
{
    if ( h_flag ) {
	/* this process might have the hal mutex, so just set the
	   'done' flag and return, exit after mutex work finishes */
	h_done = 1;
    } else {
	/* don't have to worry about the mutex, but if we just
	   return, we might return into the fgets() and wait
	   all day instead of exiting.  So we exit from here. */
	if ( h_comp_id > 0 ) {
	    hal_exit(h_comp_id);
	}
	exit(1);
    }
}

static void * hal_get_pin(const char *name, hal_type_t type)
{
    hal_pin_t *pin;
    void *data = 0;

    if (!hal_data) {
	printf("Hal data not present\n");
	return 0;
    }

    printf("getting parameter '%s'\n", name);
    /* get mutex before accessing shared data */
    printf("Getting mutex %p\n", hal_data);
    rtapi_mutex_get(&(hal_data->mutex));
    printf("Got mutex\n");
    pin = halpr_find_pin_by_name(name);
    printf("Got pin %s: %p\n", name, pin);
    if (!pin) goto out;
        /* found it */
    printf("Got pin type %d: (must be %d)\n", pin->type, type);
    if (pin->type != type) goto out;

    printf("Got pin signal %p\n", (void *) pin->signal);
    if (pin->signal) {
        hal_sig_t * sig = SHMPTR(pin->signal);
        data = SHMPTR(sig->data_ptr);
    } else
        data = &pin->dummysig;

out:
    printf("Release mutex\n");
    rtapi_mutex_give(&(hal_data->mutex));
    printf("Released\n");
    if (!data)
        printf("parameter '%s' not found\n", name);
    return data;
}

static void _hal_setup_pins(const char * axis, const char *circle)
{
    char name[256];
    int i;
    printf("Setting up pins for [%s]\n", circle);
    for (i = 0; i < AXIS_MAX; i++) {
        if (!axis) {
            pin_axis[i] = NULL;
            continue;
        }
        printf("Printing axis name: %s %d", axis, i);
        snprintf(name, 255, axis, i);
        pin_axis[i] = hal_get_pin(name, HAL_FLOAT);
        printf("Axis %d: %p\n", i, pin_axis[i]);
    }

    pin_circle_x = 0;
    pin_circle_y = 0;
    pin_circle_r = 0;
    if (!circle) return;
    snprintf(name, 255, "%s.x", circle);
    pin_circle_x = hal_get_pin(name, HAL_S32);
    snprintf(name, 255, "%s.y", circle);
    pin_circle_y = hal_get_pin(name, HAL_S32);
    snprintf(name, 255, "%s.r", circle);
    pin_circle_r = hal_get_pin(name, HAL_S32);
}

static void _hal_init() {
    char *argv[] = {""};
    int  pargc = 0;
    printf("Running glut init\n");
    if (!_glut_init)
        glutInit(&pargc, argv);
    _glut_init = 1;

    if (h_comp_id >= 0) return;
    //signal(SIGINT, _hal_quit);
    //signal(SIGTERM, _hal_quit);
    //signal(SIGPIPE, SIG_IGN);

    snprintf(h_comp_name, sizeof(h_comp_name), "cu-filter-%d", getpid());
    h_comp_id = hal_init(h_comp_name);

    _hal_setup_pins(NULL, NULL);
}

// Class definition struct.  member variables go in here
struct _HalIOPlugin {
    // the first member must always be the superclass struct
    CamUnit parent;

    // add one member variable
    CamUnitControl *ctl_limit;
    CamUnitControl *ctl_reload;
    CamUnitControl *ctl_circle;
    CamUnitControl *ctl_spacing;
    CamUnitControl *ctl_r, *ctl_g, *ctl_b;
    CamUnitControl *ctl_xor;
    CamUnitControl *ctl_axis_pins;
    CamUnitControl *ctl_axis_letter;
    CamUnitControl *ctl_axis_format;

    int circle_x;
    int circle_y;
    int circle_r;
};

// class definition.  This is pretty much a vtable, and you will rarely need to
// change it
struct _HalIOPluginClass {
    CamUnitClass parent_class;
};

GType hal_io_plugin_get_type (void);

HalIOPlugin * hal_io_plugin_new()
{
    return HAL_IO_PLUGIN(g_object_new(hal_io_plugin_get_type(), NULL));
}

// magic macro.
CAM_PLUGIN_TYPE (HalIOPlugin, hal_io_plugin, CAM_TYPE_UNIT);

// These next two functions are required as entry points for the
// Camunits plug-in API.
void cam_plugin_initialize (GTypeModule * module)
{
    // this function is defined by the magic macro above, and registers
    // the plugin with the GObject type system.
    hal_io_plugin_register_type (module);
}

CamUnitDriver * cam_plugin_create (GTypeModule * module)
{
    _hal_init();
    return cam_unit_driver_new_stock_full ("emc", "halio",
            "Read/write data to HAL pins", CAM_UNIT_RENDERS_GL,
            (CamUnitConstructor)hal_io_plugin_new, module);
}

// ============== HalIOPlugin ===============
static void _finalize (GObject *obj);
static gboolean try_set_control (CamUnit *super,
        const CamUnitControl *ctl, const GValue *proposed, GValue *actual);
static void on_input_frame_ready (CamUnit * super, const CamFrameBuffer *inbuf,
        const CamUnitFormat *infmt);
static void on_input_format_changed (CamUnit *super,
        const CamUnitFormat *infmt);
static int _gl_draw_gl (CamUnit *super);

// Class initializer.  This function sets up the class vtable, and is most
// commonly used for overriding superclass methods.
static void
hal_io_plugin_class_init (HalIOPluginClass *klass)
{
    // override the destructor
    G_OBJECT_CLASS (klass)->finalize = _finalize;

    klass->parent_class.on_input_frame_ready = on_input_frame_ready;
    klass->parent_class.try_set_control = try_set_control;
    klass->parent_class.draw_gl = _gl_draw_gl;
}

// Initializer.  This is essentially a constructor.
static void
hal_io_plugin_init (HalIOPlugin *self)
{
    // Initialize the unit with some reasonable defaults here.
    CamUnit *super = CAM_UNIT (self);

    // create a control
    self->ctl_limit = cam_unit_add_control_int (super,
            "limit", "Limit to first N axis", 0, AXIS_MAX, 1, 3, 1);

    self->ctl_reload = cam_unit_add_control_boolean (super,
            "reload", "Reload PIN configuration", 1, 1);

    self->ctl_circle = cam_unit_add_control_string (super,
            "circle", "Base name for Circle pins", "video.circle", 1);

    self->ctl_axis_pins = cam_unit_add_control_string (super,
            "axis-pins", "Base name for Axis pins", "halui.axis.%d.pos-feedback", 1);

    self->ctl_axis_letter = cam_unit_add_control_string (super,
            "axis-letters", "Letters for Axes (9 chars min)", "012345678", 1);

    self->ctl_axis_format = cam_unit_add_control_string (super,
            "axis-format", "Format string for Axis (float)", "%.3f", 1);

    self->ctl_spacing = cam_unit_add_control_int (super,
            "spacing", "Spacing between axis lines", 0, 100, 1, 20, 1);

    self->ctl_r = cam_unit_add_control_float (super, "color-r", "R component of color",
                                            0, 1, 0.01, 1, 1);
    self->ctl_g = cam_unit_add_control_float (super, "color-g", "G component of color",
                                            0, 1, 0.01, 1, 1);
    self->ctl_b = cam_unit_add_control_float (super, "color-b", "B component of color",
                                            0, 1, 0.01, 1, 1);

    self->ctl_xor = cam_unit_add_control_boolean (super,
            "gl-xor", "Enable logic XOR with image", 1, 1);

    _hal_setup_pins( cam_unit_control_get_string(self->ctl_axis_pins),
                     cam_unit_control_get_string(self->ctl_circle));
    // request notification when the input of the unit changes
    g_signal_connect (G_OBJECT(self), "input-format-changed",
            G_CALLBACK(on_input_format_changed), NULL);
}

static gboolean
try_set_control (CamUnit *super,
        const CamUnitControl *ctl, const GValue *proposed, GValue *actual)
{
    HalIOPlugin *self = HAL_IO_PLUGIN(super);

    if (ctl == self->ctl_axis_letter) {
        const char * str = g_value_get_string(proposed);
        if (strlen(str) < AXIS_MAX) {
            printf("Letters are too small %d < %d: %s\n", strlen(str), AXIS_MAX, str);
            return FALSE;
        }
    } else if (ctl == self->ctl_circle) {
        _hal_setup_pins( cam_unit_control_get_string(self->ctl_axis_pins),
                         g_value_get_string(proposed));
    } else if (ctl == self->ctl_axis_pins) {
        _hal_setup_pins( g_value_get_string(proposed),
                         cam_unit_control_get_string(self->ctl_circle));
    } else if (ctl == self->ctl_reload) {
        if (g_value_get_boolean (proposed))
            _hal_setup_pins( cam_unit_control_get_string(self->ctl_axis_pins),
                             cam_unit_control_get_string(self->ctl_circle));
        g_value_set_boolean(actual, FALSE);
        return TRUE;
    }

    g_value_copy(proposed, actual);
    return TRUE;
}

// destructor.
static void
_finalize (GObject *obj)
{
    // If we allocated memory on the heap/freestore, we'd release it here

    // invoke the superclass destructor.  hal_io_plugin_parent_class is
    // defined by the magic macro
    G_OBJECT_CLASS (hal_io_plugin_parent_class)->finalize(obj);
}

// this is the signal handler attached in "hal_io_plugin_init", and is
// called when the format of the input data changes.
static void
on_input_format_changed (CamUnit *super, const CamUnitFormat *infmt)
{
    // first, clear all available output formats from this unit
    cam_unit_remove_all_output_formats (super);

    // If there is no input, then we can't produce output.
    if (!infmt) return;

    cam_unit_add_output_format (super, infmt->pixelformat,
            infmt->name, infmt->width, infmt->height,
            infmt->row_stride);
}

// this method is called whenever the input unit produces a frame
static void
on_input_frame_ready (CamUnit *super, const CamFrameBuffer *inbuf,
        const CamUnitFormat *infmt)
{
    HalIOPlugin *self = HAL_IO_PLUGIN(super);

    cam_unit_produce_frame(super, inbuf, infmt);

    self->circle_x = self->circle_y = self->circle_r = 0;
    uint8_t * ptr = 0;
    int len;
    ptr = cam_framebuffer_metadata_get(inbuf, "marker:x", &len);
    if (ptr && len == 4) self->circle_x = *(int *)ptr;
    ptr = cam_framebuffer_metadata_get(inbuf, "marker:y", &len);
    if (ptr && len == 4) self->circle_y = *(int *)ptr;
    ptr = cam_framebuffer_metadata_get(inbuf, "marker:r", &len);
    if (ptr && len == 4) self->circle_r = *(int *)ptr;

    self->circle_x -= infmt->width/2;
    self->circle_y -= infmt->height/2;

    if (pin_circle_x) *pin_circle_x = self->circle_x;
    if (pin_circle_y) *pin_circle_y = self->circle_y;
    if (pin_circle_r) *pin_circle_r = self->circle_r;
}

static int
_gl_draw_gl (CamUnit *super)
{
    HalIOPlugin *self = HAL_IO_PLUGIN(super);
    const CamUnitFormat *outfmt = cam_unit_get_output_format(super);
    if (!outfmt) return 0;
    char buf[256], buf_format[256];
    int limit = cam_unit_control_get_int (self->ctl_limit);
    int spacing = cam_unit_control_get_int (self->ctl_spacing);
    const char *letters = cam_unit_control_get_string (self->ctl_axis_letter);
    const char *format = cam_unit_control_get_string (self->ctl_axis_format);
    int i, y = 10;

    float r = cam_unit_control_get_float(self->ctl_r);
    float g = cam_unit_control_get_float(self->ctl_g);
    float b = cam_unit_control_get_float(self->ctl_b);


    glMatrixMode (GL_PROJECTION);
    glDisable(GL_COLOR_LOGIC_OP);

    int w= 1280;
    int h = 720;

    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0,w, 0, h);
    glScalef(1, -1, 1);
    glTranslatef(0, -h, 0);
    glMatrixMode(GL_MODELVIEW);

    char *font = GLUT_BITMAP_HELVETICA_18;
    int x1, y1, x2, y2;
    int font_spacing = spacing + glutBitmapHeight(font);
    y = font_spacing;

    x1 = 0;
    y1 = 0;
    x2 = 170;
    y2 = y + font_spacing * limit;

    glColor3f(0, 0, 0);

    glBegin(GL_QUADS);
    glVertex2f(x1, y1);
    glVertex2f(x2, y1); 
    glVertex2f(x2, y2);
    glVertex2f(x1, y2); 
    glEnd();

    glColor3f(r, g, b);

    if (self->circle_r > 0) {
        glRasterPos2i(0, y);
        snprintf(buf, 255, "Circle %d %d %d", self->circle_x, self->circle_y, self->circle_r);
        glutBitmapString(font, buf);
        y += font_spacing;
    }

    for (i = 0; i < limit; i++) {
        if (!pin_axis[i]) continue;
        glRasterPos2i(0, y);
        snprintf(buf_format, 255, "Axis %%c: %s", format);
        snprintf(buf, 255, buf_format, letters[i], (pin_axis[i])?(*pin_axis[i]):0);
        glutBitmapString(font, buf);
        y += font_spacing;
    }

    glEnd ();
   
    return 0;
}

/*
 vim: sts=4 sw=4 et
 */