"""Measured-point geometry for the independent polygon/planar-pose editor."""
from __future__ import annotations

from functools import lru_cache
import hashlib
import json
import math
from pathlib import Path
import threading

import cv2
import numpy as np

from .cluster_targets import digest
from .data.orientation import orient_grid
from .data.pcd import read_pcd
from .fork_geometry import DEFAULTS, decompose_roi, svd_plane, valid_xyz

VERSION = 'pose-review-geometry-v1'


def canonical(value):
    return json.dumps(value, sort_keys=True, separators=(',', ':'), allow_nan=False).encode()


def polygon_value(value, width, height):
    p = np.asarray(value, dtype=float)
    if p.ndim != 2 or p.shape[1:] != (2,) or not 3 <= len(p) <= 128 or not np.isfinite(p).all():
        raise ValueError('多边形须有3–128个有限坐标顶点')
    if (p < 0).any() or (p[:, 0] > width).any() or (p[:, 1] > height).any():
        raise ValueError('多边形超出原图边界')
    def cross(a, b, c):
        return float(np.cross(b-a, c-a))
    for i in range(len(p)):
        a, b = p[i], p[(i+1) % len(p)]
        if np.linalg.norm(a-b) < .01:
            raise ValueError('多边形相邻顶点重复')
        for j in range(i+1, len(p)):
            if j == i+1 or (i == 0 and j == len(p)-1):
                continue
            c, d = p[j], p[(j+1) % len(p)]
            v = (cross(a,b,c), cross(a,b,d), cross(c,d,a), cross(c,d,b))
            if v[0]*v[1] <= 0 and v[2]*v[3] <= 0 and all(
                    max(min(a[k],b[k]),min(c[k],d[k])) <= min(max(a[k],b[k]),max(c[k],d[k]))+1e-9
                    for k in range(2)):
                raise ValueError('多边形自交，请调整顶点')
    if abs(cv2.contourArea(p.astype(np.float32))) < 1:
        raise ValueError('多边形面积太小')
    return p.tolist()


def raster_mask(polygon, width, height):
    """Pixel-center inclusion, image-edge polygon coordinates, reproducible RLE."""
    p = np.asarray(polygon, dtype=np.float64)
    yy, xx = np.mgrid[:height, :width]
    x, y = xx+.5, yy+.5
    inside = np.zeros((height, width), bool)
    for a, b in zip(p, np.roll(p, -1, axis=0)):
        if abs(a[1]-b[1]) > 1e-15:
            inside ^= ((a[1] > y) != (b[1] > y)) & (x < (b[0]-a[0])*(y-a[1])/(b[1]-a[1])+a[0])
    return inside


def sensor_value(value, source):
    value = value or {}
    forward = int(value.get('forward_axis', source['sensor_policy'].get('depth_axis') or 0))
    up = int(value.get('up_axis', 2))
    if forward not in (0,1,2) or up not in (0,1,2) or up == forward:
        raise ValueError('前向轴和高度轴须不同')
    fs, us = value.get('forward_sign', 1), value.get('up_sign', 1)
    if fs not in (-1,1) or us not in (-1,1):
        raise ValueError('无效轴方向')
    registered = source['sensor_policy'].get('scale_to_mm')
    scale = value.get('scale_to_m', registered/1000 if registered is not None else None)
    evidence = str(value.get('evidence', '')).strip()[:1000]
    if scale is not None and (isinstance(scale, bool) or not math.isfinite(float(scale)) or not 1e-9 <= float(scale) <= 1e6):
        raise ValueError('单位比例无效')
    if scale is not None and (registered is None or float(scale) != registered/1000) and not evidence:
        raise ValueError('手工单位比例必须填写依据')
    verified = value.get('axes_confirmed', False)
    if type(verified) is not bool:
        raise ValueError('轴向确认字段无效')
    return {'forward_axis': forward, 'up_axis': up, 'forward_sign': fs, 'up_sign': us,
        'scale_to_m': float(scale) if scale is not None else None,
        'unit_origin': 'source_registered' if registered is not None and scale == registered/1000 else
                       'human_supplied' if scale is not None else 'unknown_native_units',
        'axes_confirmed': verified, 'evidence': evidence}


