#!/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 Servo:
    def __init__(self):
        """ setup ROS """
        rospy.init_node('Servo')
        self.pub = rospy.Publisher('servo',
                                    msg.String,
                                    queue_size=10)
        self.listener_test = rospy.Subscriber('tests',
                                            msg.String,
                                            self.callback)
        self.listener_cmd = rospy.Subscriber('command',
                                            msg.String,
                                            self.callback)
        """ constants """
        # TODO ggf. konstanten tauschen
        self.position = 0  # degrees
        self.t_max = 1500E-6  # ms == 0 deg
        self.t_min = 500E-6  # ms == 180 deg
        self.alpha_max = 0  # deg
        self.alpha_min = 90  # deg
        
        """ setup GPIO """
        self.frequency = 50.0  # Hz
        self.dutycycle = 0  # %

        self.pi = pigpio.pi()

        #self.pi.set_mode(Pin.M_Cam, pigpio.OUTPUT) # eher ALT1=PWM1 fuer bcm13
        #self.pi.set_mode(Pin.M_Cam, pigpio.ALT1)

        #print(">> GPIO Mode: " + str(self.pi.get_mode(Pin.M_Cam)) + " (1: OUTPUT; 5: ALT1=PWM1)")

        #self.pi.hardware_PWM(Pin.M_Cam, 0, 0)
        #self.pi.set_PWM_dutycycle(Pin.M_Cam, 0)
        #self.pi.set_PWM_frequency(Pin.M_Cam, self.frequency)




        ## Ersatz GPIO: BCM19
        self.pi.set_mode(19, pigpio.ALT5)
        self.pi.hardware_PWM(19, 400, 0)




        # Test Servo Motion
        # CAUTION: Make sure servo movement is possible, not on storage position
        print(">> Servo Test ...")
        test_servo = False
        if test_servo == True:
	    self.test_servo_motion()
        print(">> Test Done")
		
        while not rospy.is_shutdown():
            rospy.spin()

    def callback(self, data):
        rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
                      data.data)
        if data.data == 'STOP':
            self.disable()
        elif 'SERVO' in data.data:
            try:
                numbers = re.findall('[0-9]+', data.data)
                angle = int(numbers[0])
                rospy.loginfo('Angle %s', angle)
                if angle >= 0 and angle <= 75:
                    self.go_to_degrees(int(angle))
                else:
                    rospy.loginfo('angle %s not possible', angle)
            except Exception as E:
                rospy.loginfo(E)
                rospy.loginfo('unrecognised message format %s', data.data)

    def go_to_degrees(self, degrees):
        self.step_calc(degrees)
        if self.stepwidth <= 1500E-6 and self.stepwidth >=740E-6:
            # Catching runtime errors
            if self.dutycycle < 0:
                self.dutycycle = 0
            if self.dutycycle > 255:
                self.dutycycle = 255
            self.pi.hardware_PWM(19, 400, self.calc_dutycycle(degrees))
            #self.pi.hardware_PWM(Pin.M_Cam, self.frequency, self.dutycycle)
            #self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)
            rospy.loginfo('rotating at %s degrees now', degrees)
        else:
            rospy.loginfo('stepwidth %s not possible', self.stepwidth)
        self.pub.publish(str(degrees))
        
    def disable(self):
        self.dutycycle = 0
        self.pi.hardware_PWM(19, 0, 0)
        #self.pi.hardware_PWM(Pin.M_Cam, self.frequency, 0)
        #self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)


    def step_calc(self, angle):
        # linearer Zusammenhang
        self.stepwidth = ((self.t_max - self.t_min)/(self.alpha_max - self.alpha_min)) * (angle - self.alpha_min) + self.t_min
        self.dutycycle = self.calculate_pwm(self.stepwidth)

    def calculate_pwm(self, stepwidth):
        time_per_tick = 1 / self.frequency
        pwm_fraction = stepwidth / time_per_tick
        pwm_percent = pwm_fraction * 100
        #pwm_set = pwm_percent * 10000
        pwm_set = pwm_percent
        rospy.loginfo('stepwidth: %s sec\nticktime:  %s sec\npwm_set:   %s %%', stepwidth, time_per_tick, pwm_set)
        return pwm_set
		
    def test_servo_motion(self):
        for i in range(0,65,5):
            self.go_to_degrees(i)
            rospy.sleep(3.0)
        self.go_to_degrees(0)
	
    def calc_dutycycle(angle):
        frequency = 400
        steps_max = 250000000/frequency
        t_servo_max = 2500 # us
        t_servo_min = 500 # us
        angle_max = 100 # enstpricht pwm > 50 percent
        angle_min = -100  # enspricht pwm = 0
        steps_min = (t_servo_min / t_servo_max) * steps_max
        y0 = (steps_max-steps_min)/2
        duty = (steps_max-steps_min)/(angle_max-angle_min) * angle + y0
        print("\nDutycycle: " + str(duty) + "\n")
        return duty

if __name__ == '__main__':
    try:
        s = Servo()
    except rospy.ROSInterruptException:
        pass
    finally:
        try:
            s.disable()
        except:
            pass