from machine import Pin, time_pulse_us, PWM, I2C
from pico_i2c_lcd import I2cLcd
import utime

#ULTRASONIC DISTANCE SENSOR
trig = Pin(21, Pin.OUT)  #sets GP21 as output to send trigger signal
echo = Pin(20, Pin.IN)  #sets GP20 as input to read echo signal

#LEDS (1) (warning)
green_led = Pin(27, Pin.OUT) #sets GP27 as output for green led 
red_led = Pin(26, Pin.OUT)  #sets GP26 as output for red led

#LEDS (2) (reload)

green_led2 = Pin(0, Pin.OUT)   #sets GP0 as output for green led2 
orange_led2 = Pin(1, Pin.OUT)   #sets GP1 as output for orange led2
red_led2 = Pin(2, Pin.OUT)  #sets GP2 as output for red led2 

#BUZZER
buzzer = Pin(12, Pin.OUT) #sets GP10 as output for buzzer

#BUTTON (1) SHOOTING
shoot_btn = Pin(14, Pin.IN, Pin.PULL_UP)    #sets GP14 as output for shooting button

#SERVO MOTOR (1)
servo1 = PWM(Pin(22))    #servo1 signal settled at GP22 (PWM-pulse width moderation)
servo1.freq(50)  #sets a proper signal timing so that servo1 moves accurately

#SERVO MOTOR (2)
servo2 = PWM(Pin(3))    #servo2 signal settled at GP22 (PWM-pulse width moderation)
servo2.freq(50)     #sets a proper signal timing so that servo1 moves accurately

#LCD1 16x2 I2C (1 warning)
i2c1 = I2C(0, scl=Pin(9), sda=Pin(8), freq=400000)  #sets up communication using I2C protocol using channel 0
I2C_ADDR1 = 0x27    #LCD1 adress so that microcontroller knows where to send commands
lcd1 = I2cLcd(i2c1, I2C_ADDR1, 2, 16)  #connects code to LCD1 screen with screen parameters

#LCD2 16x2 I2C (2 reload)

i2c2 = I2C(1, scl=Pin(11), sda=Pin(10), freq=400000)  #sets up communication using I2C protocol using channel 1
I2C_ADDR2 = 0x27
lcd2 = I2cLcd(i2c2, I2C_ADDR2, 2, 16)    #connects code to LCD2 screen with screen parameters

#VARIABLES FOR RELOAD SYSTEM
MAX_SHELLS = 5  #sets maximum amount of shells available for firing 
RELOAD_MS = 5000    #reload time (5000ms = 5sec)  

shells = MAX_SHELLS     #this will track the current number of shells available
reloading = False       #indicates whether the system reloading
reload_deadline = 0     #will store the time when the reload process will be complete

def measure_distance(): #defines code for measuring and returning distance
    trig.value(0)   #keeps trigger low to start clean
    utime.sleep_us(1)   #delay for stability
    trig.value(1)   #sends pulse to start measurment
    utime.sleep_us(10)  #ultrasonic sensor requires 10µs pulse to recognize a signal
    trig.value(0)   #to send an ultrasonic wave

    duration = time_pulse_us(echo, 1, 30000) #measures the travel time of the sound wave to calculate distance 

    distance = (duration * 0.0343)/2 #to calculate distance (duraition in microseconds)(CONVERT sound speed 343 m/s)
                                     #division by 2 gives us one-way distance
    return distance  #gives back the distance value

def set_servo1_angle(servo1, angle1):     #servo1 position in degrees
    duty = 1638 + int((angle1 / 180) * 7864) #servo motor doesn't understand angle as they understand pulse widths
    #Pi Pico is a 16-bit PWM that has range from 0 to 65535. 
    #1638 corresponds to 1 ms pulse that equals to 0 degrees.
    #9502 is max value where 2ms pulse will be equal to 180 degrees.
    #our formula converts angle into PWM signal
    servo1.duty_u16(duty) #u16 stays for unsigned 16-bit (using from 0 to 65535 range to set PWM duty cycle)

def set_servo2_angle(servo2, angle2):
    duty = 1638 + int((angle2 / 180) * 7864)
    servo2.duty_u16(duty)

def now_ms():   
    return utime.ticks_ms()     #returns the current time in miliseconds

def start_reload():     #starts the reloading sequence
    global reloading, reload_deadline   #to declare that I am using global variables t0 ensure system won't cound them as local 
    reloading = True        #this will mark that reloading has started 
    reload_deadline = utime.ticks_add(now_ms(), RELOAD_MS)      #this sets when the reloading should finish

def stop_reload():  #ends reloading by saying that it is False
    global reloading
    reloading = False

def reload_remaining_ms():  #how many milliseconds left until reloading is done
    if not reloading:   #checks if the reloaing is False
        return 0        #if not reloading returns 0 meaning there is reloading time left
    remaining = utime.ticks_diff(reload_deadline, now_ms())   #calculates difference between the current time to prevent wrong millisecond counter
    return remaining if remaining > 0 else 0    #gives back the remaining reload time if it's positive and if not it returns to 0
    #if reload time passed - doesn't return negative value

