#include <cmath>
#include <numbers>

#include "fiko/kinematik/mekanizma.hpp"
#include "fiko_test.hpp"

using namespace fiko;
using namespace fiko::kinematik;

namespace {
constexpr f64 pi = std::numbers::pi;
Donusumd capaKonum(f64 x, f64 y, f64 z) {
  Donusumd d;
  d.konum = {x, y, z};
  return d;
}
}  // namespace

FIKO_TEST(kinematik_serbestlik_tipleri) {
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Sabit) == 0);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Doner) == 1);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Kayar) == 1);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Vida) == 1);   // öteleme dönmeye bağlı
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Silindirik) == 2);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Kuresel) == 3);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Duzlemsel) == 3);
  FIKO_KONTROL(mafsalSerbestlik(MafsalTip::Serbest) == 6);
}

FIKO_TEST(kinematik_grubler_dof) {
  // Tek döner mafsallı sarkaç: n=2, j=1, Σf=1 → 6·(2−1−1)+1 = 1
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto kol = m.uzuvEkle("Kol");
  m.mafsalEkle({.ad = "M1", .tip = MafsalTip::Doner, .ana = 0, .cocuk = kol});
  FIKO_KONTROL(m.serbestlikDerecesi() == 1);

  // Sabit mafsal → 0 DOF (kaynaklanmış yapı): 6·(2−1−1)+0 = 0
  Mekanizma s;
  s.uzuvEkle("Gövde");
  const auto p = s.uzuvEkle("Parça");
  s.mafsalEkle({.ad = "K", .tip = MafsalTip::Sabit, .ana = 0, .cocuk = p});
  FIKO_KONTROL(s.serbestlikDerecesi() == 0);

  // 4-çubuk (kapalı zincir, düzlemsel): n=4, j=4 döner → 6·(4−1−4)+4 = −2
  // (Uzaysal formül düzlemsel mekanizmayı AŞIRI KISITLI gösterir — beklenen; DOF
  //  işareti kullanıcıya "bu düzlemsel, uzaysal formül yanıltır" uyarısı için.)
  Mekanizma d;
  d.uzuvEkle("Gövde");
  const auto a = d.uzuvEkle("A");
  const auto b = d.uzuvEkle("B");
  const auto c = d.uzuvEkle("C");
  d.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = a});
  d.mafsalEkle({.tip = MafsalTip::Doner, .ana = a, .cocuk = b});
  d.mafsalEkle({.tip = MafsalTip::Doner, .ana = b, .cocuk = c});
  d.mafsalEkle({.tip = MafsalTip::Doner, .ana = c, .cocuk = 0});
  FIKO_KONTROL(d.serbestlikDerecesi() == -2);
}

FIKO_TEST(kinematik_agac_ve_kapali_zincir_ayrimi) {
  Mekanizma agac;
  agac.uzuvEkle("Gövde");
  const auto a = agac.uzuvEkle("A");
  const auto b = agac.uzuvEkle("B");
  agac.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = a});
  agac.mafsalEkle({.tip = MafsalTip::Doner, .ana = a, .cocuk = b});
  FIKO_KONTROL(agac.agacMi());

  Mekanizma kapali;
  kapali.uzuvEkle("Gövde");
  const auto x = kapali.uzuvEkle("X");
  kapali.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = x});
  kapali.mafsalEkle({.tip = MafsalTip::Doner, .ana = x, .cocuk = x});  // iki ana → döngü
  FIKO_KONTROL(!kapali.agacMi());

  Mekanizma kopuk;
  kopuk.uzuvEkle("Gövde");
  kopuk.uzuvEkle("Yetim");  // hiç mafsalı yok → gövdeden erişilemez
  FIKO_KONTROL(!kopuk.agacMi());
}

FIKO_TEST(kinematik_doner_mafsal_90_derece) {
  // Çapa orijinde, eksen +Z. Kol ucu çapadan +X'te 10 mm.
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto kol = m.uzuvEkle("Kol");
  m.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = kol, .capa = capaKonum(0, 0, 0),
                .q = pi / 2});
  const auto dunya = m.ileriKinematik();
  const Vec3d uc = dunya[kol].noktaUygula({10, 0, 0});
  FIKO_YAKIN(uc.x, 0.0, 1e-9);    // +X → +Y (90° CCW, Z ekseni)
  FIKO_YAKIN(uc.y, 10.0, 1e-9);
  FIKO_YAKIN(uc.z, 0.0, 1e-9);
}