def basis_for(normal, sensor):
    h = np.eye(3)[sensor['up_axis']]*sensor['up_sign']
    f = np.eye(3)[sensor['forward_axis']]*sensor['forward_sign']
    d = np.asarray(normal, dtype=float)-h*np.dot(normal,h)
    if np.linalg.norm(d) < .1:
        d = f.copy()
    d /= np.linalg.norm(d)
    if np.dot(d,f) < 0:
        d = -d
    w = np.cross(d,h)
    return np.column_stack((w,d,h))


def validate_view(value):
    o, b = np.asarray(value['origin'], dtype=float), np.asarray(value['basis'], dtype=float)
    if o.shape != (3,) or b.shape != (3,3) or not np.isfinite(o).all() or not np.isfinite(b).all():
        raise ValueError('视图坐标无效')
    if not np.allclose(b.T@b, np.eye(3), atol=1e-6) or np.linalg.det(b) < .999:
        raise ValueError('视图坐标基必须为正交右手系')
    return o, b


def retained(points, exclusions):
    keep = np.ones(len(points), bool)
    if not isinstance(exclusions, list) or len(exclusions) > 40:
        raise ValueError('剔除区域过多')
    for item in exclusions:
        o, b = validate_view(item)
        j = {'wd': 1, 'wh': 2}.get(item.get('plane'))
        bounds = np.asarray(item.get('bounds'), dtype=float)
        if j is None or bounds.shape != (4,) or not np.isfinite(bounds).all():
            raise ValueError('剔除区域无效')
        local = (points-o)@b
        x0,y0,x1,y1 = bounds
        keep &= ~((local[:,0] >= min(x0,x1)) & (local[:,0] <= max(x0,x1)) &
                  (local[:,j] >= min(y0,y1)) & (local[:,j] <= max(y0,y1)))
    return keep


def pose_value(ruler, sensor):
    o, b = validate_view(ruler)
    values = np.asarray([ruler[k] for k in ('w','d','angle','half_width')], dtype=float)
    if not np.isfinite(values).all() or values[3] <= 0 or abs(values[2]) > 36000:
        raise ValueError('标尺位置、角度或长度无效')
    expected_h = np.eye(3)[sensor['up_axis']]*sensor['up_sign']
    if not np.allclose(b[:,2], expected_h, atol=1e-6):
        raise ValueError('标尺高度轴与传感器配置不同')
    w,d,angle,_ = values
    phi = np.deg2rad(angle)
    direction = -np.sin(phi)*b[:,0]+np.cos(phi)*b[:,1]
    sign = ruler.get('direction_sign', 1)
    if sign not in (-1,1):
        raise ValueError('叉入方向无效')
    direction *= sign
    point = o+w*b[:,0]+d*b[:,1]
    f = np.eye(3)[sensor['forward_axis']]*sensor['forward_sign']
    w0 = np.cross(f,expected_h)
    theta = float(np.degrees(np.arctan2(-direction@w0, direction@f)))
    scale = sensor['scale_to_m']
    return {'x': float(point[0]) if sensor['up_axis'] == 2 else None,
        'y': float(point[1]) if sensor['up_axis'] == 2 else None, 'z': None,
        'theta_degrees': float(np.degrees(np.arctan2(direction[1],direction[0]))) if sensor['up_axis'] == 2 else None,
        'workplane_theta_degrees': theta, 'workplane_position': [float(point@f),float(point@w0)],
        'direction_source': direction.tolist(), 'position_unit': 'm' if scale is not None else 'native_unknown',
        'reference_point': 'fork_face_width_center', 'height_reference_not_annotated': True,
        'normal': None, 'roll': None, 'pitch': None, 'quaternion': None,
        'full_six_dof': False, 'robot_command_eligible': False,
        'coordinate_frame': 'source_axes_scaled' if scale is not None else 'source_native',
        'source_scale_to_m': scale, 'axes_confirmed': sensor['axes_confirmed']}


