QACO.py

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



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 = 5000 
        self.SINIR_CEZASI = 15000 

    # def _harita_olustur(self):
    #     N = self.BOYUT
    #     harita = np.zeros((N, N), dtype=int)

    #     if N == 10:
    #         harita[2:8, 4] = 1 
    #         harita[7, 2:4] = 1 
    #         harita[5, 7] = 1
    #         harita[6, 7] = 1
            
    #     harita[self.START] = 0
    #     harita[self.GOAL] = 0

    #     return harita

    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[25, 25:35] = 1 
        harita[15:35, 25] = 1 
        harita[20:30, 40] = 1
        harita[20, 20:30] = 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

    #     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):
        y, x = int(round(koordinat[0])), int(round(koordinat[1]))
        if not (0 <= y < self.BOYUT and 0 <= x < self.BOYUT):
            return self.SINIR_CEZASI
        if self.HARITA[y, x] == 1:
            return self.ENGEL_CEZASI 
        return 0


def get_neighbors(y, x, BOYUT):
    komşular = []
    for dy in [-1, 0, 1]:
        for dx in [-1, 0, 1]:
            if dy == 0 and dx == 0:
                continue
            ny, nx = y + dy, x + dx
            if 0 <= ny < BOYUT and 0 <= nx < BOYUT:
                komşular.append((ny, nx))
    return komşular

def heuristic(y, x, GOAL):
    uzaklik = math.sqrt((GOAL[0] - y)**2 + (GOAL[1] - x)**2)
    return 1.0 / (uzaklik + 1e-6) 


def qaco_pathfinding(grid_map, max_iter, karinca_sayisi, beta, delta_r):

    N = grid_map.BOYUT
    START = grid_map.START
    GOAL = grid_map.GOAL
    
    # Qubit Temsili: Açı Matrisi (Radyan)
    # Başlangıç: Pi/4 (cos^2(pi/4) = 0.5, yani %50 olasilik)
    qubit_angle_matris = np.full((N, N), np.pi / 4.0)

    en_iyi_yol = None
    en_iyi_maliyet = float('inf')
    maliyet_gecmisi = []

    for t in range(max_iter):
        
        yollar = []
        
        for k_idx in range(karinca_sayisi):
            
            karinca = {'pozisyon': START, 'yol': [START], 'uzunluk': 0.0, 'tikandi': False, 'ziyaret_edilenler': {START}}
            max_adimlar = N * N 
            # max_adimlar = N * N * 3
            adim_sayisi = 0
            
            while karinca['pozisyon'] != GOAL and not karinca['tikandi'] and adim_sayisi < max_adimlar:
                cy, cx = karinca['pozisyon']
                
                komşular = get_neighbors(cy, cx, N)
                
                geçerli_komşular = []
                for ny, nx in komşular:
                    if grid_map.HARITA[ny, nx] != 1 and (ny, nx) not in karinca['ziyaret_edilenler']: 
                        geçerli_komşular.append((ny, nx))
                        
                # if not geçerli_komşular:
                #     karinca['tikandi'] = True
                #     break

                if not geçerli_komşular:
                    # tabu'yu gevşet: sadece bir önceki düğüme dönmeyi yasakla
                    prev = karinca['yol'][-2] if len(karinca['yol']) >= 2 else None
                    komsular = get_neighbors(cy, cx, N)
                    gecici = [(ny, nx) for (ny, nx) in komsular
                            if grid_map.HARITA[ny, nx] != 1 and (prev is None or (ny, nx) != prev)]
                    if not gecici:
                        karinca['tikandi'] = True
                        break
                    geçerli_komşular = gecici

                olasiliklar_listesi = []
                toplam_payda = 0.0
                
                for ny, nx in geçerli_komşular:
                    # olasilik: cos^2(Theta)
                    prob_qubit = np.cos(qubit_angle_matris[ny, nx])**2
                    eta = heuristic(ny, nx, GOAL)**beta
                    toplam_payda += prob_qubit * eta
                
                if toplam_payda == 0: 
                     sonraki_düğüm = random.choice(geçerli_komşular)
                     olasiliklar_listesi = [1.0/len(geçerli_komşular)] * len(geçerli_komşular)
                else:
                    for ny, nx in geçerli_komşular:
                        prob_qubit = np.cos(qubit_angle_matris[ny, nx])**2
                        eta = heuristic(ny, nx, GOAL)**beta
                        olasilik = (prob_qubit * eta) / toplam_payda
                        olasiliklar_listesi.append(olasilik)
                    
                    olasiliklar_array = np.array(olasiliklar_listesi) / np.sum(olasiliklar_listesi)
                    
                    secilen_index = np.random.choice(len(geçerli_komşular), p=olasiliklar_array)
                    sonraki_düğüm = geçerli_komşular[secilen_index]


                ny, nx = sonraki_düğüm
                
                uzunluk_artisi = math.sqrt((ny - cy)**2 + (nx - cx)**2)
                karinca['uzunluk'] += uzunluk_artisi

                karinca['pozisyon'] = sonraki_düğüm
                karinca['yol'].append(sonraki_düğüm)
                karinca['ziyaret_edilenler'].add(sonraki_düğüm)
                adim_sayisi += 1
            
            if karinca['pozisyon'] == GOAL:
                yollar.append(karinca)
        
        
        hedefe_ulaşanlar = [k for k in yollar if k['pozisyon'] == GOAL]
        
        if hedefe_ulaşanlar:
            
            mevcut_en_iyi_yol_k = min(hedefe_ulaşanlar, key=lambda k: k['uzunluk'])
            
            if mevcut_en_iyi_yol_k['uzunluk'] < en_iyi_maliyet:
                en_iyi_maliyet = mevcut_en_iyi_yol_k['uzunluk']
                en_iyi_yol = mevcut_en_iyi_yol_k['yol']
            
            if en_iyi_yol:
                for y, x in en_iyi_yol:
                    qubit_angle_matris[y, x] -= delta_r 
                    
                qubit_angle_matris = np.clip(qubit_angle_matris, 0.0, np.pi / 2.0)

        
        maliyet_gecmisi.append(en_iyi_maliyet)
        
        if t == max_iter - 1: #t % 5 == 0 or 
            print(f"İterasyon: {t+1}/{max_iter}, En İyi Maliyet: {en_iyi_maliyet:.2f}")
            harita_gorsellestir_qaco(grid_map, en_iyi_yol, t, max_iter)

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



    return en_iyi_yol, en_iyi_maliyet, maliyet_gecmisi