FIKO_TEST(kinematik_kayar_mafsal_oteleme) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto slayt = m.uzuvEkle("Slayt");
  m.mafsalEkle({.tip = MafsalTip::Kayar, .ana = 0, .cocuk = slayt, .q = 25.0});
  const auto dunya = m.ileriKinematik();
  const Vec3d p = dunya[slayt].noktaUygula({0, 0, 0});
  FIKO_YAKIN(p.z, 25.0, 1e-9);   // eksen = çapa +Z
  FIKO_YAKIN(p.x, 0.0, 1e-9);
}

FIKO_TEST(kinematik_zincir_iki_mafsal_birikir) {
  // Gövde →(döner 90°, çapa orijin)→ Kol1 →(kayar 5, çapa +X 10)→ Kol2
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto k1 = m.uzuvEkle("Kol1");
  const auto k2 = m.uzuvEkle("Kol2");
  m.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = k1, .q = pi / 2});
  m.mafsalEkle({.tip = MafsalTip::Kayar, .ana = k1, .cocuk = k2, .capa = capaKonum(10, 0, 0),
                .q = 5.0});
  const auto dunya = m.ileriKinematik();
  const Vec3d p = dunya[k2].noktaUygula({0, 0, 0});
  // Kol1 90° döndü → çapa (10,0,0) dünyada (0,10,0); kayma +Z'de 5 (dönme Z'yi değiştirmez)
  FIKO_YAKIN(p.x, 0.0, 1e-9);
  FIKO_YAKIN(p.y, 10.0, 1e-9);
  FIKO_YAKIN(p.z, 5.0, 1e-9);
}

FIKO_TEST(kinematik_vida_oteleme_donmeye_kilitli) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto som = m.uzuvEkle("Somun");
  m.mafsalEkle({.tip = MafsalTip::Vida, .ana = 0, .cocuk = som, .q = 2.0 * pi,  // 1 tam tur
                .vidaAdim = 3.0});
  const auto dunya = m.ileriKinematik();
  const Vec3d p = dunya[som].noktaUygula({0, 0, 0});
  FIKO_YAKIN(p.z, 3.0, 1e-9);   // 1 tur = 1 adım (3 mm)
}

FIKO_TEST(kinematik_sinir_kirpma) {
  Mafsal m;
  m.tip = MafsalTip::Doner;
  FIKO_KONTROL(!m.sinirVar());                        // varsayılan: sınır YOK
  FIKO_YAKIN(Mekanizma::sinirKirp(m, 99.0), 99.0, 1e-12);
  m.altSinir = -1.0;
  m.ustSinir = 1.0;
  FIKO_KONTROL(m.sinirVar());
  FIKO_YAKIN(Mekanizma::sinirKirp(m, 5.0), 1.0, 1e-12);
  FIKO_YAKIN(Mekanizma::sinirKirp(m, -5.0), -1.0, 1e-12);
  FIKO_YAKIN(Mekanizma::sinirKirp(m, 0.4), 0.4, 1e-12);
  // İleri kinematik de kırpılmış değeri kullanmalı (tek kapı).
  Mekanizma mek;
  mek.uzuvEkle("Gövde");
  const auto k = mek.uzuvEkle("Kol");
  mek.mafsalEkle({.tip = MafsalTip::Kayar, .ana = 0, .cocuk = k, .q = 100.0,
                  .altSinir = 0.0, .ustSinir = 10.0});
  const auto d = mek.ileriKinematik();
  FIKO_YAKIN(d[k].noktaUygula({0, 0, 0}).z, 10.0, 1e-9);  // 100 değil, 10
}

FIKO_TEST(kinematik_bozuk_indeks_cokmez) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  m.mafsalEkle({.tip = MafsalTip::Doner, .ana = 7, .cocuk = 9});  // geçersiz indeksler
  FIKO_KONTROL(!m.agacMi());
  const auto d = m.ileriKinematik();   // çökmemeli
  FIKO_KONTROL(d.size() == 1);
}

