#!/usr/bin/env python

# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020

@author: Maxi
"""

import re
import roslib
import rospy
import sys
import time

import std_msgs.msg as msg

import pigpio
import Pin

class Rotation:
    def __init__(self):
        """ setup ROS """
        rospy.init_node('Rotation')
        self.pub = rospy.Publisher('rotation',
                                    msg.String,
                                    queue_size=2)
        self.listener_test = rospy.Subscriber('tests',
                                            msg.String,
                                            self.callback)
        self.listener_switch = rospy.Subscriber('forward_switch',
                                                msg.String,
                                                self.calibrate_forward)
        self.listener_cmd = rospy.Subscriber('command',
                                            msg.String,
                                            self.callback)
                                            
        """ constants """
        self.position = 0  # degrees
        self.rot_speed = 0.081  # degrees per step
        self.test = False
        self.epsilon = 1  # degrees
        
        """ setup GPIO """
        self.frequency = 100  # Hz = steps per second
        self.dutycycle = 0  # % - 50 % is good

        self.pi = pigpio.pi()

        self.pi.set_mode(Pin.M_Rot_enable, pigpio.OUTPUT)  # high wenn aus, low wenn an
        self.pi.set_mode(Pin.M_Rot_dir, pigpio.OUTPUT)
        self.pi.set_mode(Pin.M_Rot_speed, pigpio.OUTPUT)
        
        self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0)  # frequency, dutycycle

        while not rospy.is_shutdown():
            self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus
            rospy.spin()

    def callback(self, data):
        rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
                      data.data)
        if data.data == 'STOP':
            self.stop()
        elif data.data == 'CENTER':
            self.center_position()
        elif 'ROTATE' in data.data:
            try:
                numbers = re.findall('-*[0-9]+', data.data)
                self.go_to_degrees(int(numbers[0]))
            except ValueError as E:
                rospy.loginfo(E)

    def calibrate_forward(self, data):
        rospy.loginfo(' I heard %s', data.data)
        if data.data == 'HIGH':
            self.position = 0  # degrees

    def center_position(self):
        self.go_to_degrees(0)

    def go_to_degrees(self, degrees):
        start = time.time()
        while not abs(self.position - degrees) <= self.epsilon:
            then = time.time()
            if self.position > degrees:
                self.counterclockwise()
                signum = -1
            else:
                self.clockwise()
                signum = 1
            self.rotate()
            rospy.loginfo('rotating at %s degrees now' % self.position)
            rospy.sleep(0.2)
            now = time.time()
            self.position = self.position + signum * self.frequency * self.rot_speed * (now - then)
            self.pub.publish(str(self.position))
        self.stop()

    def counterclockwise(self):
        self.pi.write(Pin.M_Rot_dir, 0)

    def clockwise(self):
        self.pi.write(Pin.M_Rot_dir, 1)
            
    def rotate(self):
        self.pi.write(Pin.M_Rot_enable, 0)  # low wenn an
        self.dutycycle = 50
        self.pi.hardware_PWM(Pin.M_Rot_speed, self.frequency, self.dutycycle * 10000)
        
    def stop(self):
        self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0)
        self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus
        
    def disable(self):
        self.pwm.stop()
        self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus

if __name__ == '__main__':
    try:
        Rotation()
    except rospy.ROSInterruptException:
        pass
    # finally:
        # GPIO.cleanup()