#include <bt_executor/admission.hpp>
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <bt_skill_interfaces/action/execute_task.hpp>
#include <std_msgs/msg/string.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <fstream>
#include <fcntl.h>
#include <unistd.h>
#include <cerrno>
#include <cstdio>

namespace bt_executor {
using namespace robot_bt;
struct Runtime {
  std::unique_ptr<StageRunner> runner;
  rclcpp::Node* node{nullptr};
  bool admitted{false}, clarification{false};
  TickStatus last{TickStatus::RUNNING};
  std::string stage;
};
class StageNode final:public BT::StatefulActionNode {
 public:
  StageNode(const std::string& name,const BT::NodeConfig& config):StatefulActionNode(name,config){}
  static BT::PortsList providedPorts(){return {BT::InputPort<std::string>("stage")};}
  BT::NodeStatus onStart()override{return step();}
  BT::NodeStatus onRunning()override{return step();}
  void onHalted()override {
    auto runtime=config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
    if(runtime->runner)runtime->runner->halt(SteadyClock::now());
  }
 private:
  BT::NodeStatus step() {
    auto runtime=config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
    auto name=getInput<std::string>("stage");if(!name)throw BT::RuntimeError(name.error());
    for(auto stage:fixed_stages())if(name.value()==stage_name(stage)) {
      runtime->stage=name.value();runtime->last=runtime->runner->tick(stage,SteadyClock::now(),runtime->node->now().nanoseconds());
      switch(runtime->last){case TickStatus::RUNNING:return BT::NodeStatus::RUNNING;
        case TickStatus::SUCCESS:return BT::NodeStatus::SUCCESS;default:return BT::NodeStatus::FAILURE;}
    }
    throw BT::RuntimeError("unknown fixed stage");
  }
};
static void append_receipt(const std::string& path,const json& data) {
  const auto line=data.dump()+"\n";int fd=::open(path.c_str(),O_WRONLY|O_APPEND|O_CREAT,0600);
  if(fd<0)throw std::runtime_error("receipt journal unavailable");
  std::size_t offset=0;while(offset<line.size()) {
    auto n=::write(fd,line.data()+offset,line.size()-offset);if(n<0&&errno==EINTR)continue;
    if(n<=0){::close(fd);throw std::runtime_error("receipt journal write failed");}offset+=n;
  }
  auto ok=::fsync(fd);::close(fd);if(ok)throw std::runtime_error("receipt journal sync failed");
}
class ExecutorNode final:public rclcpp::Node {
 public:
  using Action=iface::action::ExecuteTask;
  using Handle=rclcpp_action::ServerGoalHandle<Action>;
  ExecutorNode():Node("bt_executor") {
    robot_id_=declare_parameter<std::string>("robot_id","");
    auto allowed=declare_parameter<std::vector<std::string>>("allowed_robots",std::vector<std::string>{});
    enabled_=declare_parameter<bool>("execution_enabled",false);
    require(!robot_id_.empty()&&std::find(allowed.begin(),allowed.end(),robot_id_)!=allowed.end(),"configured robot_id must be whitelisted");
    const auto site_path=declare_parameter<std::string>("site_config_file","");
    require(!site_path.empty(),"trusted site_config_file required");
    std::ifstream stream(site_path);require(stream.good(),"cannot open trusted site file");
    const std::string raw((std::istreambuf_iterator<char>(stream)),{});trusted_=strict_json(raw);site_=load_site(trusted_);
    journal_dir_=declare_parameter<std::string>("journal_directory","");
    require(!journal_dir_.empty(),"persistent journal_directory required");
    std::filesystem::create_directories(journal_dir_);
    const double confidence=declare_parameter<double>("minimum_confidence",0.9);
    const auto lifetime=declare_parameter<std::int64_t>("observation_lifetime_ms",2000);
    require(confidence>0&&confidence<=1&&lifetime>0&&lifetime<=10000,"invalid observation policy");
    const auto timeout_policy=[this](const char* name,std::int64_t default_ms) {
      // ROS integer parameters reject floating/non-finite values before this
      // bounded conversion; milliseconds must also fit the outbound Duration.
      const auto value=declare_parameter<std::int64_t>(name,default_ms);
      require(value>0&&value<=3600000,std::string(name)+" must be 1..3600000 ms");
      return Milliseconds(value);
    };
    budgets_.readiness=timeout_policy("readiness_timeout_ms",2000);
    budgets_.acceptance=timeout_policy("acceptance_timeout_ms",2000);
    budgets_.feedback=timeout_policy("feedback_timeout_ms",5000);
    budgets_.execution=timeout_policy("skill_timeout_ms",120000);
    budgets_.cancel_stop=timeout_policy("cancel_stop_timeout_ms",5000);
    const auto reobservations=declare_parameter<std::int64_t>("max_reobservations",2);
    const auto adjustments=declare_parameter<std::int64_t>("max_posture_adjustments",1);
    require(reobservations>=0&&reobservations<=2,"max_reobservations must be 0..2");
    require(adjustments>=0&&adjustments<=1,"max_posture_adjustments must be 0..1");
    max_reobservations_=static_cast<unsigned>(reobservations);
    max_posture_adjustments_=static_cast<unsigned>(adjustments);
    driver_=std::make_unique<RosDriver>(*this,robot_id_,journal_dir_+"/ros_goal_uuids.jsonl",confidence,lifetime*1000000LL,budgets_.execution);
    registry_=std::make_unique<ActiveGoalRegistry>(*driver_,journal_dir_+"/goal_registry.log",budgets_);
    receipt_path_=journal_dir_+"/deliveries.jsonl";
    task_journal_=journal_dir_+"/task_runs.jsonl";
    std::ifstream task_history(task_journal_);std::string task_line;
    while(std::getline(task_history,task_line))if(!task_line.empty()) {
      const auto record=strict_json(task_line);seen_runs_.insert(record.at("run_id").get<std::string>());
      faulted_=record.at("state")!="RELEASED";
    }
    std::ifstream receipts(receipt_path_);std::string line;
    while(std::getline(receipts,line))if(!line.empty()) {
      auto receipt=strict_json(line);receipts_[json::array({receipt.at("task_id"),receipt.at("item_index")}).dump()]=receipt;
    }
    factory_.registerNodeType<StageNode>("RunStage");
    factory_.registerSimpleCondition("ApprovedPlanGate",[](BT::TreeNode& node) {
      return node.config().blackboard->get<std::shared_ptr<Runtime>>("runtime")->admitted?BT::NodeStatus::SUCCESS:BT::NodeStatus::FAILURE;
    });
    factory_.registerSimpleAction("RequestClarification",[](BT::TreeNode& node) {
      auto rt=node.config().blackboard->get<std::shared_ptr<Runtime>>("runtime");
      rt->clarification=true;rt->stage="NeedsClarification";return BT::NodeStatus::FAILURE;
    });
    // The only XML comes from this installed package. No action field, plan, or
    // parameter can select or inject another XML file.
    factory_.registerBehaviorTreeFromFile(ament_index_cpp::get_package_share_directory("bt_executor")+"/trees/fixed_workflow.xml");
    registry_pub_=create_publisher<std_msgs::msg::String>("goal_registry",rclcpp::QoS(1).reliable().transient_local());
    server_=rclcpp_action::create_server<Action>(this,declare_parameter<std::string>("execute_task_action","tasks/execute"),
      [this](const rclcpp_action::GoalUUID&,std::shared_ptr<const Action::Goal> goal) {
        if(!enabled_||faulted_||active_||reserved_||registry_->robot_locked(robot_id_)||driver_->faulted())return rclcpp_action::GoalResponse::REJECT;
        try {
          auto plan=strict_json(goal->approved_plan_json),context=strict_json(goal->context_json);
          auto task=admit(plan,context,goal->trace,trusted_,robot_id_);
          require(!receipts_.count(json::array({task.trace.task_id,task.item_index}).dump()),"task already delivered; reconcile receipt without replay");
          require(!seen_runs_.count(task.trace.run_id),"execution run already dispatched");
          require(goal->timeout.sec>0&&goal->timeout.sec<=3600&&goal->timeout.nanosec<1000000000,"invalid task timeout");
          reserved_=true;return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
        }catch(const std::exception& e){RCLCPP_WARN(get_logger(),"Task rejected: %s",e.what());return rclcpp_action::GoalResponse::REJECT;}
      },
      [this](std::shared_ptr<Handle> handle) {
        if(!active_||handle!=active_)return rclcpp_action::CancelResponse::REJECT;
        cancel_requested_=true;return rclcpp_action::CancelResponse::ACCEPT;
      },
      [this](std::shared_ptr<Handle> handle){accept(std::move(handle));});
    timer_=create_wall_timer(std::chrono::milliseconds(50),[this]{tick();});
  }
 private:
  std::string robot_id_,journal_dir_,receipt_path_,task_journal_;
  json trusted_;SiteConfig site_;Budgets budgets_;
  unsigned max_reobservations_{2},max_posture_adjustments_{1};
  bool enabled_{false},faulted_{false},reserved_{false},cancel_requested_{false},halting_{false},timed_out_{false};
  std::unique_ptr<RosDriver> driver_;
  std::unique_ptr<ActiveGoalRegistry> registry_;
  std::unique_ptr<ContextStore> context_;
  std::shared_ptr<Runtime> runtime_;
  BT::BehaviorTreeFactory factory_;
  std::optional<BT::Tree> tree_;
  std::shared_ptr<Handle> active_;
  rclcpp_action::Server<Action>::SharedPtr server_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr registry_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::map<std::string,json> receipts_;
  std::set<std::string> seen_runs_;
  unsigned quantity_{0};std::uint32_t sequence_{0};std::uint64_t tick_count_{0};
  SteadyTime deadline_{};
  std::string requested_status_,detail_,active_receipt_key_;
  void accept(std::shared_ptr<Handle> handle) {
    active_=std::move(handle);reserved_=false;cancel_requested_=false;halting_=false;timed_out_=false;
    quantity_=0;sequence_=0;active_receipt_key_.clear();detail_.clear();requested_status_.clear();
    try {
      const auto goal=active_->get_goal();auto task=admit(strict_json(goal->approved_plan_json),strict_json(goal->context_json),goal->trace,trusted_,robot_id_);
      const auto epoch=driver_->geometry_epoch();require(epoch.has_value(),"fresh RobotState geometry epoch required");
      active_receipt_key_=json::array({task.trace.task_id,task.item_index}).dump();
      task.initial_geometry_epoch=*epoch;
      task.max_reobservations=max_reobservations_;
      task.max_posture_adjustments=max_posture_adjustments_;
      append_receipt(task_journal_,{{"task_id",task.trace.task_id},{"run_id",task.trace.run_id},{"state","ACTIVE"}});
      seen_runs_.insert(task.trace.run_id);
      driver_->bind_task(task,site_,trusted_.at("registry_version").get<std::uint32_t>());
      deadline_=SteadyClock::now()+std::chrono::seconds(goal->timeout.sec)+std::chrono::nanoseconds(goal->timeout.nanosec);
      context_=std::make_unique<ContextStore>();runtime_=std::make_shared<Runtime>();runtime_->node=this;runtime_->admitted=true;
      runtime_->runner=std::make_unique<StageRunner>(task,site_,*driver_,*registry_,*context_,
        [this,task](const std::string& id,unsigned item,const std::string& verification) {
          if(item!=task.item_index||id!=task.trace.task_id||verification.empty())return false;
          if(!receipts_.count(active_receipt_key_)) {
            const auto* record=registry_->find(verification);
            if(!record||!record->result||!record->result->response.evidence||
               !record->result->response.verified||!record->result->response.in_destination||
               record->result->response.holding!=Holding::EMPTY)return false;
            const auto& proof=record->result->response;
            json receipt={{"task_id",id},{"item_index",item},{"run_id",task.trace.run_id},
              {"verification_goal_id",verification},{"target_id",task.target_id},{"destination_id",task.destination_id},
              {"verified_at_ns",now().nanoseconds()},{"evidence_id",verification},{"target_ref",proof.target_id},
              {"destination_ref",proof.destination_id},{"passed",proof.verified},{"empty_hand",proof.holding==Holding::EMPTY},
              {"in_destination",proof.in_destination},{"valid",proof.valid},
              {"observed_at_ns",proof.evidence->observed_at},{"valid_until_ns",proof.evidence->valid_until}};
            append_receipt(receipt_path_,receipt);receipts_[active_receipt_key_]=receipt;
          }
          quantity_=1;return true;
        },budgets_);
      auto blackboard=BT::Blackboard::create();blackboard->set("runtime",runtime_);
      tree_.emplace(factory_.createTree("TaskRoot",blackboard));
    }catch(const std::exception& e){detail_=e.what();finish("INTERVENTION_REQUIRED",false);}
  }
  void begin_halt(const std::string& status) {
    if(halting_)return;halting_=true;requested_status_=status;
    if(tree_)tree_->haltTree();if(runtime_&&runtime_->runner)runtime_->runner->halt(SteadyClock::now());
  }
  void finish(const std::string& requested_status,bool registry_stop_confirmed) {
    if(!active_)return;
    SafetySnapshot state;
    try {
      const auto config=strict_json(active_->get_goal()->context_json);
      state=driver_->safety(config.at("target_id").get<std::string>());
    }catch(const std::exception& e){detail_=std::string("invalid final context: ")+e.what();}
    const bool stop_confirmed=registry_stop_confirmed&&state.stationary;
    const bool physical_release=stop_confirmed&&state.safe&&state.holding==Holding::EMPTY&&
      runtime_&&runtime_->runner&&runtime_->runner->empty_verified(now().nanoseconds());
    std::string status=physical_release?requested_status:"INTERVENTION_REQUIRED";
    if(!physical_release&&detail_.empty())detail_="fresh final empty-hand/stationary/safe evidence missing";
    const auto id=active_->get_goal()->trace.task_id;
    json evidence={{"status",status},{"stop_confirmed",stop_confirmed},{"completed_quantity",quantity_},
      {"detail",detail_},{"goal_uuid_mappings",driver_->mappings()},
      {"safe_to_release",physical_release&&status!="INTERVENTION_REQUIRED"},
      {"current_empty_hand",state.holding==Holding::EMPTY},{"current_stationary",state.stationary}};
    if(receipts_.count(active_receipt_key_)) {
      const auto& receipt=receipts_.at(active_receipt_key_);evidence["delivery"]=receipt;
      for(const auto* key:{"evidence_id","target_ref","destination_ref","passed","empty_hand","in_destination","valid"})
        if(receipt.contains(key))evidence[key]=receipt.at(key);
    } else {evidence["empty_hand"]=physical_release;evidence["valid"]=physical_release;}
    if(runtime_&&runtime_->clarification)evidence["needs_clarification"]=true;
    const bool release=physical_release&&status!="INTERVENTION_REQUIRED";
    try {
      append_receipt(task_journal_,{{"task_id",id},{"run_id",active_->get_goal()->trace.run_id},
        {"state",release?"RELEASED":"QUARANTINED"},{"status",status}});
    }catch(const std::exception& e) {
      faulted_=true;status="INTERVENTION_REQUIRED";evidence["journal_error"]=e.what();
      evidence["status"]=status;evidence["safe_to_release"]=false;
    }
    if(!release)faulted_=true;
    auto result=std::make_shared<Action::Result>();
    result->result.status=status=="SUCCEEDED"?0:status=="CANCELED"?2:timed_out_?3:1;
    result->result.stop_state=stop_confirmed?1:0;result->result.message=detail_;
    result->result.error_code=status;result->completed_quantity=quantity_;
    if(stop_confirmed){result->result.stopped_at=now();result->result.stop_evidence_ref="registry/"+active_->get_goal()->trace.run_id;}
    result->evidence_json=evidence.dump();
    if(status=="SUCCEEDED")active_->succeed(result);
    else if(status=="CANCELED"&&active_->is_canceling())active_->canceled(result);
    else active_->abort(result);
    tree_.reset();runtime_.reset();context_.reset();active_.reset();reserved_=false;
    // registry_ and driver_ survive the tree, including STOP_UNKNOWN entries.
  }
  void publish_registry() {
    json report={{"robot_id",robot_id_},{"locked",registry_->robot_locked(robot_id_)},{"faulted",faulted_||driver_->faulted()},
                 {"uuid_mappings",driver_->mappings()},{"goals",json::array()}};
    for(const auto& entry:registry_->records()) {
      const auto& r=entry.second;report["goals"].push_back({{"goal_id",entry.first},{"task_id",r.request.trace.task_id},
        {"run_id",r.request.trace.run_id},{"skill",r.request.skill==Skill::PICK?"pick":r.request.skill==Skill::PLACE?"place":"other"},
        {"target_id",r.request.target_id},{"capture_after_ns",r.request.capture_after},
        {"trace",{{"task_id",r.request.trace.task_id},{"subtask_id",r.request.trace.subtask_id},{"run_id",r.request.trace.run_id},{"task_revision",r.request.trace.task_revision},{"plan_version",r.request.trace.plan_version},{"execution_generation",r.request.trace.execution_generation},{"attempt",r.request.trace.attempt}}},
        {"state",static_cast<int>(r.state)},{"cancel_intent",r.cancel_intent},
        {"accepted",r.accepted},{"restarted",r.restarted}});
    }
    std_msgs::msg::String m;m.data=report.dump();registry_pub_->publish(m);
  }
  void tick() {
    const auto steady=SteadyClock::now();
    try {
      registry_->pump(steady);if(++tick_count_%20==0)publish_registry();
      if(!active_)return;
      if(cancel_requested_)begin_halt("CANCELED");
      if(steady>=deadline_&&!halting_){timed_out_=true;detail_="task execution deadline";begin_halt("FAILED");}
      if(driver_->faulted()){detail_="driver durability fault";begin_halt("INTERVENTION_REQUIRED");}
      if(!halting_) {
        const auto config=strict_json(active_->get_goal()->context_json);
        runtime_->runner->update_safety(driver_->safety(config.at("target_id").get<std::string>()));
        const auto state=tree_->tickOnce();
        if(state==BT::NodeStatus::SUCCESS){detail_=runtime_->runner->detail();begin_halt(quantity_==1?"SUCCEEDED":"FAILED");}
        else if(state==BT::NodeStatus::FAILURE) {
          detail_=runtime_->runner->detail();
          if(runtime_->clarification)detail_="execution requires clarification; replan before motion";
          begin_halt(runtime_->last==TickStatus::INTERVENTION_REQUIRED?"INTERVENTION_REQUIRED":"FAILED");
        }
      }
      if(active_) {
        auto feedback=std::make_shared<Action::Feedback>();feedback->stamp=now();feedback->sequence=++sequence_;
        feedback->stage=halting_?"Stopping":runtime_->stage;
        feedback->status_json=json({{"cancel_requested",cancel_requested_},{"completed_quantity",quantity_},
          {"registry_locked",registry_->robot_locked(robot_id_)},{"detail",detail_}}).dump();active_->publish_feedback(feedback);
      }
      if(halting_) {
        bool unknown=false;for(const auto& kv:registry_->records())if(kv.second.request.robot_id==robot_id_&&kv.second.state==GoalState::STOP_UNKNOWN)unknown=true;
        if(unknown)finish("INTERVENTION_REQUIRED",false);
        else if(runtime_&&runtime_->runner) {
          const auto config=strict_json(active_->get_goal()->context_json);
          runtime_->runner->update_safety(driver_->safety(config.at("target_id").get<std::string>()));
          const auto settled=runtime_->runner->settle(steady,now().nanoseconds());
          if(settled==TickStatus::SUCCESS)finish(requested_status_,true);
          else if(settled!=TickStatus::RUNNING)finish("INTERVENTION_REQUIRED",!registry_->robot_locked(robot_id_));
        } else if(!registry_->robot_locked(robot_id_))finish("INTERVENTION_REQUIRED",true);
      }
    }catch(const std::exception& e) {
      faulted_=true;detail_=std::string("executor fault: ")+e.what();
      try{begin_halt("INTERVENTION_REQUIRED");}catch(...){}
      if(active_)finish("INTERVENTION_REQUIRED",false);
      RCLCPP_ERROR(get_logger(),"%s",detail_.c_str());
    }
  }
};
}  // namespace bt_executor
int main(int argc,char** argv) {
  rclcpp::init(argc,argv);
  try {
    auto node=std::make_shared<bt_executor::ExecutorNode>();
    rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(node);executor.spin();
  }catch(const std::exception& e){std::fprintf(stderr,"bt_executor startup failed: %s\n",e.what());rclcpp::shutdown();return 1;}
  rclcpp::shutdown();return 0;
}