if __name__ == "__main__":
    HARITA_BOYUTU = 50
    BASLANGIC = (0, 0)
    HEDEF = (49, 49)
    grid_map = GridMap(HARITA_BOYUTU, BASLANGIC, HEDEF)
    
    # QACO PARAMETRELERI
    MAX_ITER_QACO = 150   
    KARINCA_SAYISI = 30 # 50    
    BETA = 2.5 # 3.0             
    DELTA_R = 0.04 # 0.02         
    
    print(f"Harita Boyutu: {HARITA_BOYUTU}x{HARITA_BOYUTU}")
    
    
    en_iyi_rota_qaco, en_iyi_maliyet_qaco, maliyet_gecmisi_qaco = qaco_pathfinding(
        grid_map, MAX_ITER_QACO, KARINCA_SAYISI, BETA, DELTA_R
    )

    # --- METRİK GRAFİĞİ (Tüm geçmiş) ---
    plt.figure(figsize=(10, 5))
    plt.plot(maliyet_gecmisi_qaco, label='En İyi Rota Maliyeti', color='red')
    plt.title('QACO Optimizasyon Performansı (Maliyet vs. İterasyon)')
    plt.xlabel('İterasyon Sayısı')
    plt.ylabel('Rota Uzunluğu (Maliyet)')
    plt.legend()
    plt.grid(True)
    # plt.show()
    plt.savefig("QACO_2.png", dpi=300)

    print("\n" + "="*50)
    print("QACO YOL BULMA SONUÇ ÖZETİ (10x10)")
    print("="*50)
    print(f"Bulunan En İyi Rota Uzunluğu (Maliyet): {en_iyi_maliyet_qaco:.2f}")
    
    if en_iyi_rota_qaco and en_iyi_maliyet_qaco != float('inf'):
        print(f"Rota Adım Sayısı: {len(en_iyi_rota_qaco)}")
        harita_gorsellestir_qaco(grid_map, en_iyi_rota_qaco, MAX_ITER_QACO - 1, MAX_ITER_QACO) # Son halini çiz
    else:
        print("Yol bulunamadı. Parametreleri kontrol edin.")