/* 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.
  ========================================================================= */

#ifndef INCLUDED_Vision_h
#define INCLUDED_Vision_h

#include <math.h>

class PacketStream;
class PacketStreamCollection;

#include "../headers/system_config.h"
#include "../headers/field.h"
#include "../headers/gvector.h"
#include "../headers/Geometry.h"
#include "../headers/Util.h"
#include "../Main/Events.h"
#include "../Motion/Kinematics.h"

class CameraFrameSource;
class SensorData;

class FieldLine;

#include "cmvision.h"
#include "VisionInterface.h"

using namespace VisionInterface;

#include "../Main/globals.h"

extern int DebugClass;
extern char DebugInfo[1024];
// region id to label string
typedef std::map<ulong,char *> RegionLabelMapT;
extern RegionLabelMapT *RegionLabelsOld;
extern RegionLabelMapT *RegionLabelsNew;

#define MAX_TMAPS 16

typedef uchar pixel;
typedef uchar cmap_t;
typedef CMVision::image<uchar> image;
typedef CMVision::color_class_state color_class_state;
typedef CMVision::run<cmap_t> run;
typedef CMVision::region region;

// this include needs to be AFTER the run typedef
#include "SPOutVisionEncoder.h"

const int bits_y = 4;
const int bits_u = 6;
const int bits_v = 6;

// Used with our correction mask where we approximate
// a floating point multiplication with integers.
const int mask_shift = 7;

#ifdef PLATFORM_LINUX
const int bits_r = 5;
const int bits_g = 6;
const int bits_b = 5;
#endif

#define MAX_COLORS 9
#define NUM_COLORS 9

// FIXME: These should be determined from colors.txt
// DO NOT reorder these as the order is assumed elsewhere in the code!
#define COLOR_BACKGROUND 0
#define COLOR_ORANGE     1
#define COLOR_GREEN      2
#define COLOR_PINK       3
#define COLOR_CYAN       4
#define COLOR_YELLOW     5
#define COLOR_BLUE       6
#define COLOR_RED        7
#define COLOR_WHITE      8

static const double RobotArea = 13711; // mm^2 (empirically guessed, side view)

#define MIN_EXP_REGION_SIZE 16
#define MIN_EXP_RUN_LENGTH   8

// for radial map
struct RadialRun {
  GVector::vector2d<int> startPt;
  GVector::vector2d<int> endPt;
  int color;
  int length;
};

// for general scanning use
struct ScanRun {
  GVector::vector2d<int> startPt;
  GVector::vector2d<int> endPt;
  int color;
  int length;
};

struct EllipseFit {
  vector2f cen;
  float maj_axis;
  float min_axis;
  float maj_axis_angle;
  float fit_error;
  int num_error_pts; // number of points used in calculating error
};

// FIXME: put reasonable value and change code to protect against problems
#define MAX_RADIAL_RUNS (208+160)

// additional information needed by vision about objects not exported to other objects
struct VisionObjectInfo {
  region *reg,*reg2;
};

class Vision : public EventProcessor {
 private:

  Kinematics kin;

 public: // read-only outside of class

  ulong frameTimestamp;

  int num_tmaps;
  int cur_tmap;
  cmap_t *cmap; // colorized vision frame
  cmap_t *tmap[MAX_TMAPS]; // threshold map

  // HACK for variable lighting challenge
  double tmap_scores[MAX_TMAPS];
  bool score_images;
  // /HACK for variable lighting challenge

  run *rmap,*rmap2; // run maps
  region *reg; // regions

  bool red_v_or_valid;
  vector3d red_v_or_dir;
  double red_v_or_base;
  double red_v_or_scale;

  double marker_color_offset; // offset in pixels of colored region away from pink region

  ObjectInfo *object_info;

  RadialRun *radial_runs;

  VisionObjectInfo vobj_info[NUM_OBJECTINFO_OBJECTS];
  RadialObjectMap radial_map;

  color_class_state color[MAX_COLORS];

