#ifndef _Y2019_VISION_CONSTANTS_H_
#define _Y2019_VISION_CONSTANTS_H_

#include <math.h>
#include <array>
#include <string>

namespace y2019 {
namespace vision {

// Position of the idealized camera in 3d space.
struct CameraGeometry {
  static constexpr size_t kNumParams = 4;
  // In Meters from floor under imu center.
  ::std::array<double, 3> location{{0.0, 0.0, 0.0}};
  double heading = 0.0;

  void set(double *data) {
    data[0] = location[0];
    data[1] = location[1];
    data[2] = location[2];
    data[3] = heading;
  }
  static CameraGeometry get(const double *data) {
    CameraGeometry out;
    out.location[0] = data[0];
    out.location[1] = data[1];
    out.location[2] = data[2];
    out.heading = data[3];
    return out;
  }

  void Dump(std::basic_ostream<char> *o) const;
};

struct IntrinsicParams {
  static constexpr size_t kNumParams = 3;

  double mount_angle = 0.819433 / 180.0 * M_PI;  // 9.32615 / 180.0 * M_PI;
  double focal_length = 666.763;                 // 734.328;
  // This is a final rotation where the camera isn't straight.
  double barrel_mount = 2.72086 / 180.0 * M_PI;

  void set(double *data) {
    data[0] = mount_angle;
    data[1] = focal_length;
    data[2] = barrel_mount;
  }
  static IntrinsicParams get(const double *data) {
    IntrinsicParams out;
    out.mount_angle = data[0];
    out.focal_length = data[1];
    out.barrel_mount = data[2];
    return out;
  }
  void Dump(std::basic_ostream<char> *o) const;
};

// Metadata about the calibration results (Should be good enough to reproduce).
struct DatasetInfo {
  int camera_id;
  // In meters from IMU start.
  ::std::array<double, 2> to_tape_measure_start;
  // In meters,
  ::std::array<double, 2> tape_measure_direction;
  // This will multiply tape_measure_direction and thus has no units.
  double beginning_tape_measure_reading;
  const char *filename_prefix;
  int num_images;

  void Dump(std::basic_ostream<char> *o) const;
};

struct CameraCalibration {
  IntrinsicParams intrinsics;
  CameraGeometry geometry;
  DatasetInfo dataset;
};

const CameraCalibration *GetCamera(int camera_id);

// Serial number of the teensy for each robot.
constexpr uint32_t CodeBotTeensyId() { return 0xffff322e; }
constexpr uint32_t PracticeBotTeensyId() { return 0xffff3215; }
constexpr uint32_t CompBotTeensyId() { return 0xffff3210; }

// Get the IDs of the cameras in each port for a particular teensy board.
// inlined so that we don't have to deal with including it in the autogenerated
// constants.cc.
inline ::std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
  switch (processor_id) {
    case CodeBotTeensyId():
      return {{0, 0, 0, 16, 19}};
    case PracticeBotTeensyId():
      return {{14, 15, 18, 17, 1}};
    case CompBotTeensyId():
      return {{6, 7, 8, 9, 10}};
    default:
      return {{0, 0, 0, 0, 0}};
  }
}

// Rewrites constants.cc, adding camera calibration constants for the camera_id
// specified. If camera_id is less than zero, just rewrites the file without
// changing anything.
void DumpCameraConstants(const char *fname, int camera_id,
                         const CameraCalibration &value);

}  // namespace vision
}  // namespace y2019

#endif  // _Y2019_VISION_CONSTANTS_H_