#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

class ArduinoBridge : public rclcpp::Node
{
public:
  ArduinoBridge()
  : Node("arduino_bridge_cpp"), pos_(SERVO_HOME)
  {
    device_ = "/dev/ttyUSB0";
    baud_ = B9600;

    fd_ = open(device_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_ == -1) {
      RCLCPP_ERROR(this->get_logger(), "Failed to open serial port");
      exit(1);
    }
    fcntl(fd_, F_SETFL, 0);

    struct termios options;
    tcgetattr(fd_, &options);
    cfsetispeed(&options, baud_);
    cfsetospeed(&options, baud_);
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    tcsetattr(fd_, TCSANOW, &options);

    publisher_ = this->create_publisher<std_msgs::msg::Int32>("water_level", 10);

    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(1000),
      std::bind(&ArduinoBridge::loop, this));

    RCLCPP_INFO(this->get_logger(), "Arduino Bridge C++ Node Started.");
  }

private:
  const int SERVO_HOME = 0;
  const int SERVO_STEP = 45;
  const int SERVO_MAX = 180;
  const int WATER_THRESHOLD = 10;

  void loop()
  {
    int waterL = read_water_level();

    if (waterL >= 0) {
      auto msg = std_msgs::msg::Int32();
      msg.data = waterL;
      publisher_->publish(msg);
      RCLCPP_INFO(this->get_logger(), "Water Level: %d", waterL);

      // Servo logic: match Arduino sketch
      if (waterL > WATER_THRESHOLD) {
        pos_ += SERVO_STEP;
        if (pos_ >= SERVO_MAX) {
          pos_ = SERVO_HOME;
        }
      } else {
        pos_ = SERVO_HOME;
      }

      send_servo_pos(pos_);
      RCLCPP_INFO(this->get_logger(), "Sent Servo Pos: %d", pos_);
    }
  }

  int read_water_level()
  {
    char buf[256] = {0};
    int n = read(fd_, buf, sizeof(buf));
    if (n > 0) {
      std::string line(buf, n);
      size_t pos = line.find("Water Level:");
      if (pos != std::string::npos) {
        std::string val_str = line.substr(pos + 12); // length of "Water Level:"
        int val = std::stoi(val_str);
        return val;
      }
    }
    return -1;
  }

  void send_servo_pos(int pos)
  {
    std::string cmd = std::to_string(pos) + "\n";
    write(fd_, cmd.c_str(), cmd.size());
  }

  std::string device_;
  int baud_;
  int fd_;
  int pos_;

  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ArduinoBridge>());
  rclcpp::shutdown();
  return 0;
}