// ── Sürüş yasaları (law / simülasyon zaman ekseni) ──────────────────────────
FIKO_TEST(kinematik_yasa_sabit_ve_dogrusal) {
  MafsalYasa s{.tip = YasaTip::Sabit, .deger = 4.0};
  FIKO_YAKIN(yasaDegeri(s, 0.0), 4.0, 1e-12);
  FIKO_YAKIN(yasaDegeri(s, 99.0), 4.0, 1e-12);   // zamandan bağımsız

  MafsalYasa d{.tip = YasaTip::Dogrusal, .deger = 10.0, .hiz = 2.5};
  FIKO_YAKIN(yasaDegeri(d, 0.0), 10.0, 1e-12);
  FIKO_YAKIN(yasaDegeri(d, 4.0), 20.0, 1e-12);   // 10 + 2.5·4
  FIKO_YAKIN(yasaDegeri(d, -2.0), 5.0, 1e-12);   // geriye de çalışır
}

FIKO_TEST(kinematik_yasa_harmonik) {
  MafsalYasa h{.tip = YasaTip::Harmonik, .deger = 1.0, .genlik = 3.0, .periyot = 4.0};
  FIKO_YAKIN(yasaDegeri(h, 0.0), 1.0, 1e-12);        // sin(0)=0
  FIKO_YAKIN(yasaDegeri(h, 1.0), 4.0, 1e-9);         // çeyrek periyot → +genlik
  FIKO_YAKIN(yasaDegeri(h, 3.0), -2.0, 1e-9);        // 3/4 periyot → −genlik
  FIKO_YAKIN(yasaDegeri(h, 4.0), 1.0, 1e-9);         // tam periyot → başa
  MafsalYasa sifir{.tip = YasaTip::Harmonik, .deger = 7.0, .genlik = 3.0, .periyot = 0.0};
  FIKO_YAKIN(yasaDegeri(sifir, 5.0), 7.0, 1e-12);    // periyot ≤0 → hareketsiz
}

FIKO_TEST(kinematik_yasa_anahtar_kare) {
  MafsalYasa a{.tip = YasaTip::AnahtarKare};
  a.anahtarlar = {{0.0, 0.0}, {2.0, 10.0}, {3.0, 10.0}, {5.0, -6.0}};
  FIKO_YAKIN(yasaDegeri(a, -1.0), 0.0, 1e-12);   // aralık ÖNCESİ → ilk değere kırp
  FIKO_YAKIN(yasaDegeri(a, 0.0), 0.0, 1e-12);
  FIKO_YAKIN(yasaDegeri(a, 1.0), 5.0, 1e-12);    // doğrusal ara-değer
  FIKO_YAKIN(yasaDegeri(a, 2.5), 10.0, 1e-12);   // düz bölüm
  FIKO_YAKIN(yasaDegeri(a, 4.0), 2.0, 1e-12);    // 10 → −6 arası yarı yol
  FIKO_YAKIN(yasaDegeri(a, 9.0), -6.0, 1e-12);   // aralık SONRASI → son değere kırp
  MafsalYasa bos{.tip = YasaTip::AnahtarKare, .deger = 3.0};
  FIKO_YAKIN(yasaDegeri(bos, 1.0), 3.0, 1e-12);  // anahtar yoksa taban değer
}

FIKO_TEST(kinematik_zamana_ayarla_mekanizmayi_surer) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto kol = m.uzuvEkle("Kol");
  const auto mf = m.mafsalEkle({.tip = MafsalTip::Kayar, .ana = 0, .cocuk = kol});
  m.yasaEkle({.mafsal = mf, .tip = YasaTip::Dogrusal, .deger = 0.0, .hiz = 10.0});
  m.zamanaAyarla(0.0);
  FIKO_YAKIN(m.ileriKinematik()[kol].noktaUygula({0, 0, 0}).z, 0.0, 1e-9);
  m.zamanaAyarla(2.5);
  FIKO_YAKIN(m.ileriKinematik()[kol].noktaUygula({0, 0, 0}).z, 25.0, 1e-9);
  m.zamanaAyarla(1.0);   // zamanda geri gitmek de çalışır (durum t'nin fonksiyonu)
  FIKO_YAKIN(m.ileriKinematik()[kol].noktaUygula({0, 0, 0}).z, 10.0, 1e-9);
}

