import time
import curses
import pigpio
from pca9685 import pca9685

pan_ch = 0
pan_neutral = 1500
pan_max = 2000
pan_min = 1000
pan_delta = 5

tilt_ch = 2
tilt_neutral = 1500
tilt_max = 1800
tilt_min = 1200
tilt_delta = 5

trig_ch = 4
trig_off = 2200
trig_on = 900

PCA9685_ADDR = 0x40
pwm_freq = 50

pi = pigpio.pi()

servo_drv = pca9685( pi, PCA9685_ADDR )
servo_drv.set_freq( pwm_freq )

stdscr = curses.initscr()
curses.noecho()
curses.cbreak()

servo_drv.set_pulse_t( pan_ch, pan_neutral )
time.sleep( 1 )

servo_drv.set_pulse_t( tilt_ch, tilt_neutral )
time.sleep( 1 )

servo_drv.set_pulse_t( trig_ch, trig_off )
time.sleep( 1 )

pan_pulse = pan_neutral
tilt_pulse = tilt_neutral

print("Ready...")

while True:
	c = stdscr.getch()
	
	if ( c == ord('a') ):
		if ( pan_pulse < pan_max ):
			pan_pulse = pan_pulse + pan_delta
			servo_drv.set_pulse_t( pan_ch, pan_pulse )

	elif ( c == ord('d') ):
		if ( pan_pulse > pan_min ):
			pan_pulse = pan_pulse - pan_delta
			servo_drv.set_pulse_t( pan_ch, pan_pulse )

	elif ( c == ord('w') ):
		if ( tilt_pulse < tilt_max ):
			tilt_pulse = tilt_pulse + tilt_delta
			servo_drv.set_pulse_t( tilt_ch, tilt_pulse )
			
	elif ( c == ord('s') ):
		if ( tilt_pulse > tilt_min ):
			tilt_pulse = tilt_pulse - tilt_delta
			servo_drv.set_pulse_t( tilt_ch, tilt_pulse )

	elif ( c == ord(' ') ):
		servo_drv.set_pulse_t( trig_ch, trig_on )
		time.sleep( 3 )
		servo_drv.set_pulse_t( trig_ch, trig_off )
		time.sleep( 1 )

	elif ( c == ord('q') ):
		break

curses.nocbreak()
curses.echo()
curses.endwin()

pi.stop()