#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <gtk/gtk.h>
#include <X11/Xlib.h>
#include <unistd.h>
#include <cmath>

#include "callbacks.h"
#include "interface.h"
#include "support.h"

static int screen_offset_x = 0;
static int screen_offset_y = 0;
static double screen_resolution = 1.0; // meter per pixel

void set_screen_offset(int x, int y)
{
  screen_offset_x = x;
  screen_offset_y = y;
}

void set_screen_resolution(double resolution)
{
  screen_resolution = resolution;
}

inline GdkColor set_color(gushort r, gushort g, gushort b,GdkGC *gc)
{
  GdkColor color;

  color.red = r, color.green = g, color.blue = b;
  gdk_color_alloc(gdk_colormap_get_system(),&color);
  gdk_gc_set_foreground(gc, &color);
  return color;
}
inline void set_color2(GdkColor color, GdkGC *gc)
{
  gdk_color_alloc(gdk_colormap_get_system(),&color);
  gdk_gc_set_foreground(gc, &color);
}
GdkColor make_color(gushort r, gushort g, gushort b)
{
  GdkColor color;
  color.red = r; color.green = g; color.blue = b;
  return color;
}

void translate_map_to_screen(Point *dst, Point *src, int width, int height)
{
  //fprintf(stderr, "resolution: %lf\n", screen_resolution);
  //screen_offset_x = (screen_offset_x<0) ? width +screen_offset_x : screen_offset_x;
  //screen_offset_y = (screen_offset_y<0) ? height+screen_offset_y : screen_offset_y;
  dst->x =   screen_offset_x + width *0.5 + src->x/screen_resolution;
  dst->y = - screen_offset_y + height*0.5 - src->y/screen_resolution;
}

void draw_area_initialize(GuiInterface *gui)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;

  gdk_draw_rectangle(pixmap1, drawingarea1->style->white_gc,
                     TRUE, 0, 0, width, height);
  gdk_gc_set_line_attributes(gui->gc, 2, GDK_LINE_SOLID, GDK_CAP_ROUND,
                             GDK_JOIN_ROUND);
}

void draw_point(GuiInterface *gui, Point *pose, int point_size, GdkColor color)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  Point p1;

  translate_map_to_screen(&p1, pose, width, height);
  set_color2(color, gui->gc);
  gdk_draw_arc(pixmap1, gui->gc, 1, p1.x , p1.y,
	       point_size, point_size, 0, 360*64);
}

/**
 * @param gui GUIデータ
 * @param laser LRFデータ
 * @param pose LRFの位置，姿勢[m,rad]
 */
void draw_laser_range(GuiInterface *gui,
		      LaserRangeData *laser, Point *pose)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(gui->window, "pixmap1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  int i;
  double angle;
  Point p1,p2,pose2;
  
  for(i=0; i<laser->total_points; i++) {
    angle = (double)i*laser->resolution + laser->offset_fov + pose->theta;
    p1.x = pose->x + laser->range[i] * cos(angle);
    p1.y = pose->y + laser->range[i] * sin(angle);
    translate_map_to_screen(&p2, &p1, width, height);
    set_color(0xffff, 0x0000, 0x0000, gui->gc);
    gdk_draw_arc(pixmap1, gui->gc, 1, p2.x , p2.y,
 		 3, 3, 0, 360*64);
  }
  // 計測範囲境界線を描画
  translate_map_to_screen(&pose2, pose, width, height);
  set_color(0x0000, 0xffff, 0x0000, gui->gc);

  angle = laser->offset_fov + pose->theta;
  p1.x = pose->x + 100. * cos(angle);
  p1.y = pose->y + 100. * sin(angle);
  translate_map_to_screen(&p2, &p1, width, height);
  gdk_draw_line(pixmap1, gui->gc, pose2.x, pose2.y, p2.x, p2.y);
  //fprintf(stderr, "(%.2f %.2f) ", p1.x, p1.y);
  //fprintf(stderr, "(%.2f %.2f) ", p2.x, p2.y);

  angle = (double)(laser->total_points-1)*laser->resolution + laser->offset_fov + pose->theta;
  p1.x = pose->x + 100. * cos(angle);
  p1.y = pose->y + 100. * sin(angle);
  translate_map_to_screen(&p2, &p1, width, height);
  gdk_draw_line(pixmap1, gui->gc, pose2.x, pose2.y, p2.x, p2.y);
  //fprintf(stderr, "(%.2f %.2f) \n", p1.x, p1.y);
  //fprintf(stderr, "(%.2f %.2f) \n", p2.x, p2.y);
  
}

