#include "rideheight_sensing/rideheight_sensing.h"

#include <optional>

#include "vl53l0x_api.h"
#include "vl53l0x_i2c_comms.h"
#include "vl6180x_api.h"
#include "vl6180x_glue.h"

#include <ros/assert.h>
#include <sensor_msgs/Range.h>

namespace rh_sensing {
RideheightSensing::RideheightSensing(ros::NodeHandle nh,
                                     ros::NodeHandle nh_private)
    : nh{nh}, nh_private{nh_private},
      vl6180x_pub{nh.advertise<sensor_msgs::Range>("vl6180x", 10)},
      vl53l0x_pub{nh.advertise<sensor_msgs::Range>("vl53l0x", 10)} {
  // init_6180("/dev/i2c-8");
  init_53l0("/dev/i2c-8");
}

void RideheightSensing::spin() {
  sensor_msgs::Range vl6180x_msg, vl53l0x_msg;
  vl6180x_msg.radiation_type = vl53l0x_msg.radiation_type =
      sensor_msgs::Range::INFRARED;
  vl6180x_msg.field_of_view = vl53l0x_msg.field_of_view = 25.0f / 180 * M_PI;
  vl6180x_msg.min_range = 0.005f;
  vl6180x_msg.max_range = 0.1f;
  vl53l0x_msg.min_range = 0.05f;
  vl53l0x_msg.max_range = 1.0f;

  ros::Rate loop_rate(20);
  std::optional<ros::Time> first_signal_fail = std::nullopt;
  while (ros::ok()) {
    //   VL6180x_RangePollMeasurement(vl6180x_dev, &vl6180x_range);
    //   if (vl6180x_range.errorStatus == 0) {
    //     vl6180x_msg.header.stamp = ros::Time::now();
    //     vl6180x_msg.range = vl6180x_range.range_mm / 1000.0f;
    //     vl6180x_pub.publish(vl6180x_msg);
    //   } else {
    //     ROS_WARN("VL6180X Ranging error: %s",
    //              VL6180x_RangeGetStatusErrString(vl6180x_range.errorStatus));
    //   }
    VL53L0X_Error status =
        VL53L0X_PerformSingleRangingMeasurement(&vl53l0x_dev, &vl53l0x_range);
    if (status != VL53L0X_ERROR_NONE) {
      char str[512];
      ROS_ASSERT(VL53L0X_GetDeviceErrorString(status, str) ==
                 VL53L0X_ERROR_NONE);
      ROS_WARN("VL53L0X Ranging error: %s", str);
    } else if (vl53l0x_range.RangeStatus != 0) {
      if (!vl53l0x_long_range && vl53l0x_range.RangeStatus == 2) {
        // Signal fail
        if (first_signal_fail.has_value()) {
          if (ros::Time::now() - *first_signal_fail > ros::Duration(1)) {
            ROS_WARN("VL53L0X signal has failed for more than 1s, switching to "
                     "long range mode");
            config_53l0(true);
          }
        } else {
          first_signal_fail = ros::Time::now();
        }
      } else {
        char str[512];
        ROS_ASSERT(
            VL53L0X_GetRangeStatusString(vl53l0x_range.RangeStatus, str) == 0);
        ROS_WARN("VL53L0X Ranging status: %s", str);
      }
    } else {
      first_signal_fail = std::nullopt;

      vl53l0x_msg.header.stamp = ros::Time::now();
      vl53l0x_msg.range = vl53l0x_range.RangeMilliMeter / 1000.0f;
      vl53l0x_pub.publish(vl53l0x_msg);
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

void RideheightSensing::init_6180(const char *i2c_dev_path) {
  VL6180x_glue_init(i2c_dev_path);
  VL6180x_InitData(vl6180x_dev);
  VL6180x_Prepare(vl6180x_dev);
}

void RideheightSensing::init_53l0(const char *i2c_dev_path) {
  VL53L0X_i2c_init(i2c_dev_path, &vl53l0x_dev);

  // Adapted from Adafruit's VL53L0X library
  VL53L0X_Error status = VL53L0X_DataInit(&vl53l0x_dev);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in DataInit: %d", status);

  VL53L0X_DeviceInfo_t device_info;
  status = VL53L0X_GetDeviceInfo(&vl53l0x_dev, &device_info);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in GetDeviceInfo: %d",
                 status);
  ROS_INFO("[VL53L0X] Device Name: %s, Type: %s, ID: %s", device_info.Name,
           device_info.Type, device_info.ProductId);
  ROS_INFO("[VL53L0X] Rev: %u.%u", device_info.ProductRevisionMajor,
           device_info.ProductRevisionMinor);

  status = VL53L0X_StaticInit(&vl53l0x_dev);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in StaticInit: %d",
                 status);

  uint32_t ref_spad_count;
  uint8_t is_aperture_spads;
  status = VL53L0X_PerformRefSpadManagement(&vl53l0x_dev, &ref_spad_count,
                                            &is_aperture_spads);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                 "Error in PerformRefSpadManagement: %d", status);
  ROS_INFO("Ref spad count: %u, is aperture spads: %u", ref_spad_count,
           is_aperture_spads);

  uint8_t vhv_settings, phase_cal;
  status =
      VL53L0X_PerformRefCalibration(&vl53l0x_dev, &vhv_settings, &phase_cal);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                 "Error in PerformRefCalibration: %d", status);

  status =
      VL53L0X_SetDeviceMode(&vl53l0x_dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE, "Error in SetDeviceMode: %d",
                 status);

  // Enable/Disable Sigma and Signal check
  status = VL53L0X_SetLimitCheckEnable(
      &vl53l0x_dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                 "Error in SetLimitCheckEnable: %d", status);

  status = VL53L0X_SetLimitCheckEnable(
      &vl53l0x_dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
  ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                 "Error in SetLimitCheckEnable: %d", status);

  config_53l0(false);
}

void RideheightSensing::config_53l0(bool long_range) {
  this->vl53l0x_long_range = long_range;
  if (long_range) {
    VL53L0X_Error status = VL53L0X_SetLimitCheckEnable(
        &vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetLimitCheckEnable: %d", status);

    status = VL53L0X_SetLimitCheckValue(
        &vl53l0x_dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
        (FixPoint1616_t)(0.1 * 65536));
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetLimitCheckValue: %d", status);
    status = VL53L0X_SetLimitCheckValue(&vl53l0x_dev,
                                        VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
                                        (FixPoint1616_t)(60 * 65536));
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetLimitCheckValue: %d", status);
    status =
        VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&vl53l0x_dev, 33000);

    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetMeasurementTimingBudgetMicroSeconds: %d",
                   status);
    status = VL53L0X_SetVcselPulsePeriod(&vl53l0x_dev,
                                         VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetVcselPulsePeriod: %d", status);
    status = VL53L0X_SetVcselPulsePeriod(&vl53l0x_dev,
                                         VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetVcselPulsePeriod: %d", status);
  } else {
    VL53L0X_Error status = VL53L0X_SetLimitCheckEnable(
        &vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1);
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetLimitCheckEnable: %d", status);

    status = VL53L0X_SetLimitCheckValue(
        &vl53l0x_dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
        (FixPoint1616_t)(1.5 * 0.023 * 65536));
    ROS_ASSERT_MSG(status == VL53L0X_ERROR_NONE,
                   "Error in SetLimitCheckEnable: %d", status);
  }
}

} // namespace rh_sensing