/* 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 <FL/Fl_Window.H>
#include <FL/fl_draw.H>

#include <math.h>
#include <stdio.h>
#include <time.h>

using namespace std;

#include "headers/field.h"
#include "headers/Geometry.h"
#include "headers/MessageStructures.hh"
#include "headers/random.h"
#include "Localization/SRL/Constants.h"
#include "Localization/SRL/Environment.h"
#include "Localization/SRL/Functions.h"
#include "Localization/SRL/Localization.h"
#include "Localization/SRL/LocalizationEngine.h"
#include "Localization/SRL/SensorInterp.h"
#include "Vision/Vision.h"
#include "Vision/VisionInterface.h"

#include "log_processing/shared/LogReader.h"
#include "log_processing/shared/PacketDecoder.h"
#include "log_processing/shared/MotionPacketDecoder.h"
#include "log_processing/shared/VisionPacketDecoder.h"

#include "localization_test.h"
#include "LocalizationTester.h"

using namespace VisionInterface;

static const bool debug=true;

LocalizationTester::LocalizationTester() :
  vision(true),
  vision_rle_decoder(0)
{
  dist_cnt = 0;
  move_cnt = 0;
  vis_cnt  = 0;

  packet.data = NULL;
  log_file = NULL;

  mzero(obj_info);
}

LocalizationTester::~LocalizationTester()
{
  delete[] packet.data;
  if(log_file)
    fclose(log_file);
}

void LocalizationTester::init(const char *log_filename)
{
  localizer.init();

  packet.data = new uchar[MaxPacketLength];

  motion_update.pos_delta.set(0.0,0.0);
  motion_update.heading_delta.set(0.0,0.0);
  motion_update.body.neck_offset.set(100.0,0.0);
    
  log_file = fopen(log_filename,"r");
  if(log_file == NULL){
    cerr << "Couldn't open log file 'test.log'" << endl;
    exit(2);
  }
  log_reader.setSource(log_file);

  vision.initialize("vision.cfg",MaxImageXSize,MaxImageYSize);
  vis_img.buf = new uchar[MaxImageXSize*MaxImageYSize];
  vis_img.width =MaxImageXSize;
  vis_img.height=MaxImageYSize;
}

bool LocalizationTester::step(int step_cnt)
{
  int steps_taken=0;

  while(steps_taken<step_cnt && log_reader.readNextPacket(&packet,MaxPacketLength)){
    bool took_step=false;

    printf("packet:dataType=%2x length=%lu\n",packet.dataType,packet.length);

    switch(packet.dataType){
      case PacketDecoder::VisionObjLead:
        {
          continue;
          if(!vision_decoder.decode(&obj_info,&packet)){
            cerr << "Problem parsing vision object packet, packet ignored" << endl;
            continue;
          }
          if(debug) cout << "update sensors" << endl;
          bool saw_something=false;
          VisionInterface::RadialObjectMap rmap;
          mzero(rmap);
          saw_something = localizer.updatePositionSensors(&obj_info,&rmap);
          if(saw_something){
            dist_cnt+=3;
            vis_cnt++;
            steps_taken++;
            took_step = true;
          }
        }
        break;
      case PacketDecoder::VisionRLELead:
        {
          double angles[5];
          if(!vision_rle_decoder.decode(angles,&packet)){
            cerr << "Problem parsing angles from vision rle packet, packet ignored" << endl;
            continue;
          }
          if(!vision_rle_decoder.decode(DefImageXSize,DefImageYSize,(uchar *)vis_img.buf,&packet)){
            cerr << "Problem parsing image data from vision rle packet, packet ignored" << endl;
            continue;
          }
          //static int cnt=0;
          //if(++cnt > 20){
          //  for(int i=0; i<ImageXSize*ImageYSize; i++){
          //    printf("i=%d val=%u\n",i,vision.cmap[i]);
          //  }
          //}
          double body_angle  = angles[0];
          double body_height = angles[1];
          double *head_angles = &angles[2];
          vision.setBodyParams(body_angle,body_height);
          vision.setHeadAngles(head_angles);
          vision.runLowLevelVision(vis_img);
          vision.runHighLevelVision(&obj_info,vis_img);
          if(debug) cout << "update sensors" << endl;
          bool saw_something=false;
          saw_something = localizer.updatePositionSensors(&obj_info,&vision.radial_map);
          if(saw_something){
            dist_cnt+=3;
            vis_cnt++;
            steps_taken++;
            took_step = true;
          }
        }
        break;
      case PacketDecoder::VisionRLE2Lead:
        {
          double angles[5];
          if(!vision_rle_decoder.decode(angles,&packet)){
            cerr << "Problem parsing angles from vision rle packet, packet ignored" << endl;
            continue;
          }
          int image_x_size,image_y_size;
          if(!vision_rle_decoder.decode(image_x_size,image_y_size,(uchar *)vis_img.buf,MaxImageXSize,MaxImageYSize,&packet)){
            cerr << "Problem parsing image data from vision rle packet, packet ignored" << endl;
            continue;
          }
          //static int cnt=0;
          //if(++cnt > 20){
          //  for(int i=0; i<ImageXSize*ImageYSize; i++){
          //    printf("i=%d val=%u\n",i,vision.cmap[i]);
          //  }
          //}
          double body_angle  = angles[0];
          double body_height = angles[1];
          double *head_angles = &angles[2];
          vision.setBodyParams(body_angle,body_height);
          vision.setHeadAngles(head_angles);
          vision.runLowLevelVision(vis_img);
          vision.runHighLevelVision(&obj_info,vis_img);
          if(debug) cout << "update sensors" << endl;
          bool saw_something=false;
          saw_something = localizer.updatePositionSensors(&obj_info,&vision.radial_map);
          if(saw_something){
            dist_cnt+=3;
            vis_cnt++;
            steps_taken++;
            took_step = true;
          }
        }
        break;
      case PacketDecoder::MotionUpdateLead:
        if(!motion_decoder.decode(&motion_update,&packet)){
          cerr << "Problem parsing motion update packet, packet ignored" << endl;
          continue;
        }

        dist_cnt+=2;
        move_cnt++;
        steps_taken++;
        took_step = true;
        localizer.updatePositionMovement(&motion_update);
        break;
    }
      
    localizer.getPosition(&pos_info);
      
    if(debug && took_step){
      cout << "dist_cnt=" << dist_cnt << " move_cnt=" << move_cnt << " vis_cnt=" << vis_cnt << endl;
      cout << "x" << "=[" << pos_info.position.mean.x-2*pos_info.position.sx << "," 
           << pos_info.position.mean.x+2*pos_info.position.sx << "] " 
           << "mean=" << pos_info.position.mean.x << " stddev=" << pos_info.position.sx << " "
           << endl;
      cout << "y" << "=[" << pos_info.position.mean.y-2*pos_info.position.sy << "," 
           << pos_info.position.mean.y+2*pos_info.position.sy << "] " 
           << "mean=" << pos_info.position.mean.y << " stddev=" << pos_info.position.sy << " "
           << endl;
      cout << "theta" << "=[" << pos_info.heading.mean.angle()-2*pos_info.heading.sMaj << "," 
           << pos_info.heading.mean.angle()+2*pos_info.heading.sMaj << "] " 
           << "mean=" << pos_info.heading.mean.angle() << " stddev=" << pos_info.heading.sMaj << " "
         << endl;
      cout << endl;
    }
  }

  if(steps_taken<step_cnt){
    cout << "end of log" << endl;
  }

  return(steps_taken>=step_cnt);
}

void LocalizationTester::reset()
{
  localizer.resetPositionUniform();
  localizer.reset();

  log_reader.seekToStart();
}

void LocalizationTester::draw_obj_info(AppWindow *win,Canvas &canvas,vector2f robot_loc,vector2f robot_dir)
{
  static const int num_segments=16;
  static vector2f marker_locs[6]=
    {vector2f( halfLength+markerOffset, halfWidth+markerOffset),
     vector2f( halfLength+markerOffset,-halfWidth-markerOffset),
     vector2f(                     0.0, halfWidth+markerOffset),
     vector2f(                     0.0,-halfWidth-markerOffset),
     vector2f(-halfLength-markerOffset, halfWidth+markerOffset),
     vector2f(-halfLength-markerOffset,-halfWidth-markerOffset)};

  for(int i=0; i<6; i++){
    if(obj_info.marker[i].confidence > 0.4){
      uchar bright;
      float radius;
      vector2f center;

      bright = (uchar)(255.99 * obj_info.marker[i].confidence);
      center = marker_locs[i];
      radius = obj_info.marker[i].distance;

      if(obj_info.marker[i].confidence > 0.5)
        fl_color(bright,bright,bright);
      else
        fl_color(bright,0,0);
      canvas.circlef(V2COMP(center),50.0);
      canvas.circle(V2COMP(center),radius);

      for(int j=0; j<num_segments; j++){
        double t;
        vector2f ind_start,ind_end;

        t = j*2*M_PI/num_segments;
        ind_start.set(radius,0.0);
        ind_start = ind_start.rotate(t);
        ind_start += center;

        ind_end.set(100.0,0.0);
        ind_end = ind_end.rotate(M_PI+t-vector2f(V2COMP(obj_info.marker[i].loc)).angle());
        ind_end += ind_start;

        canvas.line(V2COMP(ind_start),V2COMP(ind_end));
      }
    }
  }

  static vector2f goal_edge_locs[4]=
    {vector2f(-halfLength,-goalHalfWidth),
     vector2f(-halfLength, goalHalfWidth),
     vector2f( halfLength, goalHalfWidth),
     vector2f( halfLength,-goalHalfWidth)};
  static int goal_obj_idxs[4]=
    {YELLOW_GOAL_SIDES+LEFT_SIDE,
     YELLOW_GOAL_SIDES+RIGHT_SIDE,
     CYAN_GOAL_SIDES  +LEFT_SIDE,
     CYAN_GOAL_SIDES  +RIGHT_SIDE};

  for(int i=0; i<4; i++){
    VObject *vobj;
    vobj = (&obj_info.marker[0])+goal_obj_idxs[i];

    if(vobj->confidence > 0.5){
      uchar bright;
      float radius;
      vector2f center;

      bright = (uchar)(255.99 * vobj->confidence);
      center = goal_edge_locs[i];
      radius = vobj->distance;

      if(vobj->edge == 0){
        fl_color(bright,bright,0);
        canvas.circle(V2COMP(center),radius-300.0);
        fl_color(bright,0,bright);
        canvas.circlef(V2COMP(center),50.0);
        canvas.circle(V2COMP(center),radius);
      }else{
        fl_color(bright,bright,0);
        canvas.circle(V2COMP(center),radius-300.0);
        fl_color(0,bright,bright);
        canvas.circlef(V2COMP(center),50.0);
        canvas.circle(V2COMP(center),radius);
      }

      for(int j=0; j<num_segments; j++){
        double t;
        vector2f ind_start,ind_end;

        t = j*2*M_PI/num_segments;
        ind_start.set(radius,0.0);
        ind_start = ind_start.rotate(t);
        ind_start += center;

        ind_end.set(100.0,0.0);
        ind_end = ind_end.rotate(M_PI+t-vector2f(V2COMP(vobj->loc)).angle());
        ind_end += ind_start;

        canvas.line(V2COMP(ind_start),V2COMP(ind_end));
      }
    }
  }

  if(obj_info.stripe_corner.confidence > 0.5){
    vector2f loc;
    loc.set(V2COMP(obj_info.stripe_corner.loc));
    loc = loc.project(robot_dir);
    loc += robot_loc;

    fl_color(FL_CYAN);
    canvas.dot(V2COMP(loc),5);
  }
}

void LocalizationTester::draw_point_info(AppWindow *win,Canvas &canvas,vector2f robot_loc,vector2f robot_dir)
{
  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++){
    int num_ros = vision.radial_map.num_ros[ang_idx];
    bool last_was_field=false;
    for(int ro_idx=0; ro_idx<num_ros; ro_idx++){
      RO *ro;
      uchar r,g,b;
      int size;
      bool draw;
      
      ro = &vision.radial_map.ros[ang_idx][ro_idx];
      r = g = b = 255;
      size = 3;
      draw = false;
      switch(ro->type){
        case RO_Stripe:
          draw = true;
          break;
        case RO_Wall:
          draw = true;
          size = 5;
          break;
        case RO_Goal:
          draw = true;
          if(ro->sub_type == RO_Goal_Yellow){
            b = 0;
          }else{
            // cyan
            r = 0;
          }
          break;
      }
      if(!last_was_field){
        draw = false;
      }
      last_was_field = (ro->type == RO_Field);

      if(draw){
        vector2f loc;
        loc.set(V2COMP(ro->start_pt));
        loc = loc.project(robot_dir);
        loc += robot_loc;
        fl_color(r,g,b);
        if(size == 1){
          canvas.point(V2COMP(loc));
        }else{
          canvas.dot(V2COMP(loc),size);
        }
      }
    }
  }
}

void LocalizationTester::draw_data(AppWindow *win,Canvas &canvas)
{
  static Sample *samples=NULL;

  if(samples == NULL)
    samples = new Sample[LocaleSampled::numSamples];

  localizer.copySamples(samples);

  canvas.set_range(vector2f(-3000.0,-2500.0),vector2f(3000.0,2500.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);

  draw_obj_info(win,canvas,vector2f(V2COMP(pos_info.position.mean)),vector2f(V2COMP(pos_info.heading.mean)));
  draw_point_info(win,canvas,vector2f(V2COMP(pos_info.position.mean)),vector2f(V2COMP(pos_info.heading.mean)));

  for(int i=0; i<LocaleSampled::numSamples; i++){
    Sample *samp;

    samp = &samples[i];

    fl_color(FL_WHITE);
    canvas.dot(V2COMP(samp->loc),5);

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

  vector2f robot_loc;
  robot_loc.set(V2COMP(pos_info.position.mean));
  fl_color(255,0,0);
  canvas.dot(V2COMP(robot_loc),5);
  vector2f dir_ind_end;
  dir_ind_end.set(V2COMP(pos_info.heading.mean));
  dir_ind_end = robot_loc + dir_ind_end*50.0;
  canvas.line(V2COMP(robot_loc),V2COMP(dir_ind_end));
}