# rightSensor and broker file

#Preliminary code for an outer pi, definitely not finished just an idea

print("hello")

import time
import board
import digitalio
import os
from gpiozero import Servo

from paho.mqtt import client as mqtt
from paho.mqtt import publish
from paho.mqtt import subscribe


IR = digitalio.DigitalInOut(board.D26)
IR.direction = digitalio.Direction.INPUT
 
servo = Servo(16)
 
ThermalCamOff = 1  #temp variable at the moment
 
servoPos = 0


broker = '192.168.1.15'
port = 1883
topic = "peripheralSensor/rightSensor"
clientID = "rightPumpkin"
 
print("hello")

def connect_mqtt():
    def on_connect(client, userdata, flags, rc):
        if rc==0:
            print("connected to MQTT Broker!")
        else:
            print("failed to connect")
    client = mqtt.Client(clied=nt_id)
    client.on_connect = on_connect
    client.connect(broker, port)
    return client


def publish(client, msg)
    result = client.publish(topic, msg)
    status = result[0]
    if status == 0:
        print(f"sent `{msg}` to topic `{topic}`")
    else:
        print(f"failed to send message to topic {topic}")
        

def run():
    client = connect_mqtt()
    client.loop_start()
    #sensorCodeFunction here
    
if __name__ == '__main__':
    run()

#os.system("your terminal command here")
 

 
while(True):
    
    # here should check status of the thermal camera, assign to ThermalCamState
    
    
    if(ThermalCamState==0):
        if(IR.value==1):
            #publish left IR trigger, need to change localhost when we get the broker set up.
            #os.system("mosquitto_pub -h localhost -t 'mqtt/leftsensortrig' -m '1'")
 
            client.publish("rightSensorTrig", "1")
 
            # colbi says bigger val = more counter clockwise, -1 to 1 range
            servo.value=-1
 
        time.sleep(0.8) #however long the length of time of the IR trigger is
 
    else: #if thermal cam is on
        
        # here should check status of servo position from mid pumpkin, assign to servo pos
        
        servo.value = servoPos