# SPDX-FileCopyrightText: 2021 jedgarpark for Adafruit Industries
# SPDX-License-Identifier: MIT

# Pico servo demo
# Hardware setup:
#   Servo on GP0 with external 5V power supply
#   Button on GP3 and ground

import time
import board
from digitalio import DigitalInOut, Direction, Pull
import pwmio
from adafruit_motor import servo

import os
import sys
import ssl
import wifi
import socketpool
import microcontroller
import board
import busio
import adafruit_requests
import adafruit_dht
from adafruit_io.adafruit_io import IO_HTTP, AdafruitIO_RequestError
from microcontroller import cpu



print("Servo test")
mode = -1  # track state of button mode
    
def setupLED():    
    led = DigitalInOut(board.LED)
    led.direction = Direction.OUTPUT
    led.value = True
    return led

def setupButton():    
    # Mode button setup
    button = DigitalInOut(board.GP3)
    button.direction = Direction.INPUT
    button.pull = Pull.UP
    return button



def blink(led,count):
    for _ in range(count):
        led.value = False
        time.sleep(0.1)
        led.value = True
        time.sleep(0.1)

def setupServo():
    # Servo setup
    pwm_servo = pwmio.PWMOut(board.GP0, duty_cycle=2 ** 15, frequency=50)
    servo1 = servo.Servo(pwm_servo, min_pulse=500, max_pulse=2200)  # tune pulse for specific servo

    return servo1

def setServoAngle(servo1,angle):
    # Servo setup
    print("servo set angle =",angle)
    servo1.angle = angle
    servoData = angle
    data = [servoData]
    #  send sensor data to respective feeds
#            io.send_data(picowServo,servoData)
    io.send_data("myservo",angle)
    print("sent")

    return servo1

# Servo test
def servo_direct_test1():
    print("Servo Direct Test 1")
    setServoAngle(servo1,90)
    time.sleep(2)
    setServoAngle(servo1,0)
    time.sleep(2)
    setServoAngle(servo1,90)
    time.sleep(2)
    setServoAngle(servo1,180)
    time.sleep(2)


# Servo test
def servo_direct_test():
    print("Servo Direct Test")
    print("servo test: 90")
    servo1.angle = 90
    time.sleep(2)
    print("servo test: 0")
    servo1.angle = 0
    time.sleep(2)
    print("servo test: 90")
    servo1.angle = 90
    time.sleep(2)
    print("servo test: 180")
    servo1.angle = 180
    time.sleep(2)


# Servo smooth test
def servo_smooth_test():
    counter = 1
    print("servo smooth test: 180 - 0, -1º steps")
    for angle in range(180, 0, -1):  # 180 - 0 degrees, -1º at a time.
        servo1.angle = angle
        
        if(counter > 10):	# This is used to rate limit the data to Adafruit
            servoData = angle
            data = [servoData]
            #  send sensor data to respective feeds
            #            io.send_data(picowServo,servoData)
            io.send_data("myservo",angle)
            print("sent ", angle)
            counter = 1
        else:
            counter += 1
        
        time.sleep(0.1)
#        time.sleep(0.01)
    time.sleep(1)
    print("servo smooth test: 0 - 180, 1º steps")
    for angle in range(0, 180, 1):  # 0 - 180 degrees, 1º at a time.
        servo1.angle = angle
        time.sleep(0.01)
    time.sleep(1)


def run_test(testnum):
    if testnum is 0:
        print("servo direct test")
        servo_direct_test1()
    elif testnum is 1:
        print("servo direct test")
        servo_smooth_test()

led    = setupLED()
button = setupButton()
servo1 = setupServo()
print("Type of servo = ",type(servo))

wifi.radio.connect(os.getenv('CIRCUITPY_WIFI_SSID'), os.getenv('CIRCUITPY_WIFI_PASSWORD'))

aio_username = os.getenv('ADAFRUIT_IO_USERNAME')
aio_key = os.getenv('ADAFRUIT_IO_KEY')

pool = socketpool.SocketPool(wifi.radio)
requests = adafruit_requests.Session(pool, ssl.create_default_context())
# Initialize an Adafruit IO HTTP API object
io = IO_HTTP(aio_username, aio_key, requests)
print("connected to io")


try:
# get feed
    picowServo = io.get_feed("myservo")
except AdafruitIO_RequestError:
# if no feed exists, create one
    print("Feed error")
    sys.exit(-1)

#  pack feed names into an array for the loop
feed_names = [picowServo]
print("Connected to feed.")


while True:
    if not button.value:
        blink(led,2)
        mode = (mode + 1) % 2
        print("switch to mode %d" % (mode))
        time.sleep(0.8)  # big debounce
        run_test(mode)