"""Read-only native r03 audit: geometry, stock-part layout and bit access."""
from pathlib import Path
import collections
import hashlib
import json
import math
import adsk.core as ac
import adsk.fusion as af

HERE=Path(__file__).resolve().parent


def analytic_stance(p):
    """Closed-form two-link FK plus HAA rotation; no builder transforms reused."""
    qh=math.radians(p['pose']['hip_deg'])
    qk=math.radians(p['pose']['knee_relative_deg'])
    qa=math.radians(p['pose']['haa_outward_deg'])
    l1=p['leg']['upper_length_mm'];l2=p['leg']['lower_length_mm']
    radius=p['leg']['foot_radius_mm']
    hip_x=p['body']['pitch_axis_x_mm'];haa_y=p['body']['haa_y_mm']
    lateral_offset=p['leg']['lower_center_y_mm']-haa_y
    # Resolve both link vectors before rotating the complete leg about HAA X.
    sagittal_x=-l1*math.sin(qh)-l2*math.sin(qh+qk)
    sagittal_z=-l1*math.cos(qh)-l2*math.cos(qh+qk)
    lateral=haa_y+lateral_offset*math.cos(qa)-sagittal_z*math.sin(qa)
    vertical=lateral_offset*math.sin(qa)+sagittal_z*math.cos(qa)
    haa_height=radius-vertical  # ball bottom touches the ground at z=0
    feet={leg:[front*hip_x+sagittal_x,side*lateral,haa_height+vertical]
          for leg,front,side in [('FL',1,1),('FR',1,-1),('RL',-1,1),('RR',-1,-1)]}
    return {'feet_mm':feet,'haa_height_mm':haa_height,'foot_span_mm':2*lateral}


