/* 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 __VISION_INTERFACE_H__
#define __VISION_INTERFACE_H__

#include <math.h>

#include "../headers/DogTypes.h"
#include "../headers/Geometry.h"

class Vision;

namespace VisionInterface {
  // ERS-7 56.9 deg  wide and 45.2 deg high
  // ERS-210: 57.6 deg and 47.8 deg high

  static const double ers7_HorzFOV = 56.9 * M_PI / 180.0;
  static const double ers7_VertFOV = 45.2 * M_PI/ 180.0;
  static const double ers210_HorzFOV = 58.0 * M_PI / 180.0;
  static const double ers210_VertFOV = 48.0 * M_PI / 180.0;

  // Bitmasks for which edges of the image an object borders
  const uchar OFF_EDGE_LEFT   = 1;
  const uchar OFF_EDGE_RIGHT  = 2;
  const uchar OFF_EDGE_TOP    = 4;
  const uchar OFF_EDGE_BOTTOM = 8;

  // High level vision ouput structure for detected objects
  struct VObject{
    double confidence; // [0,1] Estimate of certainty
    vector3d loc;      // Relative to front of robot (on ground)
    double left,right; // Angle to left and right of object (egocentric)
    double distance;   // Distance of object (on ground)
    double confInFrontOfIR;  // confidence of the object being in front of the IR sensor
    // The orientation of the object relative to the orientation of the robot [-M_PI,M_PI]
    // 0.0 indicates facing same way as robot, positive is facing left of where the robot is facing
    double orientation;
    uchar edge;          // Is object on edge of image (bitmasks above)
  };

  // Colors: (C)yan (G)reen (Y)ellow (P)ink
  // Markers are specified <color>(O)ver<color>
  // evens are +y, odds are -y; overall order is +x,0,-x
  const int MARKER = 0;
  const int MARKER_COP = 0;
  const int MARKER_POC = 1;
  const int MARKER_GOP = 2;
  const int MARKER_POG = 3;
  const int MARKER_YOP = 4;
  const int MARKER_POY = 5;

  const int YELLOW_GOAL = 6;
  const int YELLOW_GOAL_SIDES = 7;
  const int CYAN_GOAL   = 9;
  const int CYAN_GOAL_SIDES = 10;

  const int LEFT_SIDE =0;
  const int RIGHT_SIDE=1;

  const int BALL = 12;

  const int ROBOT      = 13;
  const int BLUE_ROBOT = 13;
  const int RED_ROBOT  = 16;
  const int NUM_ROBOTS_PER_COLOR = 3;

  const int NUM_MARKERS = 6;
  const bool USE_MIDDLE_MARKERS = false;

  const int BLUE_BAR = 19;
  const int RED_BAR  = 20;

  const int CHARGING_TOWER    = 21;
  const int CHARGING_BULLSEYE = 22;
  const int SKATEBOARD_WHEELS = 23;
  const int NUM_SKATEBOARD_WHEELS = 4;
  const int SKATEBOARD        = 27;

  const int STRIPE_CORNER     = 28;

  // number of normal vision objects
  const int NUM_VISION_OBJECTS = NUM_MARKERS + 2*3 + 1 + 2*NUM_ROBOTS_PER_COLOR;
  // number of vision object in object info structure
  const int NUM_OBJECTINFO_OBJECTS = NUM_VISION_OBJECTS + 2 + 3+NUM_SKATEBOARD_WHEELS;

  // All the objects we might detect
  struct ObjectInfo{
    VObject marker[6]; // Order:  C/P  P/C  G/P  P/G  Y/P  P/Y
    VObject yellow_goal;
    VObject yellow_goal_edges[2];
    VObject cyan_goal;
    VObject cyan_goal_edges[2];
    VObject ball;
    VObject blue_robots[3];
    VObject red_robots[3];
    VObject blue_bar;
    VObject red_bar;
    VObject charging_tower;
    VObject charging_bullseye;
    VObject skateboard_wheels[4];
    VObject skateboard;
    VObject stripe_corner;
  };
  
  
  struct HeadVelocity {
    double pan;
    double tilt;
  };
  

  // types and sub_types for struct RO

  const int RO_Field  = 0;
  const int RO_Stripe = 1;
  const int RO_Wall   = 2;
  const int RO_Goal   = 3;
  const int RO_Ball   = 4;
  const int RO_Obs    = 5;
  const int RO_Num_Types = 6;

  const int RO_Goal_Yellow = 0;
  const int RO_Goal_Cyan   = 1;

  const int RO_Obs_Unknown    = 0;
  const int RO_Obs_Red_Robot  = 1;
  const int RO_Obs_Blue_Robot = 2;

  const int RO_Max_Num_SubTypes = 3;

  // maximum number of ROs in a single scan line
  static const int MAX_ROS = 20;

  // a Radial Object
  struct RO{
    int start_run;
    int end_run;
    int size; // in pixels
    int type;
    int sub_type;
    vector2f start_pt;
    vector2f end_pt;
  };

  // RObject = Radial Object
  const int ROBJECT_VISIBLE_START     = 0;
  const int ROBJECT_VISIBLE_END       = 1;
  // must be start of non fake objects
  const int ROBJECT_FIELD_CLEAR_START = 2;
  const int ROBJECT_FIELD_CLEAR_END   = 3;
  const int ROBJECT_WALL              = 4;
  const int ROBJECT_ROBOT             = 5;
  const int ROBJECT_RED_ROBOT         = 6;
  const int ROBJECT_BLUE_ROBOT        = 7;
  const int ROBJECT_BALL              = 8;
  const int ROBJECT_CYAN_GOAL         = 9;
  const int ROBJECT_YELLOW_GOAL       =10;
  const int ROBJECT_STRIPE            =11;
  const int NUM_ROBJECTS              =12;

  static const double RadialAngleSpacing = 5.0 * M_PI / 180.0;
  static const int NUM_ANGLES = 72;
  // next line is preferred but won't compile on Aperios
  //static const int NUM_ANGLES = (int)(2*M_PI/RadialAngleSpacing + .5);

  static const double RadialFarDistance = 100000.0;

  struct RadialObjectReading {
    double confidence;
    double distance;
  };
  
  struct RadialObjectMap {
    static inline int angleToIdx(double angle) {
      int temp = (int)floor((angle/RadialAngleSpacing) + 0.5);
      int angle_idx = temp + NUM_ANGLES/2;
      return ((angle_idx % NUM_ANGLES) + NUM_ANGLES) % NUM_ANGLES;
    }
    static inline double idxToAngle(int idx) {
      return (idx - NUM_ANGLES/2) * RadialAngleSpacing;
    }
    static inline ulong idxToMask(int idx) {
      return 1UL << idx;
    }
    inline RadialObjectReading *query(int obj_idx, int angle_idx) {
      return &robjects[obj_idx][angle_idx];
    }

    RO ros[NUM_ANGLES][MAX_ROS];
    int num_ros[NUM_ANGLES];

    RadialObjectReading robjects[NUM_ROBJECTS][NUM_ANGLES];
    RadialObjectReading *closest(ulong mask, int angle_idx) {
      RadialObjectReading *closest_robj=NULL;
      
      for(int robj_idx=0; robj_idx<NUM_ROBJECTS; robj_idx++) {
        RadialObjectReading *try_robj;
        try_robj = &robjects[robj_idx][angle_idx];
        if((mask & idxToMask(robj_idx))!=0 &&
           try_robj->confidence > 0.5 && 
           (closest_robj==NULL ||
            closest_robj->distance > try_robj->distance)) {
          closest_robj = try_robj;
        }
      }

      return closest_robj;
    }
  };

  // returns the id of the old threshold or -1 on error
  int SetThreshold(Vision *vision,int threshold_id);

  void SendRawImage(Vision *vision); // Forces the output of the current raw image to the log/spout/wl
  void SendRLEImage(Vision *vision); // Forces the output of the current RLE image to the log/spout/wl
} // namespace vision

#endif
// __VISION_INTERFACE_H__