QPSO.py

import numpy as np
import random
import math
import matplotlib.pyplot as plt
from vis_tools import harita_gorsellestir


class GridMap:
    def __init__(self, boyut, start, goal):
        self.BOYUT = boyut
        self.START = start
        self.GOAL = goal
        self.HARITA = self._harita_olustur()
        self.ENGEL_CEZASI = 10000 # 3000
        self.SINIR_CEZASI = 20000 # 5000


    def _harita_olustur(self):

        harita = np.zeros((self.BOYUT, self.BOYUT), dtype=int)

        harita[100, 50:151] = 1 
        harita[50:151, 100] = 1 
        harita[70:171, 150] = 1
        harita[30, 30:121] = 1 

        # harita[1, 1:4] = 1 # (1,1) -> (1,3)
        # harita[2:4, 5] = 1 # (2,5) -> (3,5)
        # harita[4:7, 3] = 1 # (4,3) -> (6,3)
        # harita[6, 1:8] = 1 # (6,1) -> (6,7)

        # engel_sayisi = int(self.BOYUT * self.BOYUT * 0.035)

        # for _ in range(engel_sayisi):
        #     x = np.random.randint(0, self.BOYUT)
        #     y = np.random.randint(0, self.BOYUT)
        #     harita[x, y] = 1

        return harita
    
    # def _harita_olustur(self):
        N = self.BOYUT  # 200 bekliyoruz ama genellenmiş halde
        harita = np.zeros((N, N), dtype=int)

        # ---- Yardımcılar ----
        def hline(y, x1, x2, t=1):
            x1, x2 = max(0, min(x1, x2)), min(N-1, max(x1, x2))
            y0, y1 = max(0, y - (t//2)), min(N-1, y + (t-1)//2)
            harita[y0:y1+1, x1:x2+1] = 1

        def vline(x, y1, y2, t=1):
            y1, y2 = max(0, min(y1, y2)), min(N-1, max(y1, y2))
            x0, x1 = max(0, x - (t//2)), min(N-1, x + (t-1)//2)
            harita[y1:y2+1, x0:x1+1] = 1

        def gate_y(y, xs, w=3):
            # yatay duvarda kapı: xs tek bir x ya da liste olabilir
            if isinstance(xs, (list, tuple, np.ndarray)):
                for x in xs:
                    harita[max(0,y-1):min(N,y+2), max(0,x-w):min(N,x+w+1)] = 0
            else:
                x = xs
                harita[max(0,y-1):min(N,y+2), max(0,x-w):min(N,x+w+1)] = 0

        def gate_x(x, ys, w=3):
            # dikey duvarda kapı: ys tek bir y ya da liste olabilir
            if isinstance(ys, (list, tuple, np.ndarray)):
                for y in ys:
                    harita[max(0,y-w):min(N,y+w+1), max(0,x-1):min(N,x+2)] = 0
            else:
                y = ys
                harita[max(0,y-w):min(N,y+w+1), max(0,x-1):min(N,x+2)] = 0

        # ---- Dikey omurga duvarları (farklı kapılarla) ----
        v_cols = [40, 100, 150]
        for k, xc in enumerate(v_cols):
            vline(xc, 10, N-10, t=2)
        # Kapılar (S-benzeri rota için ofsetli)
        gate_x(40,  [25, 90, 160], w=3)
        gate_x(100, [50, 120, 185], w=3)
        gate_x(150, [35, 80, 145, 190], w=3)

        # ---- Yatay kesen duvarlar ----
        h_rows = [30, 75, 120, 170]
        for r in h_rows:
            hline(r, 10, N-10, t=2)
        gate_y(30,  [28, 85, 135, 178], w=3)
        gate_y(75,  [22, 60, 112, 165], w=3)
        gate_y(120, [18, 55, 108, 152, 188], w=3)
        gate_y(170, [26, 72, 128, 172], w=3)

        # ---- Eğik bariyer (meandring) + aralıklı boşluklar ----
        # y ≈ x + 40 bandı içinde 2 kalınlıkla; her 30 hücrede bir küçük geçit
        offset = 40
        thickness = 2
        for x in range(15, N-15):
            y = x + offset
            if 10 <= y < N-10:
                harita[max(0,y-thickness):min(N,y+thickness+1), max(0,x-1):min(N,x+2)] = 1
        # Geçitler
        for x in range(25, N-25, 30):
            y = x + offset
            harita[max(0,y-3):min(N,y+4), max(0,x-2):min(N,x+3)] = 0

        # ---- Başlangıç ve hedef etrafını güvene al (spawn/goal clearance) ----
        harita[0:4, 0:4] = 0
        harita[N-4:N, N-4:N] = 0

        # ---- Garantili S-koridor (temiz şerit): start→(20,20)→(N-20,20)→(N-20,N-20)→goal ----
        # Koridor genişliği 2-3 hücre: duvarları kesiyor ama dar boğazları korur.
        def carve_corridor(p1, p2, w=2):
            (y1, x1), (y2, x2) = p1, p2
            if y1 == y2:  # yatay
                y = y1
                xs = range(min(x1,x2), max(x1,x2)+1)
                harita[max(0,y-w):min(N,y+w+1), min(xs):max(xs)+1] = 0
            elif x1 == x2:  # dikey
                x = x1
                ys = range(min(y1,y2), max(y1,y2)+1)
                harita[min(ys):max(ys)+1, max(0,x-w):min(N,x+w+1)] = 0
            else:
                # Manhattan kır: iki parçada
                carve_corridor((y1,x1), (y2,x1), w)
                carve_corridor((y2,x1), (y2,x2), w)

        carve_corridor((0,0), (20,20), w=2)
        carve_corridor((20,20), (N-20,20), w=2)
        carve_corridor((N-20,20), (N-20,N-20), w=2)
        carve_corridor((N-20,N-20), (N-1,N-1), w=2)

        # Başlangıç/hedef hücresini kesin temizle
        harita[0,0] = 0
        harita[N-1,N-1] = 0

        return harita


    def kontrol_et_engel (self, koordinat):

        x = int(round(koordinat[0]))
        y = int(round(koordinat[1]))


        if not (0 <= x < self.BOYUT and 0 <= y < self.BOYUT):
            return self.SINIR_CEZASI
        
        if self.HARITA[x, y] == 1:
            return self.ENGEL_CEZASI 
        
        return 0
    

def fitness(rota_noktalari, grid_map):
    toplam_uzunluk = 0.0
    toplam_ceza = 0.0

    #Rota: Baslangic -> Ara noktalar -> Hedef
    tam_rota = [grid_map.START] + rota_noktalari + [grid_map.GOAL]

    for i in range(len(tam_rota)-1):
        p1 = tam_rota[i]
        p2 = tam_rota[i+1]

        uzunluk = math.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)
        toplam_uzunluk += uzunluk

        if uzunluk < 1e-6: # Çok küçük bir eşik değeri kontrolü
            continue

        # toplam_ceza += grid_map.kontrol_et_engel(p2)
        ADIM_SAYISI = max(2, int(uzunluk * 12))

        for j in range(ADIM_SAYISI):
            t = j / (ADIM_SAYISI - 1)

            x_interp = p1[0] * (1 - t) + p2[0] * t
            y_interp = p1[1] * (1 - t) + p2[1] * t

            interp_nokta = (x_interp, y_interp)

            ceza_miktari = grid_map.kontrol_et_engel(interp_nokta)

            if ceza_miktari > 0:
                # Segmentin tamamı için ceza ekle, böylece PSO bu rotadan hızla uzaklaşır.
                toplam_ceza += ceza_miktari + (uzunluk * 10) # Uzunluk tabanlı ek ceza
                break


    return toplam_uzunluk + toplam_ceza




def qpso_pathfinding(grid_map, max_iter, suru_buyuklugu, ara_nokta_sayisi, alfa, gorsellestir=True):

    boyut = 2 * ara_nokta_sayisi

    #Baslangic Konumlari X
    X = np.random.uniform(low=0, high=(grid_map.BOYUT -1), size=(suru_buyuklugu, boyut))

    pbest_konumlari = X.copy()
    pbest_degerleri = np.array([fitness(X[i].reshape(-1,2).tolist(), grid_map) for i in range(suru_buyuklugu)])

    # print("pbest degerleri: ", pbest_degerleri)

    gbest_index = np.argmin(pbest_degerleri)
    gbest_deger = pbest_degerleri[gbest_index]
    gbest_konum = pbest_konumlari[gbest_index].copy()

    # print("gbest_konum: ", gbest_konum)

    #performans metrikleri icin:
    en_iyi_maliyet_gecmisi = []
    ortalama_maliyet_gecmisi = []

    # 2. ana optimizasyon döngüsü
    for t in range(max_iter):

        C = np.mean(pbest_konumlari, axis=0) # Sürünün pbest merkez noktası

        mevcut_maliyetler = []

        for i in range(suru_buyuklugu):

            #Hiz ve konum guncellemesi
            #yerel cekim noktasi hesapla
            r_phi = random.random()
            m_best_i = r_phi * pbest_konumlari[i] + (1 - r_phi) * gbest_konum

            # C. yeni konum hesapla
            u = random.random()
            r_sign = np.sign(random.random() - 0.5)
            M_distance = np.abs(C - X[i])




            X_yeni_float = m_best_i + r_sign * alfa * M_distance * np.log(1/u)
            X_yeni_int = np.round(X_yeni_float).astype(int)
            X[i] = np.clip(X_yeni_int, 0, grid_map.BOYUT -1 )

            
            
            #fitness ve pbest/gbest guncellemesi
            mevcut_uygunluk = fitness(X[i].reshape(-1,2).tolist(), grid_map)
            mevcut_maliyetler.append(mevcut_uygunluk)

            if(mevcut_uygunluk < pbest_degerleri[i]):
                pbest_degerleri[i] = mevcut_uygunluk
                pbest_konumlari[i] = X[i].copy()

                if(mevcut_uygunluk < gbest_deger):
                    gbest_deger = mevcut_uygunluk
                    gbest_konum = X[i].copy()
            
            mevcut_maliyetler.append(mevcut_uygunluk)
        
        en_iyi_maliyet_gecmisi.append(gbest_deger)
        ortalama_maliyet_gecmisi.append(np.mean(mevcut_maliyetler))

        if gorsellestir and (t == max_iter - 1): #t % 10 == 0 or  
            harita_gorsellestir(grid_map, X, gbest_konum.reshape(-1, 2).tolist(), t, "QPSO")

        if(t % 10 == 0):
            print("iter: ", t, " maliyet: ", gbest_deger)

    return gbest_konum.reshape(-1,2).tolist(), gbest_deger, en_iyi_maliyet_gecmisi, ortalama_maliyet_gecmisi



if __name__ == "__main__":
    print("selam")
    HARITA_BOYUTU = 200
    BASLANGIC = (0, 0)
    HEDEF = (199,199)
    ARA_NOKTA_SAYISI = 26 #30
    grid_map = GridMap(HARITA_BOYUTU, BASLANGIC, HEDEF)

    #PSO PARAMETRELERI
    MAX_ITER = 200
    SURU_BUYUKLUGU = 150
    alfa = 1.0


    en_iyi_rota, en_iyi_maliyet, en_iyi_maliyet_gecmisi, ortalama_maliyet_gecmisi = qpso_pathfinding(grid_map, MAX_ITER, SURU_BUYUKLUGU, ARA_NOKTA_SAYISI, alfa, gorsellestir=True)
    # en_iyi_rota, en_iyi_maliyet, en_iyi_maliyet_gecmisi, ortalama_maliyet_gecmisi = pso_pathfinding(grid_map, MAX_ITER, SURU_BUYUKLUGU, ARA_NOKTA_SAYISI, W, C1, C2, gorsellestir=True)

    # --- METRİK GRAFİĞİ ---
    plt.figure(figsize=(10, 5))
    plt.plot(en_iyi_maliyet_gecmisi, label='En İyi Rota Maliyeti (Gbest)', color='red')
    plt.plot(ortalama_maliyet_gecmisi, label='Ortalama Rota Maliyeti', color='blue', linestyle='--')
    plt.title('PSO Optimizasyon Performansı (Maliyet vs. İterasyon)')
    plt.xlabel('İterasyon Sayısı')
    plt.ylabel('Maliyet (Uzunluk + Ceza)')
    plt.legend()
    plt.grid(True)
    plt.show()

    print("\n----------------------------------------------------------------")
    print("PSO Yol Bulma Sonuç Özeti:")
    print(f"Harita Boyutu: {grid_map.BOYUT}x{grid_map.BOYUT}")
    print(f"Toplam İterasyon: {MAX_ITER}")
    print(f"Bulunan En İyi Rota Maliyeti: {en_iyi_maliyet:.2f}")

    # Eğer ceza çok yüksek değilse, yol bulunmuştur.
    if en_iyi_maliyet < grid_map.SINIR_CEZASI:
        print("Sonuç: Başarıyla Engelden Kaçınılmıştır.")
    else:
        print("Sonuç: Yol bulunamadı veya bir engele/sınıra çarpma cezası yüksek.")
    print("----------------------------------------------------------------")