import multiprocessing as mp

from .calibration import ForceConverter
from .config import ControlConfig
from .controller import TimedCycleGripperController
from .feedback import TactileFeedbackProcessor
from .force_reader import TwoSensorForceReader
from .hardware import GripperClient
from .paths import ensure_project_paths


def build_controller(argv=None):
    ensure_project_paths()
    config = ControlConfig.from_args(argv)

    normal_converter = ForceConverter.from_config(
        config.normal_force_calibration,
        "normal_force_calibration",
    )
    shear_converter = ForceConverter.from_config(
        config.shear_force_calibration,
        "shear_force_calibration",
    )

    reader = TwoSensorForceReader(
        video1=config.video1,
        video2=config.video2,
        config1=config.sensor_config1,
        config2=config.sensor_config2,
        rotation1=config.rotation1,
        rotation2=config.rotation2,
        target_fps=config.sensor_fps,
        cuda=not config.sensor_cpu,
        motion_threshold=config.motion_threshold,
    )
    gripper = GripperClient(config)
    feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter)
    return TimedCycleGripperController(config, reader, gripper, feedback)


def main(argv=None):
    controller = build_controller(argv)
    controller.run()


if __name__ == "__main__":
    mp.freeze_support()
    main()

