#!/usr/bin/env python


# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:22:52 2020

@author: Maxi
"""

import roslib
import rospy
import sys
import time

import std_msgs.msg as msg

import pigpio
import RPi.GPIO as GPIO
import Pin


class Elevation:
    def __init__(self):
        self.test = False
        self.direction = None
        self.destination = None
        self.state = 'stowed'
        self.angle = 0.0  # deg
        self.speed = 0.05  # deg per second
        self.instances = 0
        
        """ setup ROS """
        rospy.init_node('Elevation')
        self.pub = rospy.Publisher('elevation',
                                    msg.String,
                                    queue_size=2)
        self.listener_test = rospy.Subscriber('tests',
                                            msg.String,
                                            self.callback)
        self.listener_cmd = rospy.Subscriber('command',
                                            msg.String,
                                            self.callback)
        self.listener_stow = rospy.Subscriber('stowed_switch',
                                            msg.String,
                                            self.callback,
                                            callback_args='stow')
        self.listener_raise = rospy.Subscriber('raised_switch',
                                            msg.String,
                                            self.callback,
                                            callback_args='raise')
        self.listener_tilt = rospy.Subscriber('tilt_switch',
                                            msg.String,
                                            self.callback,
                                            callback_args='tilt')
        """ setup GPIO """
        self.frequency = 500  # Hz
        self.dutycycle = 0  # %

        self.pi = pigpio.pi()

        self.pi.set_mode(Pin.M_Elev_dir, pigpio.OUTPUT)
        self.pi.set_mode(Pin.M_Elev_speed, pigpio.OUTPUT)

        self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
        self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)

        # GPIO.setup(Pin.M_Elev, GPIO.OUT)
        # self.pwm = GPIO.PWM(Pin.M_Cam, self.frequency)
        # self.pwm.start(self.dutycycle)

        """ keep node running """
        while not rospy.is_shutdown():
            rospy.spin()



    def callback(self, data, switch=None):
        rospy.loginfo(rospy.get_caller_id() + ' I heard %s from %s',
                    data.data, switch)
        
        tilt_cmd_recognized = 'TILT' in data.data # true,false
        
        if data.data == 'STOP':
            self.state = None
            self.stop()
        
        elif data.data == 'HIGH' and switch == 'raise':
            self.state = 'raised'
            self.angle = 0
            if self.destination == 'up':
                rospy.sleep(0.01)
                self.stop()
            elif self.destination == 'tilt': # Achtung! Tilt ist End stop!
                self.tilt()
        
        elif data.data == 'LOW' and switch == 'stow':
            self.stop()
            self.state = 'stowed'
            self.angle = 90
            if not self.direction == 'forwards': # einklappen?
                rospy.sleep(0.3)
                self.stop()
            if self.destination == 'up':
                self.elevate()
                pass
        
        elif data.data == 'HIGH' and switch == 'stow':
            pass
            #if self.destination == 'down' and not self.state == 'moving':
            #    self.retract()
                
        elif data.data == 'HIGH' and switch == 'tilt': # Tilt ist Elevation end stop
            if self.state == "stowed" or self.state == None or self.state == "raised":
                self.stop()
            else:
                self.state = 'tilted' # tilted backwards, touching end stop
                self.angle = -30
                self.stop()
                if not self.direction == 'backwards':
                    self.stop()
                if self.destination == 'up':
                    if not self.state == "raised": #pw
                        self.elevate()
        
        elif data.data == 'ELEVATE' and not self.state == 'raised':
            self.destination = 'up'
            self.state = "moving" # pw
            self.elevate()
        elif data.data == 'STOW' and not self.state == 'stowed':
            self.destination = 'down'
            self.state = "moving" # pw
            self.retract()
            """
            elif data.data == 'TILT' and not self.state == 'tilted':
                self.destination = 'tilt'
                self.state == "moving" #pw
                self.tilt() #pw
            
            elif data.data == 'TILT_10' and not self.state == 'tilted':
                self.destination = 'tilt'
                self.state == "moving" #pw
                self.tilt_variabletime(10) #pw
            elif data.data == 'TILT_15' and not self.state == 'tilted':
                self.destination = 'tilt'
                self.state == "moving" #pw
                self.tilt_variabletime(15) #pw
            elif data.data == 'TILT_18' and not self.state == 'tilted':
                self.destination = 'tilt'
                self.state == "moving" #pw
                self.tilt_variabletime(18) #pw
            """
        #elif tilt_cmd_recognized == True and not self.state == 'tilted':
        elif 'TILT' in data.data and not self.state == 'tilted':
            self.destination = 'tilt'
            self.state == "moving" #pw
            
            # Extract info from string
            start_marker = data.data.find('TILT_') + 5 #pw
            tilt_goal = data.data[start_marker:-1]
            for i in range(0,5):
                print(">> Tilt Goal set: " + str(tilt_goal))
            tilt_goal = int(tilt_goal)
            for i in range(0,5):
                print(">> Tilt Goal set: " + str(tilt_goal))
            if tilt_goal < 21:
                self.tilt_variabletime(tilt_goal) #pw
            else:
                print(">> Tilt Goal out of bounds!")
                
        rospy.loginfo('STATE: %s', self.state)
        rospy.loginfo('DIRECTION: %s', self.destination)
        self.pub.publish(msg.String('STATE: %s' % self.state))
        if not self.state == 'moving':
            self.pub.publish(msg.String('ANGLE: %s' % self.angle))
            rospy.loginfo('ANGLE: %s', self.angle)

    def elevate(self):
        if self.state == 'tilted':
            self.pi.write(Pin.M_Elev_dir, 1)
            self.direction = 'backwards'
            rospy.loginfo('moving backwards')
        else:
            self.pi.write(Pin.M_Elev_dir, 0)
            self.direction = 'forwards'
            rospy.loginfo('moving forwards')
        self.move()
        self.pub.publish(msg.String('raising mast'))

    def retract(self):
        self.pi.write(Pin.M_Elev_dir, 1)
        self.direction = 'forwards'
        self.pub.publish(msg.String('retracting mast'))
        then = time.time()
        self.move() # self.state = "moving" in dieser funktion!
        maxtime = 25.3 # max Zeit fuer retract von raised position
        now = time.time()
        while (now-then)<maxtime:
            now = time.time()
        self.stop()
        self.state = 'stowed'

    def tilt(self):
        self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
        self.direction = 'forwards'
        self.pub.publish(msg.String('tilting mast'))
        then = time.time()
        self.move()
        maxtime = 10.0 # Zeit bis in tilt position
        now = time.time()
        while (now-then)<maxtime:
            now = time.time()
        self.stop()
        self.state = None # do not set als tilted, tilted means end stop
        
    def tilt_variabletime(self, dt_movement=0):
        self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
        self.direction = 'forwards'
        self.pub.publish(msg.String('tilting mast'))
        then = time.time()
        self.move()
        maxtime = int(dt_movement) # Zeit bis in tilt position
        now = time.time()
        while (now-then)<maxtime:
            now = time.time()
        self.stop()
        self.state = None # do not set als tilted, tilted means end stop

    def move(self):
        # self.instances = self.instances + 1
        self.state = 'moving'
        self.pub.publish(msg.String('STATE: %s' % self.state))
        self.dutycycle = 160
        self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)
        self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, self.dutycycle)
        rospy.loginfo("Moving at Frequency: %s, Dutycycle %s, Direction %s", self.frequency, self.dutycycle, self.direction)
        
        # then = time.time()
        # while self.state == 'moving':
            # now = time.time()
            # delta_t = now - then
            # if self.direction == 'backwards':
                # self.angle = self.angle + self.speed * delta_t
            # else:
                # self.angle = self.angle - self.speed * delta_t
            # rospy.loginfo('ANGLE: %s', self.angle)
            # self.pub.publish(msg.String('ANGLE: %s' % self.angle))
            # rospy.sleep(0.2)
            # if not self.instances == 1:
                # self.instances = self.instances - 1
                # break        
            
    def stop(self):
        self.dutycycle = 0
        self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
        self.pub.publish(msg.String('was moving in direction: %s' % self.direction))
        self.pub.publish(msg.String('Stopped Moving'))
        rospy.loginfo('State: %s' % self.state)


if __name__ == '__main__':
    try:
        e = Elevation()
    except rospy.ROSInterruptException:
        try:
            e.disable()
        except Exception as E:
            print(E)