void show_draw_area(GuiInterface *gui)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  int width = drawingarea1->allocation.width;
  int height = drawingarea1->allocation.height;
  gtk_widget_queue_draw_area(drawingarea1,
			     0, 0,
			     width, height);
}

void get_draw_size(GuiInterface *gui, int *width, int *height)
{
  GtkWidget *drawingarea1 = (GtkWidget *) lookup_widget(gui->window, "drawingarea1");
  *width = drawingarea1->allocation.width;
  *height = drawingarea1->allocation.height;
}

// 以降GTK関連


void
initialize_for_gtk(void)
{
  XInitThreads();
  g_thread_init(NULL);
  gdk_threads_init();

  gtk_set_locale ();
  //gtk_init (&argc, &argv);
  gtk_init (NULL, NULL);
  gdk_rgb_init();
}

GThreadFunc
gtk_main_thread(gpointer data)
{
  gtk_main ();
  return NULL;
}

void
on_window1_destroy                     (GtkObject       *object,
                                        gpointer         user_data)
{
  gtk_main_quit();
}


gboolean
on_drawingarea1_key_press_event        (GtkWidget       *widget,
                                        GdkEventKey     *event,
                                        gpointer         user_data)
{
  //fprintf(stderr,"key_press\n");
  return FALSE;
}


gboolean
on_drawingarea1_button_press_event     (GtkWidget       *widget,
                                        GdkEventButton  *event,
                                        gpointer         user_data)
{
  //fprintf(stderr, "press\n");
  return FALSE;
}


gboolean
on_drawingarea1_button_release_event   (GtkWidget       *widget,
                                        GdkEventButton  *event,
                                        gpointer         user_data)
{
  //fprintf(stderr, "button_release\n");
  return FALSE;
}


gboolean
on_drawingarea1_motion_notify_event    (GtkWidget       *widget,
                                        GdkEventMotion  *event,
                                        gpointer         user_data)
{
  //fprintf(stderr, "motion_notify\n");
  return FALSE;
}


gboolean
on_drawingarea1_expose_event           (GtkWidget       *widget,
                                        GdkEventExpose  *event,
                                        gpointer         user_data)
{
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(widget, "pixmap1");
  if(pixmap1)
    gdk_draw_pixmap(widget->window, widget->style->black_gc,
		    pixmap1,
		    event->area.x, event->area.y,
		    event->area.x, event->area.y,
		    event->area.width,
		    event->area.height);

  return FALSE;
}


gboolean
on_drawingarea1_configure_event        (GtkWidget       *widget,
                                        GdkEventConfigure *event,
                                        gpointer         user_data)
{
  GtkWidget *window1 = (GtkWidget*)lookup_widget(widget, "window1");
  GtkWidget *drawingarea1 = (GtkWidget*)lookup_widget(widget, "drawingarea1");
  GdkPixmap *pixmap1 = (GdkPixmap*)lookup_widget(widget, "pixmap1");

  if(pixmap1) gdk_pixmap_unref(pixmap1);
  pixmap1 = gdk_pixmap_new(widget->window,
			   widget->allocation.width,
			   widget->allocation.height,
			   -1);
  gdk_draw_rectangle(pixmap1, widget->style->white_gc,
                     TRUE, 0, 0, 
		     widget->allocation.width,
		     widget->allocation.height);
  g_object_set_data(G_OBJECT (window1), "pixmap1", pixmap1);
  
  return FALSE;
}