  int width,height;
  int max_width,max_height;
  int max_runs,max_regions;
  int num_colors,num_runs,num_regions;
  int num_radial_runs;

  double HorzFOV;
  double VertFOV;
  double FocalDist;
  double FocalDistV;
  // size of vertical pixel in terms of size of horizontal pixel
  double YPixelSize;

  double body_angle, body_height;
  double head_angles[3];
  HeadVelocity head_vel;

  vector3d camera_loc,camera_dir,camera_up,camera_right;
  // direction vectors of ego up/horizon directions in img coordinates
  // img_hor is pointing to the right of up direction
  vector2d img_up,img_hor;

  // distortion calibration stuff
  double sq_distort_coeff,lin_distort_coeff;

public:
  // output stuff
  SPOutVisionEncoder *encoder;
  uchar *encodeBuf;

  PacketStream *visionAvgColorStream;
  PacketStream *visionColorAreaStream;
  PacketStream *visionRadialMapStream;
  PacketStream *visionRawStream;
  PacketStream *visionRLEStream;
  PacketStream *visionObjStream;

  int outCountAvgColor;  // number of frames since avg img color was output
  int outCountColorArea; // number of frames since color area was output
  int outCountRadialMap; // number of frames since radial map was output
  int outCountRaw;       // number of frames since raw image was output
  int outCountRLE;       // number of frames since RLE image was output

  // other stuff
  CMVision::image_yuv<uchar> img;
  CMVision::image_yuv<uchar> correction_mask;
  bool use_correction_mask;

  // EventProcessor related stuff
  uint cam_frame_src_id;
  uint sensor_data_id;
  CameraFrameSource *cam_frame_src;
  SensorData *sensor_data;
  ulong available_time;
  ulong last_update_time;

public:
  static char const * const name;

  static EventProcessor *create() {return new Vision;}

  Vision();
  Vision(bool use_ers7_kinematics);
  virtual ~Vision();

  // compatibility hack
  void forceUpdate(ulong time);

  virtual bool initConnections();
  virtual bool update(ulong time,const EventList *events);
  void get(ulong time);

#ifdef PLATFORM_LINUX
  bool thresholdImage(CMVision::image_classified &img);
  bool thresholdImage(CMVision::image_idx<rgb> &img);
  bool thresholdImage(CMVision::image<rgb> &img);
#endif
  bool thresholdImage(CMVision::image_yuv<uchar> &img);
  bool thresholdAndCorrectImage(CMVision::image_yuv<uchar> &img);
  
  template<class image>
  bool runLowLevelVision(image &img);
  bool getBodyParameters();

  bool onImage(int x, int y) {
    return (0<=x && x<width && 0<=y && y<height);
  }
  void clipToImage(int &x, int &y) {
    x = bound(x,0,width -1);
    y = bound(y,0,height-1);
  }
  static bool inBox(int x, int y, int x1, int y1, int x2, int y2) {
    return (x1<=x && x<x2 && y1<=y && y<y2);
  }

  int getColorUnsafe(int x,int y)
    {return(cmap[y*width+x]);}
  int getNearColor(int x,int y)
    {return(cmap[bound(y,0,height-1)*width+bound(x,0,width-1)]);}
  // adds the colors found to the color_cnt histogram
  int addToHistHorizStrip(int y, int x1, int x2, int *color_cnt);
  int addToHistVertStrip (int x, int y1, int y2, int *color_cnt);

  template<class Callback, class CallbackParams>
  bool drawLine(int x1, int y1, int x2, int y2, Callback callback, CallbackParams params);
  // returns true if hitting a boundary (or 0 dx and dy) cause line drawing to terminate
  template<class Callback, class CallbackParams>
  bool drawLine2(int x1, int y1, int dx, int dy, Callback callback, CallbackParams params);

