import numpy as np
from pathlib import Path

pres_1 = ['P1', 'P3', 'P5']
pres_2 = ['LF', 'ZS', 'FS']
lens = [8,4,4,]

con = lambda x: np.nan if x==b'' else float(x)

thresh = 400

for pre_1, l in list(zip(pres_1, [8,4,4])):
    angles = np.linspace(0,360, l+1, endpoint=True)
    print(angles)
    output = [angles]
    for pre_2 in pres_2:
        fpath = Path("data") / f"{pre_1}_{pre_2}.dat"
        conv = {i:con for i in range(2,l+2)}
        ndata = np.loadtxt(fpath, skiprows=2, delimiter='\t', converters=conv).transpose()
        depth = ndata[1]
        hardnesses = np.append(ndata[2:], [ndata[2]], axis=0)
        res = []
        for hard, angle in zip(hardnesses, angles):
            idx = np.where(np.diff(np.sign(hard - thresh))!=0 )[0][0]
            x1, x2 = depth[idx:idx+2]
            y1, y2 = hard[idx:idx+2]
            x0 = (thresh - y1)*(x2-x1)/(y2-y1) + x1
            print(angle,depth[idx:idx+2],hard[idx:idx+2], x0)
            res.append(x0)

        output.append(res)
    outpath = Path('data') / f"{pre_1}_depth.dat"
    
    np.savetxt(outpath, np.transpose(output), header="angle\tLF\tZS\tFS", delimiter='\t', comments='', fmt='%.3f')