
# subscribe to topic: /redsensor (type: float)
#
# publish to: 	/speedsetpointL  (type: float)
# 		/speedsetpointR  (type: float)
#

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension

class VelocitySetpointGenerator(Node):

    SpeedSP = Float32MultiArray()
    
    def __init__(self):
        super().__init__('velocity_setpoint_generator')
        self.subscription = self.create_subscription(
            Float32, 
            'redsensor',
            self.controller_callback,
            10)
        self.publisherSP = self.create_publisher(Float32MultiArray, 'speedsetpoint', 10)
        self.subscription  # prevent unused variable warning

        self.DistanceCenterToWheel = 0.07
        self.ForwardSpeedSetpoint = 0.1
        self.K = 2.0
        
        #self.SpeedSP.layout
        d = MultiArrayDimension()
        d.stride = 0
        d.size = 2
        self.SpeedSP.layout.dim = [d]

    def controller_callback(self, msg):
        #self.get_logger().info('I heard: "%s"' % msg.data)
        
        omega = self.K * self.make_dimensionless(msg.data)
        L = self.ForwardSpeedSetpoint - omega * self.DistanceCenterToWheel
        R = self.ForwardSpeedSetpoint + omega * self.DistanceCenterToWheel
        self.SpeedSP.data = [L, R]
        self.publisherSP.publish(self.SpeedSP)
        
    def make_dimensionless(self, meas):
        Dark = 0.6
        Bright = 1.0
        Threshold1 = 0.78
        Threshold2 = 0.83
        if meas < Dark:
            dimlessvalue = -1.0
        elif meas < Threshold1:
            dimlessvalue = -1.0 * (Threshold1 - meas ) / (Threshold1 - Dark)
        elif meas < Threshold2:
            dimlessvalue = 0.0
        elif meas < Bright:
            dimlessvalue =  (meas - Threshold2) / (Bright - Threshold2)
        else:
            dimlessvalue = 1.0
        return dimlessvalue
    
def main(args=None):
    rclpy.init(args=args)

    vel_controller = VelocitySetpointGenerator()
    rclpy.spin(vel_controller)

    vel_controller.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

