"""Execute actual road-edge clearance, detour and checkpoint order policies.
Native movement/callbacks are checked separately in the isolated game probe.
"""
from policy_harness import ROOT, body, run
import re
base=ROOT/'addons/core/Scripts/Game/ERR/10_AI'
code='\n'.join(body(base/'ERR_KnownMines.c',s) for s in ['static bool Clear(', 'static vector Detour('])
code=code.replace('vector', 'Vec').replace('Math.Clamp','Clamp').replace('ERR_ForceRules.FiniteVector','FiniteVec').replace('ERR_ForceRules.Finite','std::isfinite').replace('Vec.DistanceXZ','Vec::DistanceXZ').replace('Vec.Dot','Vec::Dot').replace('float fraction;','float fraction=0;')
order=body(base/'ERR_RoadblockMining.c','protected override bool OrderValid(').replace('protected override ','')
for obj in ['m_Unit','m_Group','m_Hold']:order=order.replace(obj+'.',obj+'->')
order=order.replace('vector.DistanceXZ','Vec::DistanceXZ').replace('vector.Distance','Vec::DistanceXZ')
run(r'''
#include <cassert>
#include <cmath>
#include <algorithm>
#include <limits>
float Clamp(float v,float a,float b){return std::clamp(v,a,b);}
struct Vec {float x=0,y=0,z=0;Vec()=default;Vec(float a,float b,float c):x(a),y(b),z(c){}
 float& operator[](int i){return i==0?x:i==1?y:z;}
 Vec operator-(Vec b)const{return {x-b.x,y-b.y,z-b.z};}Vec operator+(Vec b)const{return {x+b.x,y+b.y,z+b.z};}
 Vec operator-()const{return {-x,-y,-z};}Vec operator*(float f)const{return {x*f,y*f,z*f};}
 float LengthSq(){return x*x+y*y+z*z;}void Normalize(){float l=std::sqrt(LengthSq());x/=l;y/=l;z/=l;}
 static float Dot(Vec a,Vec b){return a.x*b.x+a.y*b.y+a.z*b.z;}
 static float DistanceXZ(Vec a,Vec b){return std::hypot(a.x-b.x,a.z-b.z);}
};
Vec Vector(float x,float y,float z){return {x,y,z};}
bool FiniteVec(Vec p){return std::isfinite(p.x)&&std::isfinite(p.y)&&std::isfinite(p.z);}
'''+code+r'''
struct Hold{Vec position;float radius=25;Vec GetOrigin(){return position;}float GetCompletionRadius(){return radius;}};
struct Group{Hold* current=nullptr;Hold* GetCurrentWaypoint(){return current;}};
struct Unit{bool Task=false,Assignment=false,PendingTask=false,PendingOrder=false,Downlink=false;};
struct Job {Unit* m_Unit;Group* m_Group;Hold* m_Hold;bool m_Known=true;Vec m_Rally,m_Position{50,0,0};
'''+order+r'''};
int main(){
 // Road edge, not centreline: 16m wide road needs 23m clearance.
 assert(!Clear({-100,0,0},{100,0,0},{0,0,22},23));
 assert(Clear({-100,0,0},{100,0,0},{0,0,24},23));
 assert(!Clear({0,0,-100},{0,0,100},{0,0,24},17)); // crossing road
 assert(!Clear({0,0,0},{0,0,0},{0,0,0},8)); // degenerate segment
 assert(Clear({0,0,0},{0,0,0},{20,0,0},8));
 float nan=std::numeric_limits<float>::quiet_NaN();
 assert(!Clear({nan,0,0},{0,0,0},{0,0,0},8));
 assert(!Clear({0,0,0},{0,0,0},{0,0,0},nan));
 // A direct route intersects the mine; both 45deg detours keep a safety gap.
 for(float distance: {10.f,12.f,18.f,22.f})for(int side: {-1,1}) {
  Vec start{distance,0,0},goal{-25,0,0},mine;
  assert(!Clear(start,goal,mine,10));
  Vec step=Detour(start,goal,mine,side);
  assert(Clear(start,step,mine,8));assert(Vec::DistanceXZ(step,mine)>15.9f);
 }
 Vec escape=Detour({5,0,0},{-20,0,0},{0,0,0},1);
 assert(escape.x>5&&escape.z==0); // strictly away if already in safety circle
 Hold hold,other;Group group{&hold};Unit unit;Job job{&unit,&group,&hold};
 assert(job.OrderValid());
 for(bool* flag:{&unit.Task,&unit.Assignment,&unit.PendingTask,&unit.PendingOrder,&unit.Downlink}){
  *flag=true;assert(!job.OrderValid());*flag=false;
 }
 group.current=&other;assert(!job.OrderValid());group.current=&hold;
 job.m_Position={44,0,0};assert(!job.OrderValid()); // hold radius plus withdrawal clearance
 job.m_Position={45,0,0};assert(job.OrderValid());
 hold.radius=30;assert(!job.OrderValid());hold.radius=25;
 hold.position={1,0,0};assert(!job.OrderValid());hold.position={0,0,0};
 job.m_Known=false;assert(!job.OrderValid());job.m_Known=true;
 job.m_Hold=nullptr;assert(!job.OrderValid());
}
''')
print('PASS: road-edge/intersection clearance, mine detour geometry, checkpoint hold authority and retask cancellation')