#pragma once
#include "robot_bt/core.hpp"
#include <cassert>
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <limits>
using namespace robot_bt;
struct Simulator : GoalDriver {
 std::vector<GoalRequest> sent; std::vector<GoalEvent> events; RosTime now{1000000};
 std::optional<Skill> unavailable_skill;
 Holding sensor_holding{Holding::EMPTY};
 Admission admission{Admission::DIRECT}; bool bad_container{false}, unknown_verification{false}, nan_geometry{false}, no_stop{false};
 bool ready(Skill skill)const override{return !unavailable_skill||*unavailable_skill!=skill;}
 void cancel(const std::string&)override{}
 void send(const GoalRequest& q)override {
  if(q.skill==Skill::PICK)sensor_holding=Holding::HOLDING_TARGET;
  if(q.skill==Skill::PLACE)sensor_holding=Holding::EMPTY;
  sent.push_back(q); GoalEvent accept; accept.goal_id=q.goal_id; accept.trace=q.trace; accept.kind=EventKind::ACCEPTED; events.push_back(accept);
  GoalEvent e=accept; e.kind=EventKind::RESULT; e.native_status=NativeStatus::SUCCEEDED; e.result.code=ResultCode::COMPLETED; e.result.stop=no_stop?StopState::UNKNOWN:StopState::CONFIRMED;
  auto& r=e.result.response; r.valid=true; r.target_id=q.target_id; r.destination_id=bad_container?"wrong-bin":q.destination_id; r.base_stopped=true; r.verified=!unknown_verification; r.in_destination=true;
  SnapshotMeta meta{1,q.trace,q.goal_id,"independent-simulator",q.geometry_epoch,now,now+1000000000}; r.evidence=meta;
  Pose p{"map",0,0,0,0,0,0,1}; if(nan_geometry)p.x=std::numeric_limits<double>::quiet_NaN();
  switch(q.skill) {
  case Skill::NAVIGATE:r.final_pose=q.registered_pose;break;
  case Skill::LOCATE_SHELF_COLUMN:r.shelf=q.shelf;r.side="front";r.column="1";r.tier="2";break;
  case Skill::LOCALIZE_TARGET:r.target=TargetBinding{meta,q.target_id,p};break;
  case Skill::EVALUATE_GRASP:r.admission=admission;r.posture_id="small-lift";break;
  case Skill::ADJUST_POSTURE:admission=Admission::DIRECT;break;
  case Skill::VERIFY_PICK:case Skill::VERIFY_TRANSPORT:r.holding=unknown_verification?Holding::UNKNOWN:Holding::HOLDING_TARGET;break;
  case Skill::CHECK_FREE_SPACE:r.placement=PlacementBinding{meta,q.target_id,r.destination_id,p,true};break;
  case Skill::VERIFY_EMPTY:case Skill::VERIFY_PLACE:r.holding=unknown_verification?Holding::UNKNOWN:Holding::EMPTY;break;
  default:break;
  }
  events.push_back(e);
 }
 std::vector<GoalEvent> drain_events()override{auto r=events;events.clear();return r;}
};
struct Fixture {
 Simulator driver; ContextStore context; std::string journal; ActiveGoalRegistry registry; TaskConfig task; SiteConfig site; unsigned deliveries{0};
 Fixture(const std::string& name):journal(test_root()+"/"+name+".journal"),registry(driver,journal) {
  task.trace={"task","root","run",1,1,1,1}; task.robot_id="sim";task.target_id="item";task.source_shelf="shelf";task.destination_id="bin";task.observe_location="observe";task.destination_location="bin-nav";
  site.locations={{"observe",Pose{"map",0,0,0,0,0,0,1}},{"source",Pose{"map",1,0,0,0,0,0,1}},{"bin-nav",Pose{"map",2,0,0,0,0,0,1}}};site.parking_locations={{"shelf/front/1","source"}};site.allowed_postures={"small-lift","carry"};site.transport_posture="carry";
 }
 static const std::string& test_root(){static const std::string root=[](){char path[]="/tmp/robot_bt_workflow_tests_XXXXXX";const char* made=::mkdtemp(path);assert(made);return std::string(made);}();return root;}
 StageRunner runner(){return StageRunner(task,site,driver,registry,context,[this](const std::string& id,unsigned index,const std::string& evidence){assert(id=="task"&&index==0&&!evidence.empty());++deliveries;return true;});}
 TickStatus execute(StageRunner& r,unsigned ticks=300) {
  Workflow flow(r); auto status=TickStatus::RUNNING; for(unsigned i=0;i<ticks&&status==TickStatus::RUNNING;++i) { driver.now=1000000+static_cast<RosTime>(i)*1000000; r.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000}); status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now); } return status;
 }
 unsigned count(Skill skill)const{unsigned n=0;for(const auto&q:driver.sent)if(q.skill==skill)++n;return n;}
};