def update_reload_leds2():   #turns leds on and off based on the reloading system response
    remaining = reload_remaining_ms()

    if shells == 0:            #if no shells red led on, orange and green leds off
        red_led2.value(1)
        orange_led2.value(1)
        green_led2.value(0)
    elif remaining > 0:     #if half way done orange led turns on, red and green leds off
        print("ORAAAAAAAAAAAAAAAAAANGE") 
        red_led2.value(0)
        orange_led2.value(1)
        green_led2.value(0)
    elif remaining > (RELOAD_MS / 2):     #if reload time more then half, red led on, orange and green led off
        red_led2.value(1)
        orange_led2.value(0)
        green_led2.value(0)
    else:                   #if remaining == 0, green led on, red and orange leds off
        red_led2.value(0)
        orange_led2.value(0)
        green_led2.value(1)

def update_servo2_reload():     #moves servo2 to represent progress
    remaining = reload_remaining_ms()      #gets the remaining time in miliseconds
    if remaining > 0:       #checks if there is still time for reload
        angle2 = (remaining / RELOAD_MS)*180        #calculates the servo angle based on remaining time
    else:
        angle2 = 0  #if no time left angle2=0
    if angle2 < 0:  #makes sure angle2 is not negative
        angle2 = 0  #sets angle2 to 0 if returns negative value
    if angle2 > 180:    #limits the angle to 180degrees   
        angle = 180     #will set angle to 180 degrees if exceeds
    set_servo2_angle(servo2, angle2)    #will move the servo to the calculated angle

def update_lcd2():  #updates the screen based on shooting availability
    lcd2.clear()    #cleans the screen
    remaining = reload_remaining_ms()

    if reloading and remaining > 0:
        line1_lcd2 = "OUT OF SHELLS!"
        line2_lcd2 = "Reload: {:.1f}s".format(remaining / 1000)
    else:
        line1_lcd2 = "Shells: {}/{}".format(shells, MAX_SHELLS)
        if shells > 0:
            line2_lcd2 = "READY!"
        else:
            line2_lcd2 = "OUT OF SHELLS"
    lcd2.move_to((16-len(line1_lcd2))//2, 0)    
    lcd2.putstr(line1_lcd2)  
    lcd2.move_to((16-len(line2_lcd2))//2, 1)    
    lcd2.putstr(line2_lcd2)  

last_shoot_state = 1

#MAIN LOOP
while True: 
    dist = measure_distance()  #to get current distance 
    #SCALING DISTANCE FOR SERVO 1
    dist_for_servo1 = max(0, min(dist, 350))  # limit only for servo1
    angle1 = (dist_for_servo1 / 350) * 70     #designed formula to give us angle1 scaling based on the distance
    set_servo1_angle(servo1, angle1)  #moves the servo1 to the specific angle
   
    print("Distance: ", round(dist, 2), "cm")   #prints and rounds the distance measured by ssensor
    print("Mortar angle: ", round(angle1, 0),"°")    #prints and rounds the angle1 of the imaginery mortar
    utime.sleep(0.05)  #delay between the readings
    
    shoot_state = shoot_btn.value()
    now = now_ms()

    if shoot_state == 0 and last_shoot_state == 1:
        now = now_ms()
        if shells > 0 and reload_remaining_ms() == 0:
            shells -= 1
            print("BOOM! Shells left:", shells)
        else:
            print("Can't shoot!")

    if shells == 0 and not reloading:
        start_reload()
    
    if reloading:
        remaining = reload_remaining_ms()
        if remaining == 0:
            stop_reload()
            shells = MAX_SHELLS
            print("Reload complete!")
    
    update_reload_leds2()
    update_servo2_reload()
    update_lcd2()
    
    if dist > 350: 
    #LEDS warning
        green_led.value(1)  #turns on green led
        red_led.value(0)    #turns off red led
    #BUZZER
        buzzer.value(0)     #turns 0ff buzzer
    #SERVO MOTOR 1
        set_servo1_angle(servo1, 70) #sets servo1 angle at maximum for our imaginary mortar
    #LCD1
        lcd1.clear()
        line1_lcd1 = ("Zone clear :)") #text for line1
        pad1_lcd1 = ((16-len(line1_lcd1))//2) #takes ammount of symbols I have in my text and centeres it at the screen
        lcd1.move_to(pad1_lcd1,0)   #moves line1Y to the amount of the flooe division
        lcd1.putstr(line1_lcd1)     #displays text for line 1
    else:
    #LEDS (warning)
        green_led.value(0)  #turns off green led
        red_led.value(1)    #turns on red led
        buzzer.value(1)     #turns on buzzer
        utime.sleep(0.05)   #pause
        red_led.value(0)    #turns off red led
    #BUZZER
        buzzer.value(0)     #turns off buzzer
        utime.sleep(0.05)   #pause
    #LCD1
        lcd1.clear() #clears lcd1 screen
        line1_lcd1 = "Warning!"  #text for my first line
        line2_lcd1 = "TARGET IN RANGE"  #text for my second line
        pad1_lcd1 = ((16-len(line1_lcd1))//2)   #takes ammount of symbols I have in my text and centeres it at the screen
        pad2_lcd1 = ((16-len(line2_lcd1))//2)   #does same for line 2
        lcd1.move_to(pad1_lcd1, 0)   #moves line1_lcd1 text to the ammount of the floor division
        lcd1.putstr(line1_lcd1)  #displays text for line1_lcd1
        lcd1.move_to(pad2_lcd1, 1)   #moves line2 text to the ammount of the floor division
        lcd1.putstr(line2_lcd1)  #displays text for line2_lcd1   
    
    utime.sleep(0.1)    #pause before the next reading