#include <cairo/cairo.h>
#include <stdlib.h>
#include <math.h>

#include "render.h"

unsigned char *displayData = NULL;

static void draw_arrow(cairo_t *cr, enum direction dir)
{
  cairo_move_to(cr, 0, 0);
  cairo_line_to(cr, 0, 1);
  cairo_line_to(cr, 1, 1);
  cairo_line_to(cr, 1, 0);

  cairo_close_path (cr);
  
  switch ( dir )
    {
    case DIR_UP:
      if ( st->mot_l > 0 && st->mot_r > 0 )
	cairo_set_source_rgb (cr, 0, 0, 1);
      else
	cairo_set_source_rgb (cr, 0, 0, 0.2);
      break;

    case DIR_DOWN:
      if ( st->mot_l < 0 || st->mot_r < 0 )
	cairo_set_source_rgb (cr, 0, 0, 1);
      else
	cairo_set_source_rgb (cr, 0, 0, 0.2);
      break;

    case DIR_LEFT:
      if ( st->mot_l < st->mot_r )
	cairo_set_source_rgb (cr, 0, 0, 1);
      else
	cairo_set_source_rgb (cr, 0, 0, 0.2);
      break;

    case DIR_RIGHT:
      if ( st->mot_l > st->mot_r )
	cairo_set_source_rgb (cr, 0, 0, 1);
      else
	cairo_set_source_rgb (cr, 0, 0, 0.2);
      break;

    default:
      cairo_set_source_rgb (cr, 0.2, 0.2, 0.2);
      break;
    }
 
  cairo_fill (cr);
}

static void draw_prox_sensors(cairo_t *cr)
{
  cairo_save(cr);

  int i;

  cairo_translate(cr, 0.75, 0);
  cairo_scale(cr, 0.3, 1);

  cairo_pattern_t *pat = cairo_pattern_create_linear (0.0, 0.0,  0.0, 1.0);
  cairo_pattern_add_color_stop_rgba (pat, 1, 1, 0, 0, 1);
  cairo_pattern_add_color_stop_rgba (pat, 0, 0, 1, 0, 1);

  for(i=0;i<KHEP_SENS_COUNT;i++)
    {
      float puiss = (prox_sens[i] + sinf(random())*5)/1000.0;
      if ( puiss < 0 )
	puiss = 0;

      cairo_rectangle(cr,
		      0, 0.1,
		      0.09, puiss);
      cairo_set_source(cr, pat);
      cairo_fill(cr);

      cairo_set_source_rgb(cr, 1, 1, 1);
      cairo_stroke(cr);

      cairo_translate(cr, 0.1, 0);

    }

  cairo_pattern_destroy(pat);
		  
  cairo_restore(cr);
}


static void draw_ambient_sensors(cairo_t *cr)
{
  cairo_save(cr);

  int i;

  cairo_scale(cr, 0.3, 1);
  cairo_translate(cr, 0.01, 0);

  cairo_pattern_t *pat = cairo_pattern_create_linear (0.0, 0.0,  0.0, 1.0);
  cairo_pattern_add_color_stop_rgba (pat, 1, 1, 0, 0, 1);
  cairo_pattern_add_color_stop_rgba (pat, 0, 0, 0, 1, 1);

  for(i=0;i<KHEP_SENS_COUNT;i++)
    {
      float puiss = 1-((amb_sens[i] + sinf(random())*5)/500.0);
      if ( puiss < 0 )
	puiss = 0;

      cairo_rectangle(cr,
		      0, 0.1,
		      0.09, puiss);
      cairo_set_source(cr, pat);
      cairo_fill(cr);

      cairo_set_source_rgb(cr, 1, 1, 1);
      cairo_stroke(cr);

      cairo_translate(cr, 0.1, 0);

    }

  cairo_pattern_destroy(pat);
		  
  cairo_restore(cr);
}

static void draw_motor_speed(cairo_t *cr)
{
  cairo_save(cr);

  int i;

  cairo_translate(cr, 0.20, 0.85);
  cairo_scale(cr, 0.5, 0.3);

  cairo_rotate(cr, -M_PI/2);


  cairo_pattern_t *pat = cairo_pattern_create_linear (0.0, 0.0,  0.0, 1.0);
  cairo_pattern_add_color_stop_rgba (pat, 1, 1, 1, 1, 1);
  cairo_pattern_add_color_stop_rgba (pat, 0, 0.4, 0.2, 0.8, 1);

  for(i=0;i<KHEP_MOT_COUNT;i++)
    {
      // not moving
      if ( st->mot_l == 0 ||
	   st->mot_r == 0 )
	continue;

      // moving
      float puiss = fabs(motor_speed[i]/khep_speed);
      if ( puiss > 1 )
	puiss = 1;

      cairo_rectangle(cr,
		      0, 0.1,
		      0.09, 1 - puiss);
      cairo_set_source(cr, pat);
      cairo_fill(cr);

      cairo_set_source_rgb(cr, 1, 1, 1);
      cairo_stroke(cr);

      cairo_translate(cr, 0.1, 0);

    }

  cairo_pattern_destroy(pat);
		  
  cairo_restore(cr);
}


static void draw_arrow_pad(cairo_t *cr)
{
  int i, j;

  cairo_save(cr);

  cairo_scale(cr, 0.5, 0.5);
  cairo_translate(cr, 0.5, 0.5);

  /* Draw arrows */
  for(i=0;i<3;i++)
    {
      cairo_save(cr);
      cairo_translate(cr, 0, 0.333*i);

      for(j=0;j<3;j++)
	{
	  cairo_save(cr);

	  cairo_translate(cr, 0.333*j, 0);
	  cairo_scale(cr, 0.333, 0.333);

	  draw_arrow(cr, (i*3) + j);

	  cairo_restore(cr);
	}

      cairo_restore(cr);
    }


  /* Draw a border */
  cairo_move_to(cr, 0, 0);
  cairo_line_to(cr, 1, 0);
  cairo_line_to(cr, 1, 1);
  cairo_line_to(cr, 0, 1);
  cairo_close_path (cr);

  cairo_set_source_rgb (cr, 0.3, 0.3, 0.3);
  cairo_stroke (cr);

  cairo_restore(cr);

}

static void draw_title(cairo_t *cr)
{
  /* Draw box title */
  cairo_move_to(cr, 0, 0);
  cairo_line_to(cr, 1, 0);
  cairo_line_to(cr, 1, 0.1);
  cairo_line_to(cr, 0, 0.1);
  cairo_close_path(cr);

  cairo_set_source_rgb (cr, 0.5, 0.5, 0.5);
  cairo_fill(cr);

  /* Draw text title */
  cairo_set_source_rgb(cr, 0.3, 0.0, 0.8);
  cairo_select_font_face(cr, "Georgia",
			 CAIRO_FONT_SLANT_ITALIC,
			 CAIRO_FONT_WEIGHT_NORMAL);

  cairo_set_font_size(cr, 0.05);
  cairo_move_to(cr, 0.02, 0.06);
  cairo_show_text(cr, "Khepera II CC");
}

void draw_scene(cairo_t *cr)
{
  /* draw the entire context white. */
  cairo_set_source_rgba(cr, 0.1, 0.1, 0.1, 1);
  cairo_paint(cr);
  
  /* who doesn't want all those nice line settings :) */
  cairo_set_line_cap(cr, CAIRO_LINE_CAP_ROUND);
  cairo_set_line_width(cr, 0.02);

  draw_title(cr);
  draw_arrow_pad(cr);
  draw_ambient_sensors(cr);
  draw_prox_sensors(cr);
  draw_motor_speed(cr);

}