FIKO_TEST(kinematik_yasa_sinira_kirpilir_ve_yasasiz_mafsal_korunur) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto a = m.uzuvEkle("A");
  const auto b = m.uzuvEkle("B");
  const auto m1 = m.mafsalEkle({.tip = MafsalTip::Kayar, .ana = 0, .cocuk = a,
                                .altSinir = 0.0, .ustSinir = 5.0});
  const auto m2 = m.mafsalEkle({.tip = MafsalTip::Kayar, .ana = a, .cocuk = b, .q = 7.0});
  m.yasaEkle({.mafsal = m1, .tip = YasaTip::Dogrusal, .hiz = 100.0});
  m.zamanaAyarla(1.0);   // yasa 100 der, sınır 5
  const auto d = m.ileriKinematik();
  FIKO_YAKIN(d[a].noktaUygula({0, 0, 0}).z, 5.0, 1e-9);       // sınıra kırpıldı
  FIKO_YAKIN(d[b].noktaUygula({0, 0, 0}).z, 12.0, 1e-9);      // 5 + 7 (yasasız mafsal korundu)
  FIKO_KONTROL(m.mafsal(m2)->q == 7.0);
  // Geçersiz mafsal indeksli yasa çökmemeli.
  m.yasaEkle({.mafsal = 99, .tip = YasaTip::Dogrusal, .hiz = 1.0});
  m.zamanaAyarla(2.0);
  FIKO_KONTROL(m.yasalar().size() == 2);
}

// ── Kapalı zincir (loop) çözücü ─────────────────────────────────────────────
namespace {
// Düzlemsel KAPALI ÜÇ-ÇUBUK zinciri: gövde(0) → A → B → C → gövde.
// Tüm mafsallar Z etrafında döner; çapa mesafeleri çubuk boylarıdır ve zincir
// ORİJİNE geri kapanır. NOT: `Mafsal` çapası yalnız ANA uzuv tarafındadır, çocuk
// tarafında ofset yoktur → kanonik 4-çubuğun "gövde uzunluğu" bu modelle ifade
// EDİLEMEZ (bilerek: Dalga 5 kapsamı tek-taraflı çapa). Bu yüzden kapanabilirlik
// ölçütü ÜÇGEN EŞİTSİZLİĞİdir: en uzun çubuk ≤ diğer ikisinin toplamı.
Mekanizma kapaliZincirUc(f64 a, f64 b, f64 c) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto A = m.uzuvEkle("Krank");
  const auto B = m.uzuvEkle("Biyel");
  const auto C = m.uzuvEkle("Sarkaç");
  Mafsal m1{.ad = "O1", .tip = MafsalTip::Doner, .ana = 0, .cocuk = A};
  Mafsal m2{.ad = "A", .tip = MafsalTip::Doner, .ana = A, .cocuk = B, .capa = capaKonum(a, 0, 0)};
  Mafsal m3{.ad = "B", .tip = MafsalTip::Doner, .ana = B, .cocuk = C, .capa = capaKonum(b, 0, 0)};
  // Halkayı kapatan: sarkacın ucu gövdedeki O2 noktasına (d,0,0) oturmalı.
  Mafsal m4{.ad = "O2", .tip = MafsalTip::Doner, .ana = C, .cocuk = 0, .capa = capaKonum(c, 0, 0)};
  m.mafsalEkle(m1); m.mafsalEkle(m2); m.mafsalEkle(m3); m.mafsalEkle(m4);
  return m;
}
}  // namespace

FIKO_TEST(kinematik_halka_kapatan_tespiti) {
  Mekanizma m = kapaliZincirUc(20, 60, 40);
  FIKO_KONTROL(!m.agacMi());                    // kapalı zincir
  const auto halka = m.halkaKapatanlar();
  FIKO_KONTROL(halka.size() == 1);              // 4 mafsaldan biri halkayı kapatır
  FIKO_KONTROL(halka[0] == 3);                  // son eklenen (gövdeye geri bağlanan)
  // Ağaç mekanizmada halka YOK.
  Mekanizma agac;
  agac.uzuvEkle("Gövde");
  const auto k = agac.uzuvEkle("Kol");
  agac.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = k});
  FIKO_KONTROL(agac.halkaKapatanlar().empty());
  f64 art = -1;
  FIKO_KONTROL(agac.kapaliZincirCoz({}, 10, 1e-9, &art));  // halka yok → anında true
  FIKO_YAKIN(art, 0.0, 1e-12);
}

