//
// Created by lgv on 2025/8/24.
//

#pragma once

#include <utility>

#include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h"
namespace cmvr {
    namespace ctrl {

        // 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
        class PID {
        public:
            PID(double kp, double ki, double kd,
                double i_max,
                double output_max_pos, double output_max_neg,
                double delta_max = 0.0)   // 输出变化最大值，0 表示不限制
                : kp_(kp), ki_(ki), kd_(kd),
                  i_max_(i_max),
                  output_max_pos_(output_max_pos),
                  output_max_neg_(output_max_neg),
                  delta_max_(delta_max),
                  prev_error_(0), integral_(0), prev_output_(0) {}

            double compute(double target, double current, double dt, double deadband = 0.0) {
                double error = target - current;

                // 死区处理
                if (fabs(error) <= deadband) {
                    error = 0.0;
                }

                // 积分累加限幅
                integral_ += error * dt;
                if (integral_ > i_max_) integral_ = i_max_;
                if (integral_ < -i_max_) integral_ = -i_max_;

                // 微分
                double derivative = (error - prev_error_) / dt;
                prev_error_ = error;

                // PID 输出
                double output = kp_ * error + ki_ * integral_ + kd_ * derivative;

                // 输出限幅
                if (output > output_max_pos_) output = output_max_pos_;
                if (output < -output_max_neg_) output = -output_max_neg_;

                // 输出斜率限制
                if (delta_max_ > 0.0) {
                    double delta = output - prev_output_;
                    if (delta > delta_max_) output = prev_output_ + delta_max_;
                    else if (delta < -delta_max_) output = prev_output_ - delta_max_;
                }

                prev_output_ = output;
                return output;
            }

        private:
            double kp_, ki_, kd_;
            double prev_error_;
            double integral_;
            double i_max_;             // 积分限幅
            double output_max_pos_;    // 向下按的最大输出
            double output_max_neg_;    // 向上抬的最大输出
            double delta_max_;         // 输出斜率限制
            double prev_output_;
        };




        class TouchController {
        public:
            TouchController(){};
            TouchController( std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand)
            :robot_(std::move(robot)),hand_(std::move(hand)),
            pid_(std::make_shared<PID>(0.05, 0.001, 0.01, 50.0, 0.5, 1.0)){}
            ~TouchController()=default;

            void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
            void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);

        private:
            std::shared_ptr<device::AbstractRobot> robot_{nullptr};
            std::shared_ptr<device::AbstractDexHand> hand_{nullptr};

            std::shared_ptr<PID> pid_{nullptr};

            const double touch_threshold_ = 5.0;  // 触控判定阈值


            // 从压阻矩阵提取触控点与压力
            bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);

        };
    }
}