from pathlib import Path
import json,csv,math
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
OUT=Path(__file__).resolve().parent
P=np.array([[-1.,-.6],[1.,-.6],[0.,1.2]]); MASS=100.;G=9.80665;PAD=.01
B=np.vstack([np.ones(3),P.T])
def cross2(a,b): return a[0]*b[1]-a[1]*b[0]
def solve(x,y):
    weights=np.linalg.solve(B,np.array([1.,x,y]));margin=min(cross2(P[(i+1)%3]-P[i],np.array([x,y])-P[i])/np.linalg.norm(P[(i+1)%3]-P[i]) for i in range(3))
    feasible=bool(min(weights)>=-1e-12)
    # Only tolerance-sized roundoff is removed; no physical loads reported outside.
    loads=np.maximum(weights,0)*MASS*G if feasible else [None]*3
    return weights,float(margin),feasible,loads
rows=[]
for y in np.linspace(-.9,1.5,81):
    for x in np.linspace(-1.3,1.3,101):
        w,margin,ok,loads=solve(x,y)
        rows.append(dict(cg_x_m=float(x),cg_y_m=float(y),signed_halfplane_margin_m=margin,static_vertical_equilibrium_feasible=ok,formal_weight_A=float(w[0]),formal_weight_B=float(w[1]),formal_weight_C=float(w[2]),reaction_A_N=loads[0],reaction_B_N=loads[1],reaction_C_N=loads[2],nominal_average_pad_pressure_A_Pa=loads[0]/PAD if ok else None,nominal_average_pad_pressure_B_Pa=loads[1]/PAD if ok else None,nominal_average_pad_pressure_C_Pa=loads[2]/PAD if ok else None))
def write(name,rr):
    with (OUT/name).open('w',newline='',encoding='utf-8') as f:
        wr=csv.DictWriter(f,fieldnames=list(rr[0]));wr.writeheader();wr.writerows(rr)
write('support-grid.csv',rows)
line=[]
for x in np.linspace(-1,1,201):
    w,m,ok,r=solve(x,0)
    line.append(dict(cg_x_m=float(x),cg_y_m=0,feasible=ok,margin_m=m,reaction_A_N=r[0],reaction_B_N=r[1],reaction_C_N=r[2]))
write('support-traverse.csv',line)
# Independent closed-form barycentric equations for chosen triangle.
max_weight_error=max_force_error=max_moment_error=0.
for row in rows:
    x=row['cg_x_m'];y=row['cg_y_m'];wc=(y+.6)/1.8;wb=(1-wc+x)/2;wa=1-wc-wb
    max_weight_error=max(max_weight_error,max(abs(row['formal_weight_'+s]-v) for s,v in zip('ABC',[wa,wb,wc])))
    if row['static_vertical_equilibrium_feasible']:
        r=np.array([row['reaction_'+s+'_N'] for s in 'ABC'])
        max_force_error=max(max_force_error,abs(sum(r)-MASS*G))
        # Horizontal moment about projected centre of gravity, vertical contact forces.
        moment=np.sum((P-np.array([x,y]))*r[:,None],axis=0)
        max_moment_error=max(max_moment_error,float(np.max(np.abs(moment))))
        assert min(r)>=0
