/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

#include <algorithm>
#include <iostream>

using namespace std;

#include <FL/Fl.H>
#include <FL/Fl_Window.H>
#include <FL/Fl_Double_Window.H>
#include <FL/fl_draw.H>

#include "localization_test.h"
#include "Localization/SRL/Sampler.h"

char* model="ERS7";

bool ProfileMode = false;
bool GoMode = false;
bool Lazy=true;

bool Interactive = !(ProfileMode || GoMode);

static const int width = 800;
static const int height= 600;

static const int NumSamplerTestSamples = 100;

AppWindow *win=NULL;

int RepeatCount=100;

class SImage;
SImage *DebugImage=NULL;

void Canvas::set_size(int _w,int _h)
{
  w = _w;
  h = _h;
}

void Canvas::set_range(vector2f _minv,vector2f _maxv)
{
  minv = _minv;
  maxv = _maxv;
}

int Canvas::transform_x(float x)
{
  float res_x;

  res_x = w*(x - minv.x)/(maxv.x - minv.x);

  return (int)rint(res_x);
}

int Canvas::transform_y(float y)
{
  float res_y;

  res_y = h - h*(y - minv.y)/(maxv.y - minv.y);

  return (int)rint(res_y);
}

int Canvas::scale_x(float x)
{
  float res_x;

  res_x = w*x/(maxv.x - minv.x);

  return (int)rint(res_x);
}

int Canvas::scale_y(float y)
{
  float res_y;

  res_y = h*y/(maxv.y - minv.y);

  return (int)rint(res_y);
}

vector2i Canvas::operator()(vector2f world_coor)
{
  return vector2i(transform_x(world_coor.x),transform_y(world_coor.y));
}

float Canvas::reverse_transform_x(int x)
{
  float res_x;

  res_x = (maxv.x - minv.x)*((double)x)/w + minv.x;

  return res_x;
}

float Canvas::reverse_transform_y(int y)
{
  float res_y;

  res_y = (maxv.y - minv.y)*((double)(h-y))/h + minv.y;

  return res_y;
}

vector2f Canvas::reverse_transform(vector2i screen_coor)
{
  return vector2f(reverse_transform_x(screen_coor.x),reverse_transform_y(screen_coor.y));
}

void Canvas::point(float xc,float yc)
{
  int img_xc,img_yc;

  img_xc = transform_x(xc);
  img_yc = transform_y(yc);

  fl_point(img_xc,img_yc);
}

void Canvas::dot(float xc,float yc,int d)
{
  int img_xc,img_yc;
  int r;

  img_xc = transform_x(xc);
  img_yc = transform_y(yc);
  r = d/2;

  fl_pie(img_xc-r,img_yc-r,d,d,0,360);
}

void Canvas::line(float x1,float y1,float x2,float y2)
{
  int img_x1,img_y1;
  int img_x2,img_y2;

  img_x1 = transform_x(x1);
  img_y1 = transform_y(y1);
  img_x2 = transform_x(x2);
  img_y2 = transform_y(y2);

  fl_line(img_x1,img_y1,img_x2,img_y2);
}

void Canvas::rectf(float x1,float y1,float x2,float y2)
{
  int img_x1,img_y1,img_x2,img_y2;

  img_x1 = transform_x(x1);
  img_y1 = transform_y(y2);
  img_x2 = transform_x(x2);
  img_y2 = transform_y(y1);
  fl_rectf(img_x1,img_y1,img_x2-img_x1,img_y2-img_y1);
}

void Canvas::rectf_wh(float x,float y,float w,float h)
{
  int img_x,img_y,img_w,img_h;

  img_x = transform_x(x);
  img_y = transform_y(y);
  img_w = scale_x(w);
  img_h = scale_y(h);
  fl_rectf(img_x,img_y-img_h,img_w,img_h);
}

void Canvas::circle(float xc,float yc,float r)
{
  int img_xc,img_yc;
  int d_x,d_y;
  int r_x,r_y;

  img_xc = transform_x(xc);
  img_yc = transform_y(yc);
  d_x = scale_x(2.0*r);
  d_y = scale_y(2.0*r);
  r_x = scale_x(r);
  r_y = scale_y(r);

  //fl_arc(img_xc-r_x,img_yc-r_y,d_x,d_y,0,360);

#if 1
  vector2f center(xc,yc);
  static const int num_segments=64;
  for(int j=0; j<num_segments; j++){
    double t1,t2;
    vector2f ind_start,ind_end;
    
    t1 = j*2*M_PI/num_segments;
    t2 = (j+1)*2*M_PI/num_segments;
    ind_start.set(r,0.0);
    ind_start = ind_start.rotate(t1);
    ind_start += center;
    
    ind_end.set(r,0.0);
    ind_end = ind_end.rotate(t2);
    ind_end += center;
    
    line(V2COMP(ind_start),V2COMP(ind_end));
  }
#endif
}