def run(_context):
    app=ac.Application.get();d=af.Design.cast(app.activeProduct)
    assert d.parentDocument.name=='Shappy V2 - RS06 Whole Robot r03'
    p=json.loads((HERE/'parameters.json').read_text())
    c=json.loads((HERE/'cad/cad_checks.json').read_text())
    for name,key in [('fusion_build.py','source_sha256'),('parameters.json','parameters_sha256')]:
        assert hashlib.sha256((HERE/name).read_bytes()).hexdigest()==c[key]
    assert all(i.healthState==af.FeatureHealthStates.HealthyFeatureHealthState for i in d.timeline)
    assert len(c['body_records'])==c['body_count']==286
    expected_collisions=c['standing_interferences']
    assert len(expected_collisions)==8,expected_collisions
    assert all('belt_PITCH_PATH' in x['two'] and x['one'].startswith('S5M_belt_drive') for x in expected_collisions)
    records=c['body_records'];names=[r['body'] for r in records]
    assert sum('HBLFSN5_drawing_reference' in n for n in names)==8
    assert sum('HNTT5_5_drawing_reference' in n for n in names)==24
    assert sum('WSI_SUS_M5' in n for n in names)==12
    assert sum('Stock_bracket_CBM5x10' in n for n in names)==16
    assert sum('Root_CB4_30' in n for n in names)==16
    assert sum('Foot_CB3_20' in n for n in names)==8
    assert sum('Foot_M3_hex_nut_AF5p5' in n for n in names)==8
    assert not any('Crossmember_gusset' in n or 'Brace_spacer' in n for n in names)
    screw_sizes={
      'HAA_stator_M3x8':(3,8,5.5,3,2.5,1.5),
      'Profile_end_M5x16':(5,16,9.5,2.75,3,1.5),
      'Stock_bracket_CBM5x10':(5,10,8.5,5,4,2.5),
      'Top_brace_M5x8':(5,8,8.5,5,4,2.5),
      'Top_brace_M5x10':(5,10,8.5,5,4,2.5),
      'Tray_CB5x14':(5,14,8.5,5,4,2.5),
      'Root_CB4_30':(4,30,7,4,3,2),
      'Foot_CB3_20':(3,20,5.5,3,2.5,1.5)}
    bits=collections.Counter();screws=0
    for row in records:
        if '_HEX' not in row['body']:continue
        sizes=next(v for k,v in screw_sizes.items() if row['body'].startswith(k))
        dia,length,hd,hh,bit,depth=sizes
        expected=math.pi*(dia/2)**2*length+math.pi*(hd/2)**2*hh-math.sqrt(3)/2*bit**2*depth
        assert abs(row['volume_mm3']-expected)<1e-3,(row['body'],row['volume_mm3'],expected)
        bits[str(bit)]+=1;screws+=1
    assert screws==88 and set(bits)=={'2.5','3','4'}
    tm=af.TemporaryBRepManager.get();geometry=[];feet={};printed=[];body_count=0
    for o in d.rootComponent.occurrences:
        for b in o.component.bRepBodies:
            assert b.isSolid;body_count+=1
            temp=tm.copy(b);assert tm.transform(temp,o.transform2)
            geometry.append((o.component.name,b.name,temp))
            if o.component.name=='D40_foot_ball_REFERENCE':
                point=b.physicalProperties.centerOfMass;point.transformBy(o.transform2)
                xyz=[getattr(point,a)*10 for a in 'xyz']
                leg=('F' if xyz[0]>0 else 'R')+('L' if xyz[1]>0 else 'R');feet[leg]=xyz
            if o.component.name in ['P2S_PETG_battery_tray','PETG_upper_link_200','C_diagonal_lower_link_195','Replaceable_PETG_foot_cup']:
                bb=b.boundingBox;size=[(getattr(bb.maxPoint,a)-getattr(bb.minPoint,a))*10 for a in 'xyz']
                assert all(x<256 for x in size)
                printed.append({'component':o.component.name,'size_mm':size})
    assert body_count==286 and len(printed)==13
    # User-selected neutral HAA stance. Historical r02 coordinates are comparisons,
    # never a pass condition for the new pose.
    assert abs(p['pose']['haa_outward_deg'])<1e-12
    assert abs(p['kinematics']['abduction_deg'])<1e-12
    analytic=analytic_stance(p)
    assert set(feet)==set(analytic['feet_mm'])=={'FL','FR','RL','RR'}
    error=max(abs(feet[k][i]-analytic['feet_mm'][k][i]) for k in feet for i in range(3))
    assert error<1e-6,{'actual':feet,'expected':analytic['feet_mm'],'error_mm':error}
    neutral_expected={'FL':[237.5,110,20],'FR':[237.5,-110,20],
                      'RL':[-242.5,110,20],'RR':[-242.5,-110,20]}
    assert max(abs(analytic['feet_mm'][k][i]-neutral_expected[k][i])
               for k in neutral_expected for i in range(3))<1e-6
    assert abs(analytic['foot_span_mm']-220)<1e-6
    assert abs(analytic['haa_height_mm']-362.08)<.005
    assert abs(c['haa_height_mm']-analytic['haa_height_mm'])<1e-6
    haa_housings=[r for r in records if r['component'].endswith('_HAA')
                  and r['body']=='RS06_D90_L55_reference_housing']
    assert len(haa_housings)==4
    assert all(abs(sum(r['bounds_mm'][i][2] for i in range(2))/2
                   -analytic['haa_height_mm'])<1e-6 for r in haa_housings)
    foot_span=max(v[1] for v in feet.values())-min(v[1] for v in feet.values())
    previous=json.loads((HERE.parent/'rs06_r02/cad/verification.json').read_text())
    previous_p=json.loads((HERE.parent/'rs06_r02/parameters.json').read_text())
    previous_feet=previous['foot_centers_mm']
    previous_span=max(v[1] for v in previous_feet.values())-min(v[1] for v in previous_feet.values())
    foot_deltas={k:[feet[k][i]-previous_feet[k][i] for i in range(3)] for k in feet}
    historical_change=max(abs(v) for delta in foot_deltas.values() for v in delta)
    def overlap(a,b):
        return all(getattr(a.maxPoint,k)>getattr(b.minPoint,k)+1e-8 and getattr(b.maxPoint,k)>getattr(a.minPoint,k)+1e-8 for k in 'xyz')
    access=[]
    for tool in c['tool_axes_local']:
        if not tool['body'].startswith('Stock_bracket_CBM5x10'):continue
        axis=tool['axis'];top=tool['head_top_mm']
        start=[top[i]+.05*axis[i] for i in range(3)]
        end=[top[i]+30.05*axis[i] for i in range(3)]
        corridor=tm.createCylinderOrCone(ac.Point3D.create(*[x/10 for x in start]),.38,
            ac.Point3D.create(*[x/10 for x in end]),.38)
        hits=[]
        for comp,name,body in geometry:
            if name==tool['body']:continue
            if not overlap(corridor.boundingBox,body.boundingBox):continue
            test=tm.copy(body)
            if tm.booleanOperation(test,corridor,af.BooleanTypes.IntersectionBooleanType) and test.volume*1000>1e-4:
                hits.append({'component':comp,'body':name,'volume_mm3':test.volume*1000})
        access.append({'screw':tool['body'],'interferences':hits})
    assert len(access)==16
    # Report a failure rather than changing a tool envelope to manufacture a pass.
    assert all(not x['interferences'] for x in access),access
    old=json.loads((HERE.parent/'rs06_r02/cad/cad_checks.json').read_text())
    old_map={(r['component'],r['body']):r for r in old['body_records']}
    unchanged=0
    for row in records:
        before=old_map.get((row['component'],row['body']))
        if before and abs(before['volume_mm3']-row['volume_mm3'])<1e-4 and max(abs(before['bounds_mm'][i][j]-row['bounds_mm'][i][j]) for i in range(2) for j in range(3))<1e-5:unchanged+=1
    mass={kind:sum(r['volume_mm3'] for r in records if r['kind']==kind)*density/1e6 for kind,density in [('aluminum',2.7),('steel',7.85),('PETG',1.27)]}
    subtotal=7.38+.91+.12+sum(mass.values())
    hashes=['fusion_build.py','parameters.json','verify_cad.py','cad/cad_checks.json','cad/shappy_rs06_r03.f3d','cad/shappy_rs06_r03.step','viewer/scene.json','research/stock_brackets.json','research/tooling_standard.json','../rs06_r02/parameters.json','../rs06_r02/cad/verification.json','../rs06_r02/cad/cad_checks.json']
    result={'revision':p['revision'],'checked_on':'2026-10-28','result':'pass within stated nominal-geometry scope',
      'native_bodies':body_count,'timeline_healthy':True,'unexpected_positive_volume_interferences':0,
      'intentional_belt_tooth_envelope_interferences':8,'motor_housings':12,'supplier_motor_cad_imported':False,
      'stock_brackets':8,'bracket_geometry':'official 2D dimensions reconstructed; undimensioned ribs/tabs/flanks approximated; no supplier STEP',
      'frame_screws':64,'modeled_screws_total':screws,'Tnuts':24,'standard_M5_washers':12,
      'hex_bit_screw_counts':dict(bits),'hex_socket_volume_checks':screws,
      'bracket_tool_access':{'checked':16,'corridor_diameter_mm':7.6,'corridor_length_mm':30,
        'scope':'straight bit-shaft corridor from outside screw head, nominal pose; handle/hand and other fastener access not verified','cases':access},
      'foot_centers_mm':feet,'foot_span_mm':foot_span,
      'pose_validation':{'haa_outward_deg':p['pose']['haa_outward_deg'],
        'basis':'independent closed-form FK from current parameters; neutral HAA stance',
        'expected_foot_centers_mm':analytic['feet_mm'],'max_analytic_foot_error_mm':error,
        'expected_foot_span_mm':analytic['foot_span_mm'],
        'haa_height_mm':c['haa_height_mm'],'expected_haa_height_mm':analytic['haa_height_mm']},
      'historical_r02_comparison':{'reference_revision':previous['revision'],
        'previous_haa_outward_deg':previous_p['pose']['haa_outward_deg'],
        'previous_foot_span_mm':previous_span,'current_foot_span_mm':foot_span,
        'foot_span_change_mm':foot_span-previous_span,'foot_center_deltas_mm':foot_deltas,
        'max_coordinate_change_mm':historical_change,
        'previous_haa_height_mm':old['haa_height_mm'],
        'haa_height_change_mm':c['haa_height_mm']-old['haa_height_mm'],
        'note':'Neutral-pose feet intentionally differ from historical r02; this comparison is not an unchanged-feet assertion.'},
      'unchanged_r02_records':unchanged,'printed_parts_P2S_bounds':printed,
      'catalog_mass_kg':{'motors':7.38,'profiles':.91,'brackets':.12},'assumed_density_mass_kg':mass,'partial_mass_kg':subtotal,
      'mass_excludes':['battery and electronics','pulleys and belts','bearings','foot elastomer','wiring','unmodeled fasteners'],
      'not_verified':['supplier manufacturing tolerances and exact geometry','strength/stiffness/fatigue','thread capacity and tightening torque','motor stator/output fasteners missing from previous layout','knee axial-retention details and final tool','full joint range','print fit and physical operation'],
      'source_sha256':{n:hashlib.sha256((HERE/n).read_bytes()).hexdigest() for n in hashes}}
    (HERE/'cad/verification.json').write_text(json.dumps(result,ensure_ascii=False,indent=2)+'\n')
    print(json.dumps({k:result[k] for k in ['result','native_bodies','frame_screws','modeled_screws_total','hex_bit_screw_counts','partial_mass_kg']},ensure_ascii=False))