  // find the (x,y) pixel location most in direction dir_x,dir_y of color color_idx
  // starts at (x,y) when performing search
  // finds a local maximum solution
  bool findCorner(int color_idx, int dir_x, int dir_y, int *x, int *y);
  // align v1 and v2 on a vertical plane
  // output plane_dir (the direction of the plane in xy coordinates)
  // v1_dz,v2_dz - the project of v1,v2 onto the plane.
  //   x is the radial (distance) component and y is the elevation component
  // returns the cos of the horizontal angle between v1 and v2
  double alignVertical(vector3d v1, vector3d v2, vector2d &plane_dir, vector2d &v1_dz, vector2d &v2_dz);

  // finds the direction of the ray through this pixel in robot coordinates
  vector3d getPixelDirection(double x, double y);
  // finds the point which through pixel (x,y) which intersects plane Z=z
  // returns true if ray intersects this z plane
  bool intersectPixelZPlane(double x, double y, double z_value, vector3d &loc);
  // convert ego vector to camera coordinates (z=distance, x/y in same direction as image x/y)
  vector3d convertToCamera(vector3d ego);
  // convert camera coordinates (z=distance, x/y in same direction as image x/y) to image coordinates
  vector2d cameraToImg(vector3d camera);
  // calculates the locations top_loc,bot_loc which result in a vertical seperation of seperation when
  // traveling in the directions top_dz,bot_dz.  The first component of top_dz,bot_dz is the distance
  // and the second component is the elevation, these vectors should be normalized before calling this
  // function.
  bool findLocVerticalSeperation(vector2d top_dz,vector2d bot_dz,double seperation,vector2d &top_loc,vector2d &bot_loc);
  // finds the ego angle on ground for a point
  double groundAngle(double x, double y);
  // finds the leftest and rightmost ego angles on ground for a bounding box
  void findSpan(double &left, double &right, double x1, double x2, double y1, double y2);
  // calculates the edge mask for a bounding box
  int calcEdgeMask(double x1, double x2, double y1, double y2);
  int calcEdgeMask(int x1,int x2,int y1,int y2);
  int calcEdgeMask(region *reg)
    {return(calcEdgeMask(reg->x1,reg->x2,reg->y1,reg->y2));}
  int calcEdgeMask(int x,int y,int slop=0);
  // calculates the amount a color leans towards one color or another
  double normalizedColorLocation(vector3d loc,vector3d dir,double base,double scale);

  // returns 0 for no ball edge found,
  //         1 for ball edge found
  template<class Image>
  int scanForBallEdge(vector2i &edge,vector2i &scan_end,vector2i start,vector2i dir,Image &img);
  template<class Image>
  void fitBallEllipse(EllipseFit *ellipse,region *or_reg,Image &img);
  template<class image>
  void findBall(VObject *ball,VisionObjectInfo *ball_info,image &img);
  void findMarkers(VObject *markers,VisionObjectInfo *marker_infos);
  void findGoal(int color_idx,VObject *goal,VObject *goal_edges,VisionObjectInfo *goal_info);
  void findRobots(int color_idx,VObject *robots,VisionObjectInfo *robot_infos,int max_robots);

  void findChargingTower(VObject *tower);
  double percRegionInside(region *outer, region *inner);
  void findChargingBullseye(VObject *bullseye);
  void findSkateboardWheels(VObject *wheels);
  void findSkateboard(VObject *skateboard);

  void findObjectInFrontOfIR(ObjectInfo *obj_info);
  // returned error is in [-1.0,1.0]
  float findVisualServoingError();
  template<class image>
  bool runHighLevelVision(ObjectInfo *obj_info,image &img);

  void AddRadialObject(double x, double y, RadialObjectMap *radial_map, int robj_idx, int ang_idx);
  void radialFindObjects(int num_radial_runs,RadialRun *radial_runs,RO *ros,int *num_ros,int ang_idx,float angle);
  void UpdateRadialAngle(int ang_idx,float angle,GVector::vector2d<int> scan_start_img,GVector::vector2d<int> scan_end_img);
  void createRadialMap();

  int detectTransitionsScan(vector2d *line_pts,int max_line_pts,ScanRun *runs,int num_runs);
  int findTransitions(vector2d *line_pts,int max_line_pts);
  int fitLines(FieldLine *lines,int max_lines,vector2d *line_pts,int num_line_pts);
  void findCornerGoalieBox(VObject *corner);


public:
  int setThreshold(int threshold_id);