void Canvas::circlef(float xc,float yc,float r)
{
  int img_xc,img_yc;
  int d_x,d_y;
  int r_x,r_y;

  img_xc = transform_x(xc);
  img_yc = transform_y(yc);
  d_x = scale_x(2.0*r);
  d_y = scale_y(2.0*r);
  r_x = scale_x(r);
  r_y = scale_y(r);

  fl_pie(img_xc-r_x,img_yc-r_y,d_x,d_y,0,360);
}

SamplerTester::SamplerTester()
{
}

SamplerTester::~SamplerTester()
{
}

void SamplerTester::init()
{
  sampler.init();
  sampler.samples = new Sample[1000];
  sampler.NumSamples = NumSamplerTestSamples;
  sampler.randomize_samples();
  sampler.init_samples();
}

bool SamplerTester::step(int step_cnt)
{
  sampler.step_samples(step_cnt);

  return false;
}

void SamplerTester::reset()
{
  sampler.randomize_samples();
}

void SamplerTester::draw_data(AppWindow *win,Canvas &canvas)
{
  //canvas.set_range(vector2f(-3000.0,-2500.0),vector2f(3000.0,2500.0));
  canvas.set_range(vector2f(-2500.0,-1900.0),vector2f(2500.0,1900.0));

  fl_color(FL_BLACK);
  fl_rectf(0,0,canvas.w,canvas.h);

  fl_color(FL_GREEN);
  // main field walls
  canvas.line(-halfLength, halfWidth, halfLength, halfWidth);
  canvas.line(-halfLength,-halfWidth, halfLength,-halfWidth);
  canvas.line(-halfLength,-halfWidth,-halfLength, halfWidth);
  canvas.line( halfLength,-halfWidth, halfLength, halfWidth);
  // goal walls
  fl_color(FL_YELLOW);
  canvas.line(-halfLength-goalDepth, -goalHalfWidth, -halfLength-goalDepth, goalHalfWidth);
  canvas.line(-halfLength-goalDepth, -goalHalfWidth, -halfLength, -goalHalfWidth);
  canvas.line(-halfLength-goalDepth,  goalHalfWidth, -halfLength,  goalHalfWidth);
  fl_color(FL_CYAN);
  canvas.line( halfLength+goalDepth, -goalHalfWidth,  halfLength+goalDepth, goalHalfWidth);
  canvas.line( halfLength+goalDepth, -goalHalfWidth,  halfLength, -goalHalfWidth);
  canvas.line( halfLength+goalDepth,  goalHalfWidth,  halfLength,  goalHalfWidth);
  // center line
  fl_color(FL_GREEN);
  canvas.line(0.0,-halfWidth,0.0,halfWidth);
  // penalty lines
  canvas.line( penaltyRegionLengthOffset,-penaltyRegionHalfWidth, penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  canvas.line( penaltyRegionLengthOffset,-penaltyRegionHalfWidth, halfLength,-penaltyRegionHalfWidth);
  canvas.line( penaltyRegionLengthOffset, penaltyRegionHalfWidth, halfLength, penaltyRegionHalfWidth);
  canvas.line(-penaltyRegionLengthOffset,-penaltyRegionHalfWidth,-penaltyRegionLengthOffset, penaltyRegionHalfWidth);
  canvas.line(-penaltyRegionLengthOffset,-penaltyRegionHalfWidth,-halfLength,-penaltyRegionHalfWidth);
  canvas.line(-penaltyRegionLengthOffset, penaltyRegionHalfWidth,-halfLength, penaltyRegionHalfWidth);
  // wedges
  canvas.line(-halfLength+wedgeSize,-halfWidth,-halfLength,-halfWidth+wedgeSize);
  canvas.line( halfLength-wedgeSize,-halfWidth, halfLength,-halfWidth+wedgeSize);
  canvas.line(-halfLength+wedgeSize, halfWidth,-halfLength, halfWidth-wedgeSize);
  canvas.line( halfLength-wedgeSize, halfWidth, halfLength, halfWidth-wedgeSize);
  // center circle
  canvas.circle(0.0,0.0,centerCircleRadius);

  for(int samp_idx=0; samp_idx<sampler.NumSamples; samp_idx++){
    Sample *samp;
    samp = &sampler.samples[samp_idx];
    //switch(samp->line_idx){
    //  case 0:
    //    fl_color(FL_RED);
    //    break;
    //  case 1:
    //    fl_color(FL_YELLOW);
    //    break;
    //  case 2:
    //    fl_color(FL_BLUE);
    //    break;
    //  default:
    //    fl_color(FL_WHITE);
    //    break;
    //}
    fl_color(FL_WHITE);
    canvas.dot(V2COMP(samp->loc),5);

    vector2d heading;
    vector2d dir_ind_end;
    heading.set(cos(samp->angle),sin(samp->angle));
    dir_ind_end = samp->loc + heading*50.0;
    fl_color(FL_YELLOW);
    canvas.line(V2COMP(samp->loc),V2COMP(dir_ind_end));
  }
}

AppWindow::AppWindow(const char *log_filename) :
  Fl_Double_Window(0,0,width,height)
{
  resizable(this);

  canvas.set_size(width,height);

  keep_running = true;
  LocalizationMode = true;

  sampler_tester.init();
  if(log_filename)
    localization_tester.init(log_filename);
}

AppWindow::~AppWindow()
{
}

void AppWindow::resize(int x,int y,int w, int h)
{
  Fl_Double_Window::resize(x,y,w,h);

  canvas.set_size(w,h);
}

void AppWindow::draw()
{
  fl_push_no_clip();
  if(LocalizationMode)
    localization_tester.draw_data(this,canvas);
  else
    sampler_tester.draw_data(this,canvas);    
  fl_pop_clip();
}

int AppWindow::handle(int event)
{
  if(LocalizationMode){
    switch(event){
      case FL_KEYDOWN:
        {
          int key = Fl::event_key();
          switch(key){
            case 'm':
              LocalizationMode = false;
              break;
            case FL_Escape:
            case 'q':
              keep_running = false;
              break;
            case ' ':
              localization_tester.step(1);
              redraw();
              break;
            case 's':
              if(Fl::event_key(FL_Shift_L) || Fl::event_key(FL_Shift_R)){
                localization_tester.step(100);
              }else{
                localization_tester.step(10);
              }
              redraw();
              break;
            case 'r':
              localization_tester.reset();
              redraw();
              break;
          }
        }
        return 1;
        break;
    }
  }else{
    switch(event){
      case FL_KEYDOWN:
        {
          int key = Fl::event_key();
          switch(key){
            case 'm':
              LocalizationMode = true;
              break;
            case FL_Escape:
            case 'q':
              keep_running = false;
              break;
            case ' ':
              sampler_tester.step(1);
              redraw();
              break;
            case 's':
              if(Fl::event_key(FL_Shift_L) || Fl::event_key(FL_Shift_R)){
                sampler_tester.step(100);
              }else{
                sampler_tester.step(10);
              }
              redraw();
              break;
            case 'r':
              sampler_tester.reset();
              redraw();
              break;
          }
        }
        return 1;
        break;
    }
  }

  return 0;
}

void AppWindow::run()
{
  int times_cnt=0;

  while(keep_running) {
    if(Interactive){
      Fl::check();
    }else{
      if(LocalizationMode)
        keep_running = localization_tester.step(1);
      else
        keep_running = sampler_tester.step(1);
      if(ProfileMode && !keep_running){
        times_cnt++;
        keep_running = (times_cnt<RepeatCount);
        if(LocalizationMode)
          localization_tester.reset();
      }
    }

    if(Interactive)
      usleep(10000);
  }
}

int main(int argc, char *argv[])
{
  int next_opt;
  for(next_opt=1; next_opt < argc && argv[next_opt][0]=='-'; next_opt++) {
    char *opt;
    opt = argv[next_opt];
    if(strcmp(opt,"--profile")==0) {
      ProfileMode = true;
      Lazy = false;
    } else if(strcmp(opt,"--go")==0) {
      GoMode = true;
    } else if(strcmp(opt,"--repeat")==0) {
      next_opt++;
      if(!(next_opt < argc)) {
        fprintf(stderr,"must specify numeric argument after --repeat\n");
        exit(5);
      }
      RepeatCount = atoi(argv[next_opt]);
    }
  }
  argc -= (next_opt-1);
  argv = &argv[next_opt];

  Interactive = !(ProfileMode || GoMode);

  bool grab;
    
  grab = false;

  Fl::visual(FL_DOUBLE|FL_INDEX);

  if(argc > 1){
    win = new AppWindow(argv[0]);
  }else{
    printf("No log file specified, only sample debugging is available\n");
    win = new AppWindow(NULL);
  }
  win->label("Localization Test");

  win->show();

  win->run();

  return(0);
}