from dataclasses import dataclass
import time

from .filters import ExponentialFilter


def shear_magnitude(sample):
    return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5


def _component_shear_enabled(converter):
    return getattr(converter, "input_mode", "magnitude") == "components"


@dataclass
class ForceBaseline:
    left_normal: float = 0.0
    right_normal: float = 0.0
    left_shear: float = 0.0
    right_shear: float = 0.0
    left_shear_x: float = 0.0
    left_shear_y: float = 0.0
    right_shear_x: float = 0.0
    right_shear_y: float = 0.0


@dataclass
class ForceFeedback:
    left_normal: float
    right_normal: float
    min_normal: float
    max_normal: float
    normal_diff: float
    left_shear: float
    right_shear: float
    max_shear: float
    shear_diff: float
    left_shear_x: float = 0.0
    left_shear_y: float = 0.0
    right_shear_x: float = 0.0
    right_shear_y: float = 0.0


class TactileFeedbackProcessor:
    def __init__(self, config, normal_converter, shear_converter):
        self.config = config
        self.normal_converter = normal_converter
        self.shear_converter = shear_converter
        self.baseline = ForceBaseline()
        self.reset_filters()

    def reset_filters(self):
        self.left_normal_filter = ExponentialFilter(self.config.filter_alpha)
        self.right_normal_filter = ExponentialFilter(self.config.filter_alpha)
        self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
        self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
        self.left_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
        self.left_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
        self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
        self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)

    def _convert_shear(self, sample):
        if _component_shear_enabled(self.shear_converter):
            shear_x = self.shear_converter.convert_component(sample["fshearx"])
            shear_y = self.shear_converter.convert_component(sample["fsheary"])
            return (shear_x ** 2 + shear_y ** 2) ** 0.5, shear_x, shear_y
        return self.shear_converter.convert(shear_magnitude(sample)), 0.0, 0.0

    def calibrate_baseline(self, reader):
        seconds = self.config.baseline_seconds
        if seconds <= 0:
            self.baseline = ForceBaseline()
            return self.baseline

        left_values = []
        right_values = []
        left_shear_values = []
        right_shear_values = []
        left_shear_x_values = []
        left_shear_y_values = []
        right_shear_x_values = []
        right_shear_y_values = []
        deadline = time.perf_counter() + seconds
        while time.perf_counter() < deadline:
            left, right = reader.get_samples(timeout=0.2)
            if left is not None:
                left_values.append(self.normal_converter.convert(left["fnormal"]))
                shear, shear_x, shear_y = self._convert_shear(left)
                left_shear_values.append(shear)
                left_shear_x_values.append(shear_x)
                left_shear_y_values.append(shear_y)
            if right is not None:
                right_values.append(self.normal_converter.convert(right["fnormal"]))
                shear, shear_x, shear_y = self._convert_shear(right)
                right_shear_values.append(shear)
                right_shear_x_values.append(shear_x)
                right_shear_y_values.append(shear_y)
            time.sleep(0.01)

        self.baseline = ForceBaseline(
            left_normal=sum(left_values) / len(left_values) if left_values else 0.0,
            right_normal=sum(right_values) / len(right_values) if right_values else 0.0,
            left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0,
            right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0,
            left_shear_x=sum(left_shear_x_values) / len(left_shear_x_values) if left_shear_x_values else 0.0,
            left_shear_y=sum(left_shear_y_values) / len(left_shear_y_values) if left_shear_y_values else 0.0,
            right_shear_x=sum(right_shear_x_values) / len(right_shear_x_values) if right_shear_x_values else 0.0,
            right_shear_y=sum(right_shear_y_values) / len(right_shear_y_values) if right_shear_y_values else 0.0,
        )
        return self.baseline

    def read(self, reader, timeout=0.3):
        left, right = reader.get_samples(timeout=timeout)
        if left is None or right is None:
            return None

        left_force = max(
            0.0,
            self.normal_converter.convert(left["fnormal"]) - self.baseline.left_normal,
        )
        right_force = max(
            0.0,
            self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal,
        )
        if _component_shear_enabled(self.shear_converter):
            _, raw_left_shear_x, raw_left_shear_y = self._convert_shear(left)
            _, raw_right_shear_x, raw_right_shear_y = self._convert_shear(right)
            left_shear_x = raw_left_shear_x - self.baseline.left_shear_x
            left_shear_y = raw_left_shear_y - self.baseline.left_shear_y
            right_shear_x = raw_right_shear_x - self.baseline.right_shear_x
            right_shear_y = raw_right_shear_y - self.baseline.right_shear_y
            left_shear = (left_shear_x ** 2 + left_shear_y ** 2) ** 0.5
            right_shear = (right_shear_x ** 2 + right_shear_y ** 2) ** 0.5
        else:
            left_shear_x = left_shear_y = right_shear_x = right_shear_y = 0.0
            left_shear = max(
                0.0,
                self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear,
            )
            right_shear = max(
                0.0,
                self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
            )

        left_normal = self.left_normal_filter.update(left_force)
        right_normal = self.right_normal_filter.update(right_force)
        left_shear_filtered = self.left_shear_filter.update(left_shear)
        right_shear_filtered = self.right_shear_filter.update(right_shear)
        left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x)
        left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y)
        right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x)
        right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y)

        return ForceFeedback(
            left_normal=left_normal,
            right_normal=right_normal,
            min_normal=min(left_normal, right_normal),
            max_normal=max(left_normal, right_normal),
            normal_diff=left_normal - right_normal,
            left_shear=left_shear_filtered,
            right_shear=right_shear_filtered,
            max_shear=max(left_shear_filtered, right_shear_filtered),
            shear_diff=left_shear_filtered - right_shear_filtered,
            left_shear_x=left_shear_x_filtered,
            left_shear_y=left_shear_y_filtered,
            right_shear_x=right_shear_x_filtered,
            right_shear_y=right_shear_y_filtered,
        )