FIKO_TEST(kinematik_dort_cubuk_halka_kapanir) {
  // Üçgen eşitsizliği sağlanıyor: 60 ≤ 20 + 40 (sınırda, düz konum) → kapanabilir.
  Mekanizma m = kapaliZincirUc(20, 60, 40);
  f64 art = 1e9;
  const bool ok = m.kapaliZincirCoz({0}, 200, 1e-6, &art);  // 0. mafsal SÜRÜLEN (krank)
  FIKO_KONTROL(ok);
  FIKO_KONTROL(art < 1e-6);
  // Halka gerçekten kapandı mı: sarkacın ucu gövde orijinine dönmeli.
  const auto d = m.ileriKinematik();
  const Vec3d uc = d[3].noktaUygula({40, 0, 0});   // C ucu (c=40)
  FIKO_YAKIN(uc.x, 0.0, 1e-5);
  FIKO_YAKIN(uc.y, 0.0, 1e-5);
  FIKO_YAKIN(uc.z, 0.0, 1e-5);
}

FIKO_TEST(kinematik_dort_cubuk_krank_donerken_kapali_kalir) {
  Mekanizma m = kapaliZincirUc(30, 60, 40);  // 60 < 30+40 → her açıda kapanır
  const f64 adimlar[] = {0.0, 0.6, 1.2, 2.0, 3.0, 4.5};
  for (f64 q : adimlar) {
    m.mafsal(0)->q = q;                    // krankı sür
    f64 art = 1e9;
    FIKO_KONTROL(m.kapaliZincirCoz({0}, 200, 1e-6, &art));
    const auto d = m.ileriKinematik();
    const Vec3d uc = d[3].noktaUygula({40, 0, 0});
    FIKO_KONTROL(std::hypot(uc.x, uc.y) < 1e-4);   // her açıda halka KAPALI
  }
}

FIKO_TEST(kinematik_cozulemez_halka_false_doner) {
  // Ulaşılamaz geometri: üçgen eşitsizliği İHLAL — 100 > 5 + 5 → zincir kapanamaz.
  Mekanizma m = kapaliZincirUc(5, 5, 100);
  f64 art = 0;
  FIKO_KONTROL(!m.kapaliZincirCoz({0}, 60, 1e-6, &art));  // yakınsamaz
  FIKO_KONTROL(art > 1e-3);                                // artık büyük kalır
}

// ── Çift taraflı çapa (montaj "mate") + GERÇEK 4-çubuk ──────────────────────
FIKO_TEST(kinematik_capa_cocuk_birim_eski_davranis) {
  // Birim çocuk çapası → dönüşüm ESKİ formülle birebir aynı olmalı (geri uyum).
  Mafsal m;
  m.tip = MafsalTip::Doner;
  m.capa = capaKonum(10, 0, 0);
  m.q = pi / 3;
  const Donusumd t = mafsalDonusumu(m);
  const Donusumd beklenen = m.capa * Donusumd{{0, 0, 0}, Quatd::eksenAci({0, 0, 1}, pi / 3), {1, 1, 1}};
  FIKO_YAKIN(t.konum.x, beklenen.konum.x, 1e-12);
  FIKO_YAKIN(t.konum.y, beklenen.konum.y, 1e-12);
  FIKO_YAKIN(t.yonelim.z, beklenen.yonelim.z, 1e-12);
}

FIKO_TEST(kinematik_capa_cocuk_ofseti_geri_iter) {
  // Çocuk çapası (5,0,0): çocuk uzvun mafsal noktası kendi yerelinde (5,0,0)'dadır →
  // çocuğun ORİJİNİ mafsal çerçevesinden 5 mm GERİDE oturur.
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto k = m.uzuvEkle("Kol");
  Mafsal mf{.tip = MafsalTip::Doner, .ana = 0, .cocuk = k, .capa = capaKonum(20, 0, 0)};
  mf.capaCocuk = capaKonum(5, 0, 0);
  m.mafsalEkle(mf);
  const auto d = m.ileriKinematik();
  const Vec3d kok = d[k].noktaUygula({0, 0, 0});
  FIKO_YAKIN(kok.x, 15.0, 1e-9);   // 20 − 5
  FIKO_YAKIN(kok.y, 0.0, 1e-9);
  // Çocuğun (5,0,0) noktası TAM mafsal çerçevesine oturmalı.
  const Vec3d mafsalNoktasi = d[k].noktaUygula({5, 0, 0});
  FIKO_YAKIN(mafsalNoktasi.x, 20.0, 1e-9);
  FIKO_YAKIN(mafsalNoktasi.y, 0.0, 1e-9);
}

