text: #ifndef DRIVE_VFLOOKUP_H_
  #define DRIVE_VFLOOKUP_H_
  
  #include <math.h>
  #include <stdint.h>
  #include <cmath>
  #include <algorithm>
  
  class ValueFuncLookup {
   public:
    ValueFuncLookup() {
      h_ = w_ = a_ = v_ = 0;
      scale_ = 1.;
      data_ = NULL;
    }
    ~ValueFuncLookup();
  
    bool Init(const char *fname);
  
    static float h2f(uint16_t h) {
      typedef union {
        uint32_t u;
        float f;
      } FP32;
      static const FP32 magic = {(254 - 15) << 23};
      FP32 o;
  
      o.u = (h & 0x7fff) << 13;   // exponent/mantissa bits
      o.f *= magic.f;             // exponent adjust
      o.u |= (h & 0x8000) << 16;  // sign bit
      return o.f;
    }
  
    float V(float x, float y, float theta, float v) {
      float ftheta = fmodf(theta * a_ * 1.0/(2*M_PI), a_);
      if (ftheta < 0)
        ftheta += a_;
      int itheta = std::floor(ftheta);
      ftheta -= itheta;
      // due to fp precision issues, we might still be rounded to a_ here
      if (itheta >= a_)
        itheta -= a_;
      float fv = std::min(std::max(v - vmin_, 0.0f), v_ - 1.0f);
      int iv = std::floor(fv);
      fv -= iv;
      float fx = x * scale_;
      int ix = std::floor(fx);
      fx -= ix;
      float fy = -y * scale_;
      int iy = std::floor(fy);
      fy -= iy;
      if (ix < 0 || ix >= w_ - 1 || iy < 0 || iy >= h_ - 1)
        return 1000.0f;
  
      int idx_xy = ix + iy * w_;
      int di = idx_xy + itheta * w_ * h_ + iv * w_ * h_ * a_;
  
      int nexttheta =
          itheta < a_ - 1 ? w_ * h_ : -w_ * h_ * (a_ - 1);
  
      //     vtyx
      float V0000 = h2f(data_[di]);
      float V0001 = h2f(data_[di + 1]);
      float V0010 = h2f(data_[di + w_]);
      float V0011 = h2f(data_[di + w_ + 1]);
      float V0100 = h2f(data_[di + nexttheta]);
      float V0101 = h2f(data_[di + nexttheta + 1]);
      float V0110 = h2f(data_[di + nexttheta + w_]);
      float V0111 = h2f(data_[di + nexttheta + w_ + 1]);
      float V1000 = V0000;
      float V1001 = V0001;
      float V1010 = V0010;
      float V1011 = V0011;
      float V1100 = V0100;
      float V1101 = V0101;
      float V1110 = V0110;
      float V1111 = V0111;
      if (iv < v_ - 1) {
        V1000 = h2f(data_[di + w_ * h_ * a_]);
        V1001 = h2f(data_[di + w_ * h_ * a_ + 1]);
        V1010 = h2f(data_[di + w_ * h_ * a_ + w_]);
        V1011 = h2f(data_[di + w_ * h_ * a_ + w_ + 1]);
        V1100 = h2f(data_[di + w_ * h_ * a_ + nexttheta]);
        V1101 = h2f(data_[di + w_ * h_ * a_ + nexttheta + 1]);
        V1110 = h2f(data_[di + w_ * h_ * a_ + nexttheta + w_]);
        V1111 = h2f(data_[di + w_ * h_ * a_ + nexttheta + w_ + 1]);
      }
      // lerp
      return (1 - fv) *
                 ((1 - ftheta) * ((1 - fy) * ((1 - fx) * V0000 + fx * V0001) +
                                  fy * ((1 - fx) * V0010 + fx * V0011)) +
                  ftheta * ((1 - fy) * ((1 - fx) * V0100 + fx * V0101) +
                            fy * ((1 - fx) * V0110 + fx * V0111))) +
             fv * ((1 - ftheta) * ((1 - fy) * ((1 - fx) * V1000 + fx * V1001) +
                                   fy * ((1 - fx) * V1010 + fx * V1011)) +
                   ftheta * ((1 - fy) * ((1 - fx) * V1100 + fx * V1101) +
                             fy * ((1 - fx) * V1110 + fx * V1111)));
    }
  
   private:
    // height, width, number of angles, number of velocities
    int h_, w_, a_, v_;
    float scale_;  // meters / pixel
    float vmin_;
    uint16_t *data_;
  };
  
  #endif  // DRIVE_VFLOOKUP_H_
  