assert max_weight_error<1e-12 and max_force_error<1e-9 and max_moment_error<1e-9
assert np.allclose(solve(0,0)[0],[1/3]*3)
for i in range(3):assert np.allclose(solve(*P[i])[0],np.eye(3)[i])
assert not solve(1,0)[2] and solve(1,0)[3]==[None]*3
assert abs(solve(2/3,0)[0][0])<1e-12
meta={'title':'Three contacts, one weight: where can the load be supported?','kind':'Exact static-equilibrium teaching calculation; no landing tests','mass_kg':MASS,'g_m_s2':G,'contact_coordinates_m':P.tolist(),'nominal_effective_pad_area_m2':PAD,'rows':{'grid':len(rows),'traverse':len(line)},'formulas':{'barycentric':'sum(wi)=1; sum(wi*xi)=xCG; sum(wi*yi)=yCG','vertical_reactions':'Ri=wi*m*g only when every wi>=0','margin':'minimum signed inward perpendicular distance to the three edge lines; positive inside, zero boundary, negative outside','nominal_pressure':'p_average_i=Ri/A_pad'},'assumptions':['Rigid body, uniform vertical gravity and exactly three noncollinear contacts on a horizontal plane.','Only vertical contact reactions and weight; no horizontal forces, applied couples, adhesion or anchoring.','Contacts are unilateral: they can push up but cannot pull down.','At an edge or vertex, zero reactions are possible but the geometric reserve is zero; this is marginal equilibrium.','The pad pressure estimate assumes a specified effective loaded area and a centred vertical resultant. Point-contact geometry does not resolve real pressure distribution or expand the support polygon.'],'limitations':['A negative formal barycentric weight means this three-contact equilibrium is infeasible, not a physical negative contact force. No clamping-and-renormalizing is used to invent equilibrium.','Outside the triangle, the signed halfplane margin is a constraint measure, not necessarily Euclidean distance to the triangle.','No slope, touchdown dynamics, impact load, buckling, friction demand, terrain yielding, actuator limits or safety factor.','Changing mass scales loads and nominal pressures, not the projected geometric support region in this ideal model.','No validated bearing capacity or ground pressure allowable is supplied; nominal pressure is not a terrain safety forecast.'],'validation':{'closed_form_weight_max_error':max_weight_error,'force_residual_max_N':max_force_error,'moment_residual_max_Nm':max_moment_error,'centroid_equal_load':True,'vertices_single_contact_load':True,'edge_zero_opposite_reaction':True,'outside_physical_reactions_absent':True},'sources':[{'title':'Orsolino et al., Feasible Region: an Actuation-Aware Extension of the Support Region (2019)','url':'https://arxiv.org/abs/1903.07999','scope':'Support polygon static-balance assumptions; actuator constraints can further restrict feasibility.'},{'title':'MIT Underactuated Robotics, Highly-articulated Legged Robots','url':'https://underactuated.csail.mit.edu/humanoids.html','scope':'Contact-force and balance framework; static geometry is not a dynamic stability guarantee.'}]}
(OUT/'study.json').write_text(json.dumps(meta,indent=2)+'\n',encoding='utf-8')
plt.rcParams.update({'font.family':'DejaVu Sans','font.size':10,'axes.spines.top':False,'axes.spines.right':False})
fig,ax=plt.subplots(1,2,figsize=(11.8,4.9),constrained_layout=True)
X=np.array([r['cg_x_m'] for r in rows]).reshape(81,101);Y=np.array([r['cg_y_m'] for r in rows]).reshape(81,101);Z=np.array([r['signed_halfplane_margin_m'] for r in rows]).reshape(81,101)
im=ax[0].pcolormesh(X,Y,Z,cmap='RdBu',vmin=-.6,vmax=.6,shading='auto');fig.colorbar(im,ax=ax[0],label='Signed edge-line margin (m)')
poly=np.vstack([P,P[0]]);ax[0].plot(poly[:,0],poly[:,1],color='#15253c',lw=2);ax[0].scatter(P[:,0],P[:,1],c='black',s=35)
for i,(x,y) in enumerate(P):ax[0].annotate('ABC'[i],(x,y),xytext=(7,7),textcoords='offset points',weight='bold')
ax[0].plot([-1,1],[0,0],'--',color='black',lw=1);ax[0].plot(0,0,'o',color='#ffd166');ax[0].set(aspect='equal',xlabel='Projected centre of gravity x (m)',ylabel='Projected centre of gravity y (m)',title='A  |  Support exists inside the triangle')
for s,col in zip('ABC',['#393f94','#087f8c','#b42e8d']):ax[1].plot([r['cg_x_m'] for r in line],[np.nan if r['reaction_'+s+'_N'] is None else r['reaction_'+s+'_N'] for r in line],color=col,label='Contact '+s,lw=2)
for a,b in [(-1,-2/3),(2/3,1)]:ax[1].axvspan(a,b,color='#f5dfdf')
ax[1].axvline(-2/3,color='#555',ls=':');ax[1].axvline(2/3,color='#555',ls=':');ax[1].set(xlabel='Projected centre of gravity x at y = 0 (m)',ylabel='Upward reaction force (N)',title='B  |  Reactions redistribute as the load moves',xlim=(-1,1));ax[1].legend();ax[1].grid(alpha=.2)
fig.suptitle('Static support: pushing contacts cannot pull',fontsize=16,weight='bold')
fig.supxlabel('100 kg, horizontal plane, gravity only. Pink shaded traverse regions have no feasible three-contact equilibrium.',fontsize=9)
for ext in ['png','svg','pdf']:fig.savefig(OUT/f'support-triangle.{ext}',dpi=180)
print(json.dumps(meta['validation']))