namespace {
// GERÇEK düzlemsel 4-çubuk (çift taraflı çapa ile artık ifade edilebilir):
// gövde pivotları O1=(0,0,0) ve O2=(d,0,0); krank a, biyel b, sarkaç c.
//   ağaç: gövde→krank (O1), gövde→sarkaç (O2)
//   halka: krank→biyel (krank ucu a) ve biyel→sarkaç (biyel ucu b ↔ sarkaç ucu c)
Mekanizma dortCubukGercek(f64 a, f64 b, f64 c, f64 d) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto K = m.uzuvEkle("Krank");
  const auto B = m.uzuvEkle("Biyel");
  const auto S = m.uzuvEkle("Sarkaç");
  m.mafsalEkle({.ad = "O1", .tip = MafsalTip::Doner, .ana = 0, .cocuk = K});
  m.mafsalEkle({.ad = "O2", .tip = MafsalTip::Doner, .ana = 0, .cocuk = S,
                .capa = capaKonum(d, 0, 0)});
  m.mafsalEkle({.ad = "A", .tip = MafsalTip::Doner, .ana = K, .cocuk = B,
                .capa = capaKonum(a, 0, 0)});
  Mafsal kapat{.ad = "B", .tip = MafsalTip::Doner, .ana = B, .cocuk = S,
               .capa = capaKonum(b, 0, 0)};
  kapat.capaCocuk = capaKonum(c, 0, 0);  // sarkacın UCU biyelin ucuna oturur
  m.mafsalEkle(kapat);
  return m;
}
}  // namespace

FIKO_TEST(kinematik_gercek_dort_cubuk_grashof) {
  // Grashof: en kısa(a=20) + en uzun(d=70) ≤ diğer ikisi(60+40=100) → krank TAM TUR döner.
  Mekanizma m = dortCubukGercek(20, 60, 40, 70);
  FIKO_KONTROL(!m.agacMi());
  FIKO_KONTROL(m.halkaKapatanlar().size() == 1);
  for (f64 q : {0.0, 0.8, 1.7, 2.6, 3.5, 4.4, 5.3}) {
    m.mafsal(0)->q = q;                       // krankı sür
    f64 art = 1e9;
    FIKO_KONTROL(m.kapaliZincirCoz({0}, 300, 1e-6, &art));
    // Halka kapalı: biyelin ucu (b) ile sarkacın ucu (c) ÇAKIŞIK olmalı.
    const auto d3 = m.ileriKinematik();
    const Vec3d biyelUcu = d3[2].noktaUygula({60, 0, 0});
    const Vec3d sarkacUcu = d3[3].noktaUygula({40, 0, 0});
    FIKO_KONTROL(std::hypot(biyelUcu.x - sarkacUcu.x, biyelUcu.y - sarkacUcu.y) < 1e-4);
    // Sarkaç kökü O2'de sabit kalmalı (gövdeye bağlı).
    const Vec3d sarkacKok = d3[3].noktaUygula({0, 0, 0});
    FIKO_YAKIN(sarkacKok.x, 70.0, 1e-6);
    FIKO_YAKIN(sarkacKok.y, 0.0, 1e-6);
  }
}

// ── DMU / çarpışma taraması ─────────────────────────────────────────────────
namespace {
Aabbd kutuYap(f64 sx, f64 sy, f64 sz) {
  Aabbd a;
  a.genislet({-sx / 2, -sy / 2, -sz / 2});
  a.genislet({sx / 2, sy / 2, sz / 2});
  return a;
}
}  // namespace

FIKO_TEST(kinematik_carpisma_komsu_atlanir) {
  // Gövde ve kol MAFSALLA bağlı ve kutuları iç içe → komşu olduğu için raporlanmamalı.
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto k = m.uzuvEkle("Kol");
  m.mafsalEkle({.tip = MafsalTip::Doner, .ana = 0, .cocuk = k});
  const std::vector<Aabbd> kutular{kutuYap(50, 50, 50), kutuYap(40, 40, 40)};
  FIKO_KONTROL(carpismaTara(m, kutular, 0, 1, 5).empty());          // komşu → atlanır
  const auto zorla = carpismaTara(m, kutular, 0, 1, 5, /*komsuAtla*/ false);
  FIKO_KONTROL(zorla.size() == 1);                                   // zorlanınca görünür
  FIKO_KONTROL(zorla[0].uzuvA == 0 && zorla[0].uzuvB == 1);
  FIKO_KONTROL(zorla[0].hacimOran > 0.9);                            // 40³ tamamen 50³ içinde
}

