#!/usr/bin/env python

# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020

@author: Maxi
"""

import roslib
import rospy
import sys

import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer

# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType

import RPi.GPIO as GPIO
import Pin


class Switch:
    def __init__(self):
        """ configure RPi, GPIO """
        print(GPIO.getmode())
        self.is_high = False

        """ configure ROS """
        self.node()
        
        init_message = rospy.get_param('~message', 'hello')
        rate = float(rospy.get_param('~rate', '1.0'))
        topic = rospy.get_param('~topic', 'chatter')
        rospy.loginfo('rate = %d', rate)
        rospy.loginfo('topic = %s', topic)

        # create a dynamic reconfigure server
        self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
        # create a publisher
        self.pub = rospy.Publisher(topic, node_example, queue_size=10)
        # set the message to publish
        msg = node_example()
        msg.a = 1
        msg.b = 2
        msg.message = init_message


        """ start main processing loop """
        while not rospy.is_shutdown():
            # self.send_message()    
            self.detect_IO_change()
            # if self.event_based:
                # self.check_IO_value()      

            # sleep before next message
            rospy.sleep(1/rate)  # rate != 0

    def reconfigure(self, config, level):
        # fill in local variables from dynamic reconfigure items
        self.message = config["message"]
        self.a = config["a"]
        self.b = config["b"]

        rospy.loginfo('reconfiguring with %s' % config)
        return config

    """ trying to split stuff into functions """

    def setup_pwr_low(self):
        """ for switches connected to 3.3V --> will send 'LOW' when closing """
        print("setting up GPIO")
        print('setting PULL DOWN, expect to send LOW')
        GPIO.setup(self.pin_out,
                    GPIO.OUT)
        GPIO.output(self.pin_out,
                    1)
        GPIO.setup(self.pin,
                   GPIO.IN,
                   pull_up_down=GPIO.PUD_DOWN)
        GPIO.add_event_detect(self.pin,
                              GPIO.FALLING,
                              callback=self.send_if_low,
                              bouncetime=1000)
                              
    def setup_pwr_high(self):
        """ for switches connected to 3.3V --> will send 'HIGH' when closing """
        print("setting up GPIO")
        print('setting PULL DOWN, expect to send LOW')
        GPIO.setup(self.pin_out,
                    GPIO.OUT)
        GPIO.output(self.pin_out,
                    1)
        GPIO.setup(self.pin,
                   GPIO.IN,
                   pull_up_down=GPIO.PUD_UP)
        GPIO.add_event_detect(self.pin,
                              GPIO.RISING,
                              callback=self.send_if_high,
                              bouncetime=1000)
    def setup_gnd_low(self):
        """ for switches connected to GND --> will send 'LOW' when triggered """
        print("setting up GPIO")
        print('setting PULL UP, expect to send HIGH')
        GPIO.setup(self.pin_out,
                    GPIO.OUT)
        GPIO.output(self.pin_out,
                    0)
        GPIO.setup(self.pin,
                   GPIO.IN,
                   pull_up_down=GPIO.PUD_UP)
        GPIO.add_event_detect(self.pin,
                              GPIO.FALLING,
                              callback=self.send_if_low,
                              bouncetime=1000)
    def setup_gnd_high(self):
        """ for switches connected to GND --> will send 'HIGH' when triggered """
        print("setting up GPIO")
        print('setting PULL UP, expect to send HIGH')
        GPIO.setup(self.pin_out,
                    GPIO.OUT)
        GPIO.output(self.pin_out,
                    0)
        GPIO.setup(self.pin,
                   GPIO.IN,
                   pull_up_down=GPIO.PUD_UP)
        GPIO.add_event_detect(self.pin,
                              GPIO.RISING,
                              callback=self.send_if_high,
                              bouncetime=1000)

    def node(self):
        # initialize the node
        rospy.loginfo('launching node')
        rospy.init_node('switch', anonymous=True)

    def send_message(self):
        # set the message to publish
        msg = node_example()

        msg.message = self.message
        msg.a = self.a
        msg.b = self.b
        self.pub.publish(msg)
        rospy.loginfo(rospy.get_caller_id() + ' sent message a: %s, b: %s' % (self.a, self.b))
        
    def set_output(self):
        GPIO.setup(self.output_pin,
                    GPIO.OUT)

    def send_if_high(self, channel=None):
      # if self.check_IO_value():
      rospy.loginfo(rospy.get_caller_id() + 'Switch is HIGH == released')
      my_msg = msg.String()
      my_msg.data = 'HIGH'
      self.SwitchPub.publish(my_msg)
          
    def send_if_low(self, channel=None):
      # if not self.check_IO_value():
      rospy.loginfo(rospy.get_caller_id() + 'Switch is LOW == pressed')
      my_msg = msg.String()
      my_msg.data = 'LOW'
      self.SwitchPub.publish(my_msg)

    def check_IO_value(self):
        if GPIO.input(self.pin) == GPIO.LOW:
            rospy.loginfo('%s is LOW' % self.pin)
            return False
        elif GPIO.input(self.pin) == GPIO.HIGH:
            rospy.loginfo('%s is HIGH' % self.pin)
            return True

    def detect_IO_change(self):
        self.is_high_old = self.is_high
        self.is_high = self.check_IO_value()
        # if self.is_high_old and not self.is_high:
            # self.send_if_low()
        # elif not self.is_high_old and self.is_high:
            # self.send_if_high()
        # else:
            # pass
        if self.is_high:
            self.send_if_high()
        else:
            self.send_if_low()
        rospy.sleep(0.3)