namespace frc971.vision.sift;

// Represents the location of a keypoint in field coordinates.
struct KeypointFieldLocation {
  x:float (id: 0);
  y:float (id: 1);
  z:float (id: 2);
}

// Represents a single feature extracted from an image.
table Feature {
  // Contains the descriptor data. OpenCV likes to represent them as floats, but
  // they're really ubytes.
  //
  // TODO(Brian): These are scaled to be convertible to chars. Should we do
  // that to minimize storage space? Or maybe int16?
  //
  // The size of this depends on the parameters. It is width*width*hist_bins.
  // Currently we have width=4 and hist_bins=8, which results in a size of
  // 4*4*8=128.
  descriptor:[ubyte] (id: 0);

  // Location of the keypoint.
  x:float (id: 1);
  y:float (id: 2);

  // Size of the keypoint neighborhood.
  size:float (id: 3);

  // Angle of the keypoint.
  // This is in [0,360) clockwise.
  angle:float (id: 4);

  // How good of a keypoint this is.
  response:float (id: 5);

  // Which octave this keypoint is from.
  octave:int (id: 6);

  // Where this feature's keypoint is on the field. This will only be filled out
  // for training features, not ones extracted from query images.
  field_location:KeypointFieldLocation (id: 7);
}

// Represents a single match between a training image and a query image.
struct Match {
  // The index of the feature for the query image.
  query_feature:int (id: 0);
  // The index of the feature for the training image.
  train_feature:int (id: 1);
  // How "good" the match is.
  distance:float (id: 2);
}

// Represents all the matches between a single training image and a query
// image.
table ImageMatch {
  matches:[Match] (id: 0);
  // The index of the training image within all the training images.
  train_image:int (id: 1);
}

table TransformationMatrix {
  // The matrix data for a row-major 4x4 homogeneous transformation matrix.
  // This implies the bottom row is (0, 0, 0, 1).
  data:[float] (id: 0);
}

// Calibration information for a given camera on a given robot.
table CameraCalibration {
  // The name of the camera node which this calibration data applies to.
  node_name:string (id: 0);
  // The team number of the robot which this calibration data applies to.
  team_number:int (id: 1);

  // Intrinsics for the camera.
  //
  // This is the standard OpenCV intrinsics matrix in row major order (3x3).
  intrinsics:[float] (id: 2);

  // Fixed extrinsics for the camera. This transforms from camera coordinates to
  // robot coordinates. For example: multiplying (0, 0, 0, 1) by this results in
  // the position of the camera aperature in robot coordinates.
  fixed_extrinsics:TransformationMatrix (id: 3);

  // Extrinsics for a camera on a turret. This will only be filled out for
  // applicable cameras. For turret-mounted cameras, fixed_extrinsics defines
  // a position for the center of rotation of the turret, and this field defines
  // a position for the camera on the turret.
  //
  // The combination of the two transformations is underdefined, so nothing can
  // distinguish between the two parts of the final extrinsics for a given
  // turret position.
  //
  // To get the final extrinsics for a camera using this transformation,
  // multiply (in order):
  //   fixed_extrinsics
  //   rotation around the Z axis by the turret angle
  //   turret_extrinsics
  turret_extrinsics:TransformationMatrix (id: 4);

  // This is the standard OpenCV 5 parameter distortion coefficients
  dist_coeffs:[float] (id: 5);

  // Timestamp for when the calibration was taken on the realtime clock.
  calibration_timestamp:int64 (id: 6);

  // The id of the camera which this calibration data applies to.
  // Expected to be formatted as Year-Number (YY-##).
  camera_id:string (id: 7);
}

// Contains the information the EKF wants from an image matched against a single
// training image.
//
// This is represented as a transformation from the camera to the target
// (camera_to_target) and a transformatoin from the field to the target
// (field_to_target).
//
// We also send the map from the field to the camera, which can be computed
// with the first two, to make it easier to display.
table CameraPose {
  // Transformation matrix from the camera to the target.
  // (0, 0, 0) is the aperture of the camera (we pretend it's an ideal pinhole
  // camera). Positive Z is out of the camera. Positive X and Y are right
  // handed, but which way they face depends on the camera extrinsics.
  camera_to_target:TransformationMatrix (id: 0);

  // Field coordinates of the target, represented as a transformation matrix
  // from the field to the target.
  // (0, 0, 0) is the center of the field, on the level of most of the field
  // (not the region under the truss). Positive X is towards the red alliance's
  // PLAYER STATION. Positive Z is up. The coordinate system is right-handed.
  //
  // Note that the red PLAYER STATION is where the red drive teams are. This is
  // often where the blue robots are shooting towards.
  //
  // The value here will be selected from a small, static set of targets we
  // train images on.
  field_to_target:TransformationMatrix (id: 1);

  // The pose of the camera in the field coordinate frame
  field_to_camera:TransformationMatrix (id: 2);

  // 2D image coordinate representing target location on the matched image
  query_target_point_x:float (id: 3);
  query_target_point_y:float (id: 4);
  // Perceived radius of target circle
  query_target_point_radius:float (id: 5);
  // Number of features used in this match.
  homography_feature_count:int (id: 6);
  // Training image used for this pose.
  training_image_index:int (id: 7);
}

table ImageMatchResult {
  // The matches from this image to each of the training images which matched.
  // Each member is against the same captured image.
  image_matches:[ImageMatch] (id: 0);

  // The transformations for this image for each of the training images which
  // matched.
  // TODO(Brian): Include some kind of covariance information for these.
  camera_poses:[CameraPose] (id: 1);

  // The features for this image.
  features:[Feature] (id: 2);

  // Timestamp when the frame was captured.
  image_monotonic_timestamp_ns:long (id: 3);

  // Information about the camera which took this image.
  camera_calibration:CameraCalibration (id: 4);

  // Total number of match result send failures.
  send_failures:uint64 (id: 5);
}

root_type ImageMatchResult;