FIKO_TEST(kinematik_carpisma_hareketle_olusur) {
  // İki BAĞIMSIZ kol (ikisi de gövdeye bağlı, birbirine DEĞİL) — biri dönerken
  // diğerinin üstüne gelir. Komşu değiller → çarpışma raporlanmalı.
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto a = m.uzuvEkle("KolA");
  const auto b = m.uzuvEkle("KolB");
  // KolA sabit: +X'te 40 mm'de duruyor.
  Mafsal ma{.tip = MafsalTip::Sabit, .ana = 0, .cocuk = a, .capa = capaKonum(40, 0, 0)};
  m.mafsalEkle(ma);
  // KolB döner: yarıçap 40, açı 0'da +X'te (A ile ÇAKIŞIK), açı π'de −X'te (uzak).
  Mafsal mb{.tip = MafsalTip::Doner, .ana = 0, .cocuk = b};
  mb.capaCocuk = capaKonum(-40, 0, 0);  // kol ucu merkezden 40 mm dışarıda
  const auto mbi = m.mafsalEkle(mb);
  const std::vector<Aabbd> kutular{Aabbd{}, kutuYap(20, 20, 20), kutuYap(20, 20, 20)};

  // π..π (uzak konum): çarpışma YOK
  m.yasaEkle({.mafsal = mbi, .tip = YasaTip::Sabit, .deger = pi});
  FIKO_KONTROL(carpismaTara(m, kutular, 0, 1, 3).empty());

  // 0..0 (çakışık konum): çarpışma VAR
  Mekanizma m2 = m;
  m2.yasalariTemizle();
  m2.yasaEkle({.mafsal = mbi, .tip = YasaTip::Sabit, .deger = 0.0});
  const auto c = carpismaTara(m2, kutular, 0, 1, 3);
  FIKO_KONTROL(c.size() == 1);
  FIKO_KONTROL(c[0].uzuvA == 1 && c[0].uzuvB == 2);
  FIKO_KONTROL(c[0].hacimOran > 0.5);
}

FIKO_TEST(kinematik_carpisma_zaman_taramasi_ilk_ani_verir) {
  // Kol π'den 0'a doğrusal süpürülür → SONDA çarpışır; ilk çarpışma anı raporlanır.
  Mekanizma m;
  m.uzuvEkle("Gövde");
  const auto a = m.uzuvEkle("KolA");
  const auto b = m.uzuvEkle("KolB");
  m.mafsalEkle({.tip = MafsalTip::Sabit, .ana = 0, .cocuk = a, .capa = capaKonum(40, 0, 0)});
  Mafsal mb{.tip = MafsalTip::Doner, .ana = 0, .cocuk = b};
  mb.capaCocuk = capaKonum(-40, 0, 0);
  const auto mbi = m.mafsalEkle(mb);
  m.yasaEkle({.mafsal = mbi, .tip = YasaTip::Dogrusal, .deger = pi, .hiz = -pi});  // t:0→1, q:π→0
  const auto c = carpismaTara(m, {Aabbd{}, kutuYap(20, 20, 20), kutuYap(20, 20, 20)}, 0, 1, 21);
  FIKO_KONTROL(c.size() == 1);      // tek çift, YALNIZ ilk an
  FIKO_KONTROL(c[0].zaman > 0.5);   // yaklaşırken değil, sona doğru
  FIKO_KONTROL(c[0].zaman <= 1.0);
}

FIKO_TEST(kinematik_carpisma_gecersiz_girdi_bos_doner) {
  Mekanizma m;
  m.uzuvEkle("Gövde");
  FIKO_KONTROL(carpismaTara(m, {}, 0, 1, 5).empty());          // kutu yok
  FIKO_KONTROL(carpismaTara(m, {kutuYap(1, 1, 1)}, 0, 1, 0).empty());  // adım 0
  Mekanizma bos;
  FIKO_KONTROL(carpismaTara(bos, {kutuYap(1, 1, 1)}, 0, 1, 5).empty());  // uzuv < 2
}