import multiprocessing as mp

from gripper_control.calibration import ForceConverter
from gripper_control.feedback import TactileFeedbackProcessor
from gripper_control.force_reader import TwoSensorForceReader
from gripper_control.hardware import GripperClient
from gripper_control.paths import ensure_project_paths

from .controller import SideTriggerGripController
from .settings import Demo02Config


def build_controller(
    argv=None,
    status_callback=None,
    include_visuals=False,
    visual_size=320,
):
    ensure_project_paths()
    config = Demo02Config.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,
        include_visuals=include_visuals,
        visual_size=visual_size,
    )
    gripper = GripperClient(config)
    feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter)
    return SideTriggerGripController(
        config,
        reader,
        gripper,
        feedback,
        status_callback=status_callback,
    )


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


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