#include "robot_bt/core.hpp"
#include <cassert>
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <limits>
#include <stdexcept>
using namespace robot_bt;
struct Driver final : GoalDriver {
  bool available{true};
  std::vector<GoalRequest> sent;
  std::vector<std::string> cancellations;
  std::vector<GoalEvent> events;
  bool ready(Skill) const override { return available; }
  void send(const GoalRequest& request) override { sent.push_back(request); }
  void cancel(const std::string& id) override { cancellations.push_back(id); }
  std::vector<GoalEvent> drain_events() override { auto result=events; events.clear(); return result; }
};
std::string journal(const std::string& name) {
 static const std::string root=[](){char path[]="/tmp/robot_bt_registry_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();
 return root+"/"+name+".journal";
}
GoalRequest request(unsigned attempt=1) {
 GoalRequest q; q.robot_id="sim_robot"; q.trace={"task","nav","run",1,1,1,attempt}; q.skill=Skill::NAVIGATE; return q;
}
GoalEvent event(const GoalRecord& r, EventKind kind) { GoalEvent e; e.goal_id=r.request.goal_id; e.trace=r.request.trace; e.kind=kind; return e; }
void test_journal_excludes_second_executor() {
 Driver d; auto path=journal("process_lock"); ActiveGoalRegistry first(d,path); bool refused=false;
 try { ActiveGoalRegistry second(d,path); } catch(const std::runtime_error&) { refused=true; }
 assert(refused);
}
void test_one_send_and_restart_lock() {
 Driver d; auto path=journal("one"); auto now=SteadyTime{}; std::string id;
 {
 ActiveGoalRegistry r(d,path);
 auto started=r.start(request(),now); assert(started && d.sent.size()==1); id=*started;
 for(int i=0;i<50;++i) { r.pump(now); auto second=r.start(request(),now); assert(!second); }
 assert(d.sent.size()==1 && r.robot_locked("sim_robot"));
 }
 Driver other; ActiveGoalRegistry recovered(other,path);
 assert(recovered.robot_locked("sim_robot"));
 assert(recovered.find(id)->state==GoalState::STOP_UNKNOWN);
 assert(!recovered.start(request(2),now)); assert(other.sent.empty());
}
void test_cancellation_and_stale_messages() {
 Driver d; Budgets b; b.acceptance=Milliseconds(10); b.cancel_stop=Milliseconds(20);
 ActiveGoalRegistry r(d,journal("cancel"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
 r.pump(now+Milliseconds(11)); assert(r.find(id)->cancel_intent && d.cancellations.size()==1);
 auto ack=event(*r.find(id),EventKind::CANCEL_ACK); d.events.push_back(ack); r.pump(now+Milliseconds(12)); assert(r.robot_locked("sim_robot"));
 auto accepted=event(*r.find(id),EventKind::ACCEPTED); d.events.push_back(accepted); r.pump(now+Milliseconds(13)); assert(d.cancellations.size()==2);
 auto stale=event(*r.find(id),EventKind::RESULT); stale.trace.execution_generation=2; stale.native_status=NativeStatus::SUCCEEDED; stale.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},""};
 d.events.push_back(stale); r.pump(now+Milliseconds(14)); assert(r.robot_locked("sim_robot"));
 r.pump(now+Milliseconds(32)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
 auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::CANCELED; result.result={ResultCode::CANCELED,StopState::CONFIRMED,{},"stopped"};
 d.events.push_back(result); r.pump(now+Milliseconds(33)); assert(!r.robot_locked("sim_robot"));
 assert(!r.start(request(),now)); assert(r.start(request(2),now));
}
void test_feedback_and_protocol_failure() {
 Driver d; Budgets b; b.feedback=Milliseconds(10); ActiveGoalRegistry r(d,journal("feedback"),b); auto now=SteadyTime{}; auto id=*r.start(request(),now);
 d.events.push_back(event(*r.find(id),EventKind::ACCEPTED)); r.pump(now);
 auto e=event(*r.find(id),EventKind::FEEDBACK); e.sequence=2; d.events.push_back(e); r.pump(now+Milliseconds(5));
 e.sequence=1; d.events.push_back(e); r.pump(now+Milliseconds(12)); assert(!r.find(id)->cancel_intent);
 d.events.push_back(e); r.pump(now+Milliseconds(16)); assert(r.find(id)->cancel_intent);
 auto result=event(*r.find(id),EventKind::RESULT); result.native_status=NativeStatus::SUCCEEDED; result.result={ResultCode::FAILED,StopState::CONFIRMED,{},"contradiction"};
 d.events.push_back(result); r.pump(now+Milliseconds(17)); assert(r.find(id)->state==GoalState::STOP_UNKNOWN);
 assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,false,"evidence"));
 assert(!r.reconcile(id,request().trace,StopState::CONFIRMED,true,""));
 assert(r.reconcile(id,request().trace,StopState::CONFIRMED,true,"operator-17"));
}
void test_rejection_releases_and_unknown_stops_lock() {
 Driver d; ActiveGoalRegistry r(d,journal("reject")); auto now=SteadyTime{}; auto id=*r.start(request(),now);
 d.events.push_back(event(*r.find(id),EventKind::REJECTED)); r.pump(now); assert(!r.robot_locked("sim_robot"));
 id=*r.start(request(2),now); auto e=event(*r.find(id),EventKind::RESULT); e.native_status=NativeStatus::SUCCEEDED; e.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},""};
 d.events.push_back(e); r.pump(now); assert(r.robot_locked("sim_robot"));
}
void test_completion_after_deadline_cannot_be_success() {
 Driver d;Budgets b;b.acceptance=Milliseconds(10);ActiveGoalRegistry r(d,journal("late_success"),b);auto now=SteadyTime{};auto id=*r.start(request(),now);
 auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::CONFIRMED,{},"late"};d.events.push_back(result);
 r.pump(now+Milliseconds(11));assert(r.find(id)->cancel_intent);assert(!r.robot_locked("sim_robot"));
}
void test_contradictory_rejection_cannot_release_unknown_motion() {
 Driver d;ActiveGoalRegistry r(d,journal("contradiction"));auto now=SteadyTime{};auto id=*r.start(request(),now);
 auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::SUCCEEDED;result.result={ResultCode::COMPLETED,StopState::UNKNOWN,{},"uncertain execution"};d.events.push_back(result);r.pump(now);
 d.events.push_back(event(*r.find(id),EventKind::REJECTED));r.pump(now);assert(r.robot_locked("sim_robot"));
}
void test_accepted_goal_remote_rejection_releases_stopped_resource() {
 Driver d;ActiveGoalRegistry r(d,journal("remote_rejected"));auto now=SteadyTime{};auto id=*r.start(request(),now);d.events.push_back(event(*r.find(id),EventKind::ACCEPTED));r.pump(now);
 auto result=event(*r.find(id),EventKind::RESULT);result.native_status=NativeStatus::ABORTED;result.result={ResultCode::REJECTED,StopState::CONFIRMED,{},"remote controller refused"};d.events.push_back(result);r.pump(now);assert(!r.robot_locked("sim_robot"));assert(r.find(id)->result->code==ResultCode::REJECTED);
}
void test_geometry_and_snapshot_binding() {
 Pose p{"map",1,2,0,0,0,0,1}; assert(valid_pose(p)); auto q=p; q.x+=0.06; assert(!within_tolerance(q,p,0.05,0.1));
 q=p; q.x=std::numeric_limits<double>::quiet_NaN(); assert(!valid_pose(q)); assert(!within_tolerance(q,p,0.05,0.1));
 TargetBinding t; t.meta={1,request().trace,"goal","perception",3,100,300}; t.target_id="item"; t.pose=p;
 assert(valid_target(t,request().trace,"item",3,100,200));
 assert(!valid_target(t,request().trace,"item",4,100,200));
 assert(!valid_target(t,request().trace,"item",3,101,200));
 assert(!valid_target(t,request().trace,"item",3,100,301));
 PlacementBinding place{t.meta,"item","bin",p,true};
 assert(!valid_placement(place,request().trace,"item","wrong",3,100,200));
 assert(!SkillResponse{}.valid);
}
int main() {
 test_accepted_goal_remote_rejection_releases_stopped_resource(); test_contradictory_rejection_cannot_release_unknown_motion(); test_completion_after_deadline_cannot_be_success(); test_journal_excludes_second_executor(); test_one_send_and_restart_lock(); test_cancellation_and_stale_messages(); test_feedback_and_protocol_failure(); test_rejection_releases_and_unknown_stops_lock(); test_geometry_and_snapshot_binding();
 std::cout << "core registry and geometry tests passed\n";
}