text: /*
   * BSD 3-Clause License
   *
   * Copyright (c) 2019, Analog Devices, Inc.
   * All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions are met:
   *
   * 1. Redistributions of source code must retain the above copyright notice, this
   *    list of conditions and the following disclaimer.
   *
   * 2. Redistributions in binary form must reproduce the above copyright notice,
   *    this list of conditions and the following disclaimer in the documentation
   *    and/or other materials provided with the distribution.
   *
   * 3. Neither the name of the copyright holder nor the names of its
   *    contributors may be used to endorse or promote products derived from
   *    this software without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
   * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
   * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
   * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   */
  #include "calibration_96tof1.h"
  
  #include <aditof/eeprom_interface.h>
  #include <glog/logging.h>
  #include <math.h>
  
  #define EEPROM_SIZE 131072
  
  Calibration96Tof1::Calibration96Tof1()
      : m_depth_cache(nullptr), m_geometry_cache(nullptr), m_range(16000) {
      std::unordered_map<float, param_struct> Header;
      Header[EEPROM_VERSION].value = {0};
      Header[EEPROM_VERSION].size =
          sizeof(Header[EEPROM_VERSION].value.size()) * 4;
      Header[TOTAL_SIZE].value = {1000};
      Header[TOTAL_SIZE].size = sizeof(Header[TOTAL_SIZE].value.size()) * 4;
      Header[NUMBER_OF_MODES].value = {3};
      Header[NUMBER_OF_MODES].size = sizeof(Header[TOTAL_SIZE].value.size()) * 4;
  
      std::unordered_map<float, param_struct> CameraIntrinsic;
      CameraIntrinsic[EEPROM_VERSION].value = {0};
      CameraIntrinsic[EEPROM_VERSION].size =
          (uint32_t)(CameraIntrinsic[EEPROM_VERSION].value.size() * 4);
      CameraIntrinsic[CAL_SER_NUM].value = {0};
      CameraIntrinsic[CAL_SER_NUM].size =
          (uint32_t)(CameraIntrinsic[CAL_SER_NUM].value.size() * 4);
      CameraIntrinsic[CAL_DATE].value = {12042019};
      CameraIntrinsic[CAL_DATE].size =
          (uint32_t)(CameraIntrinsic[CAL_SER_NUM].value.size() * 4);
      CameraIntrinsic[INTRINSIC].value = {0, 0, 0, 0, 0, 0, 0, 0, 0};
      CameraIntrinsic[INTRINSIC].size =
          (uint32_t)(CameraIntrinsic[INTRINSIC].value.size() * 4);
  
      m_calibration_map[HEADER].size = (uint32_t)getPacketSize(Header);
      m_calibration_map[HEADER].packet = Header;
      m_calibration_map[CAMERA_INTRINSIC].size =
          (uint32_t)getPacketSize(CameraIntrinsic);
      m_calibration_map[CAMERA_INTRINSIC].packet = CameraIntrinsic;
      m_calibration_map[HEADER].packet[TOTAL_SIZE].value = {
          getMapSize(m_calibration_map)};
  }
  
  Calibration96Tof1::~Calibration96Tof1() {
      if (m_depth_cache) {
          delete[] m_depth_cache;
      }
  
      if (m_geometry_cache) {
          delete[] m_geometry_cache;
      }
  }
  
  //! DisplayCalMap - Display the entire calibration map
  /*!
      DisplayCalMap - Displays the entire calibration map.
      Calibratin map is nested hash map consiting of primary key( packet type key)
      and secondary key( parameter type key)
  */
  aditof::Status Calibration96Tof1::displayCalMap() const {
      using namespace aditof;
  
      for (const auto &mapElement : m_calibration_map) {
          std::cout << "Key: " << mapElement.first;
          const packet_struct &sub_packet_map = mapElement.second;
          std::cout << "\t Size: " << sub_packet_map.size << std::endl;
  
          for (const auto &packet : sub_packet_map.packet) {
              std::cout << "\tSub Key: " << packet.first;
              std::cout << "\tSub Size: " << packet.second.size;
              std::cout << "\tSub Value: ";
  
              for (const auto &value : packet.second.value)
                  std::cout << value << " ";
              std::cout << std::endl;
          }
      }
  
      return Status::OK;
  }
  
  //! SaveCalMap - Save the entire calibration map
  /*!
  SaveCalMap - Saves the entire calibration map as binary to a file.
  \eeprom - Reference to an eeprom instance
  */
  aditof::Status Calibration96Tof1::saveCalMap(aditof::EepromInterface &eeprom) {
      using namespace aditof;
  
      std::vector<float> data;
      for (const auto &mapElement : m_calibration_map) {
          data.push_back(mapElement.first);
          const packet_struct &sub_packet_map = mapElement.second;
          data.push_back((float)sub_packet_map.size);
  
          for (const auto &packet : sub_packet_map.packet) {
              data.push_back(packet.first); // write parameter key
              data.push_back(
                  (float)packet.second.size); // write size of parameter
  
              for (const auto &value : packet.second.value) {
                  data.push_back(
                      static_cast<float>(value)); // write parameter values
              }
          }
      }
  
      float size = static_cast<float>(data.size() * sizeof(uint32_t));
      eeprom.write((uint32_t)0, (uint8_t *)&size, (size_t)4);
      eeprom.write((uint32_t)4, (uint8_t *)data.data(), (size_t)size);
  
      return Status::OK;
  }
  
  //! ReadCalMap - Read the entire calibration map
  /*!
  ReadCalMap - Read the entire calibration map from a binary file
  \eeprom - Reference to an eeprom instance
  */
  aditof::Status Calibration96Tof1::readCalMap(aditof::EepromInterface &eeprom) {
      using namespace aditof;
  
      Status status = Status::OK;
      uint8_t *data;
      float read_size = 100;
      uint32_t j = 0;
      float key;
  
      eeprom.write(EEPROM_SIZE - 5, (uint8_t *)&read_size, 4);
  
      eeprom.read(0, (uint8_t *)&read_size, 4);
      LOG(INFO) << "EEPROM calibration data size " << read_size << " bytes";
  
      if (read_size > EEPROM_SIZE) {
          LOG(WARNING) << "Invalid calibration data size";
          return Status::GENERIC_ERROR;
      }
  
      data = (uint8_t *)malloc((size_t)read_size);
  
      status = eeprom.read(4, data, (size_t)read_size);
      if (status != Status::OK) {
          free(data);
          LOG(WARNING) << "Failed to read from eeprom";
          return status;
      }
  
      while (j < read_size) {
          key = *(float *)(data + j);
          j += 4;
  
          packet_struct sub_packet_map;
  
          sub_packet_map.size = (uint32_t) * (float *)(data + j);
          j += 4;
  
          for (unsigned int i = 0;
               i < sub_packet_map.size /
                       (sizeof(float));) // Parse all the sub-packets
          {
              float parameter_key;
              parameter_key =
                  *(float *)(data + j); // Parse key of parameter from sub packet
              j += 4;
              i++;
              sub_packet_map.packet[parameter_key].size =
                  (uint32_t) *
                  (float *)(data + j); // Parse size of parameter from sub packet
              j += 4;
              i++;
  
              uint32_t number_elements =
                  sub_packet_map.packet[parameter_key].size / sizeof(float);
              std::list<float> elements;
              for (unsigned int k = 0; k < number_elements; k++) {
                  sub_packet_map.packet[parameter_key].value.push_back(
                      *(float *)(data +
                                 j)); // Parse size of parameter from sub packet
                  j += 4;
                  i++;
              }
          }
          m_calibration_map[key].size = sub_packet_map.size;
          m_calibration_map[key].packet = sub_packet_map.packet;
      }
  
      free(data);
  
      return Status::OK;
  }
  
  //! getAfeFirmware - Get the firmware for a mode
  /*!
  getAfeFirmware - Get the firmware for a mode
  \param mode - Camera mode
  \param data - Buffer where to store the firmware
  */
  aditof::Status
  Calibration96Tof1::getAfeFirmware(const std::string &mode,
                                    std::vector<uint16_t> &data) const {
      using namespace aditof;
  
      uint8_t cal_mode;
  
      if (mode == "near") {
          cal_mode = 3;
      } else if (mode == "medium") {
          cal_mode = 5;
      } else if (mode == "far") {
          cal_mode = 7;
      } else {
          LOG(WARNING) << "Invalid firmware mode " << mode.c_str();
          return Status::INVALID_ARGUMENT;
      }
  
      for (const auto &mapElement : m_calibration_map) {
          float key = mapElement.first;
          const packet_struct &sub_packet_map = mapElement.second;
  
          if (cal_mode == key) {
              for (const auto &packet : sub_packet_map.packet) {
                  if (packet.first == 5) {
                      for (const auto &value : packet.second.value) {
                          data.push_back(static_cast<uint16_t>(value));
                      }
                      return Status::OK;
                  }
              }
          }
      }
  
      return Status::GENERIC_ERROR;
  }
  
  //! getGainOffset - Get the depth gain ad offset values for a mode
  /*!
  getGainOffset - Get the depth gain ad offset values for a mode
  \param mode - Camera mode
  \param gain - Stores the retuned gain value
  \param offset - Stores the retuned offset value
  */
  aditof::Status Calibration96Tof1::getGainOffset(const std::string &mode,
                                                  float &gain,
                                                  float &offset) const {
      using namespace aditof;
  
      uint8_t cal_mode;
  
      if (mode == "near") {
          cal_mode = 2;
      } else if (mode == "medium") {
          cal_mode = 4;
      } else if (mode == "far") {
          cal_mode = 6;
      } else {
          LOG(WARNING) << "Invalid firmware mode " << mode.c_str();
          return Status::INVALID_ARGUMENT;
      }
  
      for (const auto &mapElement : m_calibration_map) {
          float key = mapElement.first;
          const packet_struct &sub_packet_map = mapElement.second;
  
          if (cal_mode == key) {
              for (const auto &packet : sub_packet_map.packet) {
                  if (packet.first == 26) {
                      gain = packet.second.value.front();
                  }
                  if (packet.first == 27) {
                      offset = packet.second.value.front();
                  }
              }
              return Status::OK;
          }
      }
  
      return Status::GENERIC_ERROR;
  }
  
  //! getIntrinsic - Get the geometric camera calibration
  /*!
  getIntrinsic - Get the geometric camera clibration
  \param key - Specifies which calibration values to get: 
               intrinsincs or distortion coefficients 
  \param data - Buffer to store the returned data
  */
  aditof::Status Calibration96Tof1::getIntrinsic(float key,
                                                 std::vector<float> &data) const {
      using namespace aditof;
  
      bool validParam = (INTRINSIC == key) || (DISTORTION_COEFFICIENTS == key);
  
      if (!validParam) {
          LOG(WARNING) << "Invalid intrinsic " << std::to_string(key).c_str();
          return Status::INVALID_ARGUMENT;
      }
  
      for (const auto &mapElement : m_calibration_map) {
          float mapKey = mapElement.first;
          const packet_struct &sub_packet_map = mapElement.second;
  
          if (mapKey == CAMERA_INTRINSIC) {
              for (const auto &packet : sub_packet_map.packet) {
                  float packetKey = packet.first;
  
                  if (packetKey == key) {
                      const param_struct &param_map = packet.second;
                      const std::list<float> &valList = param_map.value;
  
                      data.insert(data.begin(), valList.begin(), valList.end());
                      return Status::OK;
                  }
              }
              LOG(WARNING) << "No intrinsics found in the device memory for key "
                           << std::to_string(key).c_str();
          }
      }
  
      return Status::GENERIC_ERROR;
  }
  
  //! setMode - Sets the mode to be used for depth calibration
  /*!
  setMode - Sets the mode to be used for depth calibration
  \param mode - Camera depth mode
  \param range - Max range for selected mode
  \param frameWidth - Width of the depth image in pixels
  \param frameHeight - Height of the depth image in pixels
  */
  aditof::Status Calibration96Tof1::setMode(const std::string &mode, int range,
                                            unsigned int frameWidth,
                                            unsigned int frameheight) {
      using namespace aditof;
  
      Status status = Status::OK;
      std::vector<float> cameraMatrix;
      const int16_t pixelMaxValue = (1 << 12) - 1; // 4095
      float gain = 1.0, offset = 0.0;
  
      status = getGainOffset(mode, gain, offset);
      if (status != Status::OK) {
          LOG(WARNING) << "Failed to read gain and offset from eeprom";
          return status;
      } else {
          LOG(INFO) << "Camera calibration parameters for mode: " << mode
                    << " are gain: " << gain << " "
                    << "offset: " << offset;
      }
      buildDepthCalibrationCache(gain, offset, pixelMaxValue, range);
      m_range = range;
  
      status = getIntrinsic(INTRINSIC, cameraMatrix);
      if (status != Status::OK) {
          LOG(WARNING) << "Failed to read intrinsic from eeprom";
          return status;
      } else {
          LOG(INFO) << "Camera intrinsic parameters:\n"
                    << "    fx: " << cameraMatrix[0] << "\n"
                    << "    fy: " << cameraMatrix[4] << "\n"
                    << "    cx: " << cameraMatrix[2] << "\n"
                    << "    cy: " << cameraMatrix[5];
      }
      buildGeometryCalibrationCache(cameraMatrix, frameWidth, frameheight);
  
      return status;
  }
  
  //! calibrateDepth - Calibrate the depth data
  /*!
  calibrateDepth - Calibrate the depth data using the gain and offset
  \param frame - Buffer with the depth data, used to return the calibrated data
  \param frame_size - Number of samples in the frame data
  */
  aditof::Status Calibration96Tof1::calibrateDepth(uint16_t *frame,
                                                   uint32_t frame_size) {
      using namespace aditof;
  
      uint16_t *cache = m_depth_cache;
  
      uint16_t *end = frame + (frame_size - frame_size % 8);
      uint16_t *framePtr = frame;
  
      for (; framePtr < end; framePtr += 8) {
          *framePtr = *(cache + *framePtr);
          *(framePtr + 1) = *(cache + *(framePtr + 1));
          *(framePtr + 2) = *(cache + *(framePtr + 2));
          *(framePtr + 3) = *(cache + *(framePtr + 3));
          *(framePtr + 4) = *(cache + *(framePtr + 4));
          *(framePtr + 5) = *(cache + *(framePtr + 5));
          *(framePtr + 6) = *(cache + *(framePtr + 6));
          *(framePtr + 7) = *(cache + *(framePtr + 7));
      }
  
      end += (frame_size % 8);
  
      for (; framePtr < end; framePtr++) {
          *framePtr = *(cache + *framePtr);
      }
  
      return Status::OK;
  }
  
  //! calibrateCameraGeometry - Compensate for lens distorsion in the depth data
  /*!
  calibrateCameraGeometry - Compensate for lens distorsion in the depth data
  \param frame - Buffer with the depth data, used to return the calibrated data
  \param frame_size - Number of samples in the frame data
  */
  aditof::Status Calibration96Tof1::calibrateCameraGeometry(uint16_t *frame,
                                                            uint32_t frame_size) {
      using namespace aditof;
  
      for (uint32_t i = 0; i < frame_size; i++) {
          if (frame[i] != m_range) {
              frame[i] = static_cast<uint16_t>(frame[i] * m_geometry_cache[i]);
          }
          if (frame[i] > m_range) {
              frame[i] = m_range;
          }
      }
  
      return Status::OK;
  }
  
  // Create a cache to speed up depth calibration computation
  void Calibration96Tof1::buildDepthCalibrationCache(float gain, float offset,
                                                     int16_t maxPixelValue,
                                                     int range) {
      if (m_depth_cache) {
          delete[] m_depth_cache;
      }
  
      m_depth_cache = new uint16_t[maxPixelValue + 1];
      for (int16_t current = 0; current <= maxPixelValue; ++current) {
          int16_t currentValue =
              static_cast<int16_t>(static_cast<float>(current) * gain + offset);
          m_depth_cache[current] = currentValue <= range ? currentValue : range;
      }
  }
  
  // Create a cache to speed up depth geometric camera calibration computation
  void Calibration96Tof1::buildGeometryCalibrationCache(
      const std::vector<float> &cameraMatrix, unsigned int width,
      unsigned int height) {
  
      float fx = cameraMatrix[0];
      float fy = cameraMatrix[4];
      float x0 = cameraMatrix[2];
      float y0 = cameraMatrix[5];
  
      if (m_geometry_cache) {
          delete[] m_geometry_cache;
      }
  
      m_geometry_cache = new double[width * height];
      for (uint16_t i = 0; i < height; i++) {
          for (uint16_t j = 0; j < width; j++) {
  
              double tanXAngle = (x0 - j) / fx;
              double tanYAngle = (y0 - i) / fy;
  
              m_geometry_cache[i * width + j] =
                  1.0 / sqrt(1 + tanXAngle * tanXAngle + tanYAngle * tanYAngle);
          }
      }
  }
  
  // Calculate and return the total size of calibration map
  float Calibration96Tof1::getMapSize(
      const std::unordered_map<float, packet_struct> &calibration_map) const {
      float total_size = 0;
      // Calculate total size of calibration map
      for (const auto &mapElement : calibration_map) {
          total_size += mapElement.second.size; // Add size of all the sub packets
      }
      return total_size;
  }
  
  // Calculate and return the size of a packet
  float Calibration96Tof1::getPacketSize(
      const std::unordered_map<float, param_struct> &packet) const {
      float packet_size = 0;
      for (const auto &mapElement : packet) {
          packet_size +=
              mapElement.second.size + 8; // Added 8 for size of key and size
      }
      return packet_size;
  }
  