  // HACK for variable lighting challenge
  static const double tmap_score_alpha = .99;
  void scoreImage();
  void startThreshTest();
  double endThreshTest();
  // /HACK

  const vector3d &get_camera_loc() {return camera_loc;}
  const vector3d &get_camera_dir() {return camera_dir;}
  int getColor(int x,int y)
    {return((x>=0 && y>=0 && x<width && y<height)?
            cmap[y*width+x] : COLOR_BACKGROUND);}
  int getWidth() {return width;}
  int getHeight() {return height;}
  bool initialize(char *config_file,int width_param,int height_param);
  void initializeStreams(PacketStreamCollection *out_psc);
  bool close();

  // calculate the image up/right vectors from the camera pos/dir vectors
  void calcImgVectors();

  void setHeadAngles(const double *angles);
  void setHeadVelocities(const double *angles);
  void setBodyParams(double body_angle_param, double body_height_param) {
    body_angle=body_angle_param;
    body_height=body_height_param;
  }

  RadialObjectMap *getRadialMap() {
    return &radial_map;
  }

  HeadVelocity *getHeadVelocity() {
    return &head_vel;
  }

  bool processFrame(ObjectInfo *obj_info,uchar *data_y,uchar *data_u,uchar *data_v, int width, int height, int skip);
#ifdef PLATFORM_LINUX
  bool processFrame(ObjectInfo *obj_info, CMVision::image_classified *img, int width, int height);
  bool processFrame(ObjectInfo *obj_info, CMVision::image_idx<rgb> *img, int width, int height);
  bool processFrame(ObjectInfo *obj_info, CMVision::image<rgb> *img, int width, int height);
#endif
  bool saveThresholdImage(char *filename);

  static ulong calcRegionId(region *reg)
  {
    ulong reg_id;
    reg_id = 0;
    reg_id |= (reg->x1 & 0xFF) << 24;
    reg_id |= (reg->x2 & 0xFF) << 16;
    reg_id |= (reg->y1 & 0xFF) <<  8;
    reg_id |= (reg->y2 & 0xFF)      ;
    return reg_id;
  }

  void sendAvgColor ();
  void sendRawImage ();
  void sendRLEImage ();
  void sendColorArea();
  void sendRadialMap();

  bool ballDisabled;
  bool calcTotalArea;
  bool ballGreenFiltersDisabled;
};

template<class Callback, class CallbackParams>
bool Vision::drawLine(int x0,int y0,
                      int x1,int y1,
                      Callback callback, CallbackParams params)
{
  int w,h;
  int sx,sy;
  int dx,dy;

  dx = sign(x1 - x0);
  dy = sign(y1 - y0);
  w = abs(x1 - x0);
  h = abs(y1 - y0);
  sx = h;
  sy = w;

  while(true) {
    // draw point
    if(!callback(x0,y0,params))
      return false;

    if(x0==x1 && y0==y1)
      break;

    // find next point
    if(sx < sy){
      sx += h;
      x0 += dx;
    } else {
      sy += w;
      y0 += dy;
    }
  }

  return true;
}

template<class Callback, class CallbackParams>
bool Vision::drawLine2(int x, int y,
                       int vx,int vy,
                       Callback callback, CallbackParams params)
{
  int w,h;
  int sx,sy;
  int dx,dy;

  if(vx==0 && vy==0) return true;

  dx = sign(vx);
  dy = sign(vy);
  w = abs(vx);
  h = abs(vy);
  sx = h;
  sy = w;

  // FIXME: This can be sped up by calculating the
  //  number of pixels that will be hit before the
  //  edge of the image and replacing these 4 tests
  //  with a single test
  while(x>=0 && x<width && y>=0 && y<height){
    // draw point
    if(!callback(x,y,params))
      return false;

    // find next point
    if(sx < sy){
      sx += h;
      x += dx;
    }else{
      sy += w;
      y += dy;
    }
  }

  return true;
}


#endif