#!/usr/bin/env python

# Copyright 2022 Trossen Robotics
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#    * Redistributions of source code must retain the above copyright
#      notice, this list of conditions and the following disclaimer.
#
#    * Redistributions in binary form must reproduce the above copyright
#      notice, this list of conditions and the following disclaimer in the
#      documentation and/or other materials provided with the distribution.
#
#    * Neither the name of the copyright holder nor the names of its
#      contributors may be used to endorse or promote products derived from
#      this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

"""Contains the `RpiPixels` ROS2 node that controls of neopixels through an RPi GPIO pin."""

import time

from interbotix_rpi_msgs.msg import PixelCommands
import rclpy
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from rpi_ws281x import PixelStrip

class RpiPixels(Node):
    """The RpiPixels node controls neopixels through an RPi GPIO pin."""

    def __init__(self):
        """Construct the RpiPixels node."""
        rclpy.init()
        super().__init__(node_name='rpi_pixels')

        self.declare_parameter('num_pixels')
        self.declare_parameter('gpio_pin')
        self.num_pixels = self.get_parameter('num_pixels').get_parameter_value().integer_value
        gpio_pin = self.get_parameter('gpio_pin').get_parameter_value().integer_value
        self.pixels = PixelStrip(self.num_pixels, gpio_pin)
        self.pixels.begin()
        self.sub_pixels = self.create_subscription(
            PixelCommands,
            'commands/pixels',
            self.pixel_cb)

    def set_color(
        self,
        pixel: int = 0,
        color: int = 0x000000,
        set_all_leds: bool = False
    ):
        """
        Set the color of an LED, or all LEDs.

        :param pixel: (optional) The pixel number to set the color of; defaults to 0
        :param color: (optional) Hex value of the color to change to; defaults to 0x000000 (black)
        :param set_all_leds: (optional) `True` to set all LEDs to the specified color, `False` to
            set only the LED specified in the `pixel` param; defaults to `False`
        """
        if set_all_leds:
            for i in range(self.num_pixels):
                self.pixels.setPixelColor(i, color)
        else:
            self.pixels.setPixelColor(pixel, color)
        self.pixels.show()

    def set_brightness(self, brightness: int) -> None:
        """
        Set the brighness of all LEDs.

        :param brightness: The brightness to set all LEDs to
        """
        self.pixels.setBrightness(brightness)
        self.pixels.show()

    def brightness_fade_in(self, period: int = 10) -> None:
        """
        Fade brightness in over the specified period.

        :param period: (optional) Time in seconds to fade in over; defaults to 10
        """
        for i in range(256):
            self.set_brightness(i)
            time.sleep(period/1000.0)

    def brightness_fade_out(self, period: int = 10) -> None:
        """
        Fade brightness out over the specified period.

        :param period: (optional) Time in seconds to fade out over; defaults to 10
        """
        for i in range(255, -1, -1):
            self.set_brightness(i)
            time.sleep(period/1000.0)

    def pulse(self, iterations: int = 5, period: int = 10) -> None:
        """
        Pulse all LEDs some number of times with a specified period per pulse.

        Pulsing the LEDs will slowly fade in and slowly fade out some number of times.

        :param iterations: (optional) Number of times to pulse the LEDs; defaults to 5
        :param period: (optional) Period of each pulse in seconds; defaults to 10
        """
        for _ in range(iterations):
            self.brightness_fade_out(period)
            self.brightness_fade_in(period)

    def blink(
        self,
        pixel: int = 0,
        set_all_leds: bool = False,
        period: int = 500,
        iterations: int = 3
    ) -> None:
        """
        Blink one or all LEDs some number of times with a specified period per blink.

        :param pixel: (optional) The pixel number to blink; defaults to 0
        :param set_all_leds: (optional) `True` to blink all LEDs, `False` to blink only the LED
            specified in the `pixel` param; defaults to `False`
        :param period: (optional) Time in milliseconds for each blink; defaults to 500
        :param iterations: (optional) Number of times to blink the LEDs; defaults to 3
        """
        original_color = self.pixels.getPixelColor(pixel)
        original_brightness = self.pixels.getBrightness()
        for _ in range(iterations):
            if set_all_leds: self.set_brightness(0)
            else: self.set_color(pixel)
            time.sleep(period/1000.0)
            if set_all_leds: self.set_brightness(original_brightness)
            else: self.set_color(pixel, original_color)
            time.sleep(period/1000.0)

    def pixel_cb(self, msg: PixelCommands) -> None:
        """
        Command instructions to neopixels through a ROS callback method

        :param msg: The incoming PixelCommands message
        """
        if (msg.cmd_type == 'color'):
            self.set_color(msg.pixel, msg.color, msg.set_all_leds)
        elif (msg.cmd_type == 'brightness'):
            self.set_brightness(msg.brightness)
        elif (msg.cmd_type == 'pulse'):
            self.pulse(msg.iterations, msg.period)
        elif (msg.cmd_type == 'blink'):
            self.blink(msg.pixel, msg.set_all_leds, msg.period, msg.iterations)


def main():
    rpi_pixels = RpiPixels()
    ex = MultiThreadedExecutor()
    ex.add_node(rpi_pixels)
    ex.spin()


if __name__ == '__main__':
    main()
