#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
from geometry_msgs.msg import Point

import RPi.GPIO as GPIO
from time import sleep 
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
AN2 =33  #pitch 
AN1 = 32  #roll
DIG2 = 18 #pitch
DIG1 = 37 #roll
GPIO.setup(AN2,GPIO.OUT)
GPIO.setup(AN1, GPIO.OUT)
GPIO.setup(DIG2, GPIO.OUT)
GPIO.setup(DIG1, GPIO.OUT)
sleep(1)
p1=GPIO.PWM(AN1,100)
p2=GPIO.PWM(AN2,100)

__author__= "4dimentional@kau.kr"


target_x = 0;
target_y = 0;
error_x = 0;
error_y = 0;
diff_x = 0;
diff_y = 0;
max_output_x = 76.7;
min_output_x = -76.7;
max_output_y = 76.7;
min_output_y = -76.7;


def head_motion_callback(rec_point):
	global target_y,target_x
	global error_x,error_y
	global diff_x,diff_y
	global max_output_x, min_output_x
	global max_output_y, min_output_y

	Px,Dx =20, 30
	Py,Dy = 20 , 30
	
	prev_x = 0
	prev_y = 0
	
	target_x = rec_point.x
	target_y = rec_point.y
	
	error_x = 0 - target_x
	error_y = 0 - target_y
	
	diff_x = prev_x - error_x
	diff_y = prev_y - error_y
	
	prev_x = error_x
	prev_y = error_y
	
	output_x = Px*error_x + Dx*diff_x
	output_y = Py*error_y + Dy*diff_y
	
	if output_x > max_output_x:
		output_x = max_output_x
	elif output_x < min_output_x:
		output_x = min_output_x
	
	if output_x < 0:
		left_speed = abs(output_x)
		GPIO.output(DIG2,GPIO.LOW)
		p2.start(left_speed)
	elif output_x > 0:
		right_speed = abs(output_x)
		GPIO.output(DIG2,GPIO.HIGH)
		p2.start(right_speed)
		
	if output_y > max_output_y:
		output_y = max_output_y
	elif output_y < min_output_y:
		output_y = min_output_y
	
	if output_y < 0:
		upward_speed = abs(output_y)
		GPIO.output(DIG1,GPIO.LOW)
		p1.start(upward_speed)
	elif output_y > 0:
		downward_speed = abs(output_y)
		GPIO.output(DIG1,GPIO.HIGH)
		p1.start(downward_speed)
		
		
	print(output_y)

def head_vel_callback(headCTR):
	head_roll = headCTR.data
	
	if head_roll==4:
		rospy.loginfo("Head downward")		
		GPIO.output(DIG2, GPIO.HIGH)
		p1.start(0)		# set AN2 as HIGH, M2B will turn ON
		p2.start(30)				# set Direction for M2  
						#delay for 2 second

	if head_roll==2:
		rospy.loginfo("Head upward")
		GPIO.output(DIG2, GPIO.LOW)         
		p1.start(0)                       
		p2.start(30)                       
		                          
			    
	if head_roll==5:
		rospy.loginfo("STOP")
		p1.start(0)                          # Direction can ignore
		p2.start(0)                          # Direction can ignore
		                            #delay for 3 second

def listener():
	rospy.init_node("furo_head")		
	rospy.Subscriber("cmd_head",Float32, head_vel_callback)
	rospy.Subscriber("target_coordinate",Point, head_motion_callback)

	rate = rospy.Rate(200)
	while not rospy.is_shutdown():
		rate.sleep()

	
if __name__ == '__main__':
	listener()