class GeometryEngine:
    def __init__(self, dataset, cache):
        self.dataset, self.cache = Path(dataset), Path(cache)
        self.cache.mkdir(parents=True, exist_ok=True)
        self.semaphore = threading.BoundedSemaphore(1)
        self.code_hash = hashlib.sha256(Path(__file__).read_bytes()+Path(__file__).with_name('fork_geometry.py').read_bytes()).hexdigest()

    @lru_cache(maxsize=6)
    def registered(self, source_json):
        source = json.loads(source_json)
        pcd = Path(source['source_paths']['pcd'])
        if digest(pcd) != source['source_anchor_hash']:
            raise ValueError('原始PCD哈希不匹配，拒绝读取')
        image = self.dataset/source['image']
        if digest(image) != source['files'][source['image']]:
            raise ValueError('原图哈希不匹配')
        header, points = read_pcd(pcd)
        w,h = source['size']
        if len(points) != w*h or source['sensor_policy']['grid'] != [h,w]:
            raise ValueError('点云与图像网格不匹配')
        if (int(header['HEIGHT'][0]),int(header['WIDTH'][0])) not in ((h,w),(1,h*w)):
            raise ValueError('PCD头网格不匹配')
        if source['pixel_rotation_degrees'] != source['sensor_policy'].get('pixel_rotation_degrees',0):
            raise ValueError('图像旋转配置不匹配')
        mapping = orient_grid(np.arange(len(points),dtype=np.int32).reshape(h,w),source['sensor_policy'])
        return points, mapping

    def compute(self, key, source, polygon, sensor):
        w,h = source['size']
        polygon = polygon_value(polygon,w,h)
        sensor = sensor_value(sensor,source)
        binding = {'version':VERSION,'code':self.code_hash,'key':key,
            'pcd':source['source_anchor_hash'],'polygon':polygon,'sensor':sensor}
        token = hashlib.sha256(canonical(binding)).hexdigest()
        path = self.cache/(token+'.npz')
        meta_path = self.cache/(token+'.json')
        with self.semaphore:
            if path.exists() and meta_path.exists() and (self.cache/(token+'.bin')).exists():
                return json.loads(meta_path.read_text())
            points, mapping = self.registered(canonical(source).decode())
            mask = raster_mask(polygon,w,h)
            yy,xx = np.nonzero(mask)
            if not len(xx):
                raise ValueError('区域不包含像素中心')
            x0,x1,y0,y1 = xx.min(),xx.max()+1,yy.min(),yy.max()+1
            grid = points[mapping[y0:y1,x0:x1],:3].astype(np.float64)
            grid[~mask[y0:y1,x0:x1]] = np.nan
            scale = sensor['scale_to_m']
            config = dict(DEFAULTS, ransac_max_iterations=180, maximum_planes_per_region=6)
            result = decompose_roi(grid, config=config, scale_to_mm=scale*1000 if scale is not None else None)
            uv = result['uv']+np.array([x0,y0])
            ids = mapping[uv[:,1],uv[:,0]]
            xyz = points[ids,:3].astype(np.float64)*(scale if scale is not None else 1.)
            labels = np.zeros(len(ids),np.int32)
            candidates = []
            h_axis = np.eye(3)[sensor['up_axis']]*sensor['up_sign']
            f_axis = np.eye(3)[sensor['forward_axis']]*sensor['forward_sign']
            # Geometric candidates only; rejected/background surfaces remain selectable.
            ranked = sorted(result['faces'], key=lambda f: -f['points']*
                max(.01,abs(np.dot(f['plane'][:3],f_axis)))**2*
                (.05 if f['status'] != 'selected_candidate' or abs(np.dot(f['plane'][:3],h_axis)) > .75 else 1.))
            for number, face in enumerate(ranked[:24],1):
                ix = face['indices']; labels[ix] = number
                normal = np.array(face['plane'][:3])
                basis = basis_for(normal,sensor)
                p = xyz[ix]; local = p@basis
                low,high = np.quantile(local,[.01,.99],axis=0)
                center = basis@np.array([(low[0]+high[0])/2,np.median(local[:,1]),np.median(local[:,2])])
                candidates.append({'id':number,'points':len(ix),'normal':normal.tolist(),
                    'center':center.tolist(),'basis':basis.tolist(),
                    'half_width':max(float((high[0]-low[0])/2),1e-8),
                    'residual_median':float(face['residual_median']/(1000 if scale is not None else 1)),
                    'regions':face['regions'],'status':face['status'],'flags':face['reasons']})
            basis = basis_for(f_axis,sensor)
            if len(xyz):
                low,high = np.quantile(xyz@basis,[.01,.99],axis=0)
                center = np.median(xyz,axis=0)
                half = max(float((high[0]-low[0])/2),1e-8)
            else:
                center,half = np.zeros(3),.5
            manual = {'id':0,'points':len(xyz),'normal':f_axis.tolist(),'center':center.tolist(),
                      'basis':basis.tolist(),'half_width':half,'status':'manual_all_roi'}
            # Float32 preview only. Full measured coordinates/indices are saved separately.
            good = valid_xyz(points[:,:3])
            context_ids = np.flatnonzero(good)
            if len(context_ids)>18000:
                context_ids = context_ids[np.random.default_rng(42).choice(len(context_ids),18000,replace=False)]
            context_ids = context_ids[~np.isin(context_ids,ids)]
            sampled = np.arange(len(ids))
            if len(sampled)>30000:
                sampled = np.sort(np.random.default_rng(43).choice(len(ids),30000,replace=False))
            context = np.column_stack((points[context_ids,:3]*(scale if scale is not None else 1.),
                                       np.full(len(context_ids),-1),context_ids))
            preview = np.vstack((context,np.column_stack((xyz[sampled],labels[sampled],ids[sampled])))).astype('<f4')
            temporary = path.with_suffix('.tmp')
            with temporary.open('wb') as stream:
                np.savez_compressed(stream,source_indices=ids.astype(np.int32),uv=uv.astype(np.int32),
                    xyz=xyz,face_ids=labels,polygon=np.asarray(polygon),mask=mask)
            temporary.replace(path)
            (self.cache/(token+'.bin')).write_bytes(preview.tobytes())
            meta = {'token':token,'target_key':key,'polygon':polygon,'sensor':sensor,
                'candidates':candidates,'manual':manual,'roi_points':len(ids),
                'regions':result['regions'],'preview_points':len(preview),'preview_stride':5,
                'preview_dtype':'little_endian_float32','geometry_version':VERSION,
                'full_resolution_artifact_sha256':digest(path),'no_pose_GT_used':True}
            meta_path.write_bytes(canonical(meta))
            return meta

    def load(self, token, key):
        if not isinstance(token,str) or len(token)!=64 or any(c not in '0123456789abcdef' for c in token):
            raise ValueError('无效点云版本')
        path = self.cache/(token+'.json')
        if not path.exists():
            raise ValueError('点云版本不存在，请重新提取')
        meta = json.loads(path.read_text())
        if meta['target_key'] != key:
            raise ValueError('点云版本不属于当前目标')
        return meta

    def selected(self, meta, ids, exclusions):
        valid = {c['id'] for c in meta['candidates']} | {0}
        if not isinstance(ids,list) or not ids or any(type(i) is not int or i not in valid for i in ids):
            raise ValueError('请选择有效叉取面，或选择手动全部区域')
        with np.load(self.cache/(meta['token']+'.npz'),allow_pickle=False) as z:
            data = {k:z[k] for k in z.files}
        keep = np.ones(len(data['xyz']),bool) if 0 in ids else np.isin(data['face_ids'],ids)
        keep &= retained(data['xyz'],exclusions)
        data['selected'] = keep
        return data
