#!/usr/bin/env python3

from functools import partial
from cell_volume_class import DetectedCell
import numpy as np
import rospy
import tf
from octomap_msgs.msg import Octomap
from sensor_msgs.msg import PointCloud2
from larcc_volumetric.msg import OccupiedPercentage


def percentage_callback(msg, config):
    """Gather octomap message

    Args:
        msg (OccupiedPercentage): message with occupied percentage
    """
    config['percentage'] = msg.percentage
    return


def main():
    # Define initial parameters
    occupied_percentage_topic = '/occupied_percentage'
    occupied_threshold = 0.05
    config = dict()
    config['percentage'] = None

    # Initialize the node
    rospy.init_node('cell_occupancy_subscriber')

    # Subscribe to the OccupiedPercentage
    percentage_callback_partial = partial(percentage_callback, config=config)
    rospy.Subscriber(occupied_percentage_topic, OccupiedPercentage, percentage_callback_partial)
    
    # Defining rate
    rate = rospy.Rate(30)

    # Run
    while not rospy.is_shutdown():
        if config['percentage'] is None:
            continue

        if config['percentage'] >= occupied_threshold:
            print('Somebody in the cell!')
        else:
            print('Nobody in the cell')

        rate.sleep()


if __name__ == '__main__':
    main()