#include "fiko/kinematik/mekanizma.hpp"

#include <algorithm>
#include <cmath>
#include <numbers>
#include <vector>

namespace fiko::kinematik {

bool& halkaOtoCoz() {
  static bool a = true;  // kapalı zincirde FK tek başına yanlış → varsayılan AÇIK
  return a;
}

f64& girisimAsgariHacim() {
  static f64 a = 1e-9;  // eski davranış birebir: kesişim hacmi ~0 → aday elenir
  return a;
}

Vec3f& girisimRenk() {
  static Vec3f a{0.95f, 0.15f, 0.10f};  // DMU kırmızısı
  return a;
}

f32& girisimAlfa() {
  static f32 a = 0.55f;  // yarı saydam → altındaki parçalar görünür kalır
  return a;
}

const char* mafsalTipAdi(MafsalTip t) {
  switch (t) {
    case MafsalTip::Sabit:      return "Sabit";
    case MafsalTip::Doner:      return "Döner";
    case MafsalTip::Kayar:      return "Kayar";
    case MafsalTip::Silindirik: return "Silindirik";
    case MafsalTip::Vida:       return "Vida";
    case MafsalTip::Kuresel:    return "Küresel";
    case MafsalTip::Duzlemsel:  return "Düzlemsel";
    case MafsalTip::Serbest:    return "Serbest";
  }
  return "?";
}

int mafsalSerbestlik(MafsalTip t) {
  switch (t) {
    case MafsalTip::Sabit:      return 0;
    case MafsalTip::Doner:      return 1;
    case MafsalTip::Kayar:      return 1;
    case MafsalTip::Vida:       return 1;  // öteleme dönmeye BAĞLI → tek bağımsız değişken
    case MafsalTip::Silindirik: return 2;
    case MafsalTip::Kuresel:    return 3;
    case MafsalTip::Duzlemsel:  return 3;
    case MafsalTip::Serbest:    return 6;
  }
  return 0;
}

std::size_t Mekanizma::uzuvEkle(std::string ad, u32 sahne, Donusumd taban) {
  uzuvlar_.push_back({std::move(ad), sahne, taban});
  return uzuvlar_.size() - 1;
}

std::size_t Mekanizma::mafsalEkle(Mafsal m) {
  mafsallar_.push_back(std::move(m));
  return mafsallar_.size() - 1;
}

int Mekanizma::serbestlikDerecesi() const {
  const int n = (int)uzuvlar_.size();
  const int j = (int)mafsallar_.size();
  if (n <= 0) return 0;
  int toplamF = 0;
  for (const Mafsal& m : mafsallar_) toplamF += mafsalSerbestlik(m.tip);
  return 6 * (n - 1 - j) + toplamF;
}

f64 Mekanizma::sinirKirp(const Mafsal& m, f64 deger) {
  if (!m.sinirVar()) return deger;
  return std::clamp(deger, m.altSinir, m.ustSinir);
}

f64 yasaDegeri(const MafsalYasa& y, f64 t) {
  switch (y.tip) {
    case YasaTip::Sabit:
      return y.deger;
    case YasaTip::Dogrusal:
      return y.deger + y.hiz * t;
    case YasaTip::Harmonik:
      if (y.periyot <= 0.0) return y.deger;
      return y.deger + y.genlik * std::sin(2.0 * std::numbers::pi * t / y.periyot + y.faz);
    case YasaTip::AnahtarKare: {
      if (y.anahtarlar.empty()) return y.deger;
      if (t <= y.anahtarlar.front().first) return y.anahtarlar.front().second;
      if (t >= y.anahtarlar.back().first) return y.anahtarlar.back().second;
      for (std::size_t i = 1; i < y.anahtarlar.size(); ++i) {
        const auto& a = y.anahtarlar[i - 1];
        const auto& b = y.anahtarlar[i];
        if (t <= b.first) {
          const f64 dt = b.first - a.first;
          if (dt <= 1e-12) return b.second;           // çakışık anahtar → sonrakine atla
          return a.second + (b.second - a.second) * (t - a.first) / dt;
        }
      }
      return y.anahtarlar.back().second;
    }
  }
  return y.deger;
}

std::size_t Mekanizma::yasaEkle(MafsalYasa y) {
  yasalar_.push_back(std::move(y));
  return yasalar_.size() - 1;
}

void Mekanizma::zamanaAyarla(f64 t) {
  for (const MafsalYasa& y : yasalar_) {
    if (y.mafsal >= mafsallar_.size()) continue;
    Mafsal& m = mafsallar_[y.mafsal];
    const f64 v = yasaDegeri(y, t);
    if (y.degisken == 1) m.q2 = v;
    else if (y.degisken == 2) m.q3 = v;
    else m.q = v;  // sınır kırpması mafsalDonusumu içinde TEK KAPIDAN uygulanır
  }
}

Donusumd mafsalDonusumu(const Mafsal& m) {
  // Eksen = çapa çerçevesinin +Z'si (yerel). Hareket ÇAPA YERELİNDE kurulur, sonra
  // çapa ile ana uzva taşınır: T = capa * hareket.
  Donusumd hareket;  // birim
  const f64 q = Mekanizma::sinirKirp(m, m.q);
  switch (m.tip) {
    case MafsalTip::Sabit:
      break;
    case MafsalTip::Doner:
      hareket.yonelim = Quatd::eksenAci({0, 0, 1}, q);
      break;
    case MafsalTip::Kayar:
      hareket.konum = {0, 0, q};
      break;
    case MafsalTip::Silindirik:
      hareket.yonelim = Quatd::eksenAci({0, 0, 1}, q);
      hareket.konum = {0, 0, m.q2};
      break;
    case MafsalTip::Vida: {
      // Öteleme dönmeye kilitli: tam turda `vidaAdim` mm ilerler (sağ vida).
      hareket.yonelim = Quatd::eksenAci({0, 0, 1}, q);
      const f64 tur = q / (2.0 * std::numbers::pi);
      hareket.konum = {0, 0, tur * m.vidaAdim};
      break;
    }
    case MafsalTip::Kuresel:
      // q/q2/q3 = XYZ sırasıyla iç-eksen dönmeleri (küçük açı kullanımı için yeter).
      hareket.yonelim = (Quatd::eksenAci({0, 0, 1}, m.q3) * Quatd::eksenAci({0, 1, 0}, m.q2) *
                         Quatd::eksenAci({1, 0, 0}, m.q))
                            .birim();
      break;
    case MafsalTip::Duzlemsel:
      // Çapa düzlemi = yerel XY; q/q2 öteleme, q3 normal etrafında dönme.
      hareket.konum = {q, m.q2, 0};
      hareket.yonelim = Quatd::eksenAci({0, 0, 1}, m.q3);
      break;
    case MafsalTip::Serbest:
      hareket.konum = {q, m.q2, m.q3};
      break;
  }
  // Çocuk çapası: mafsal çerçevesi çocuk uzvun YERELİNDE nerede duruyorsa oradan
  // geri gelinir (birim çapa → eski davranış birebir).
  return m.capa * hareket * m.capaCocuk.ters();
}

namespace {
// Gövdeden yayılım: hangi mafsal ağaca girdi (çocuğu İLK çözen), hangisi halkayı
// kapatıyor. `ileriKinematik` ile AYNI sıra → çözücü ve FK tutarlı.
void agacAyristir(const std::vector<Mafsal>& mafsallar, std::size_t n,
                  std::vector<char>& agacta, std::vector<std::size_t>& halka) {
  agacta.assign(mafsallar.size(), 0);
  halka.clear();
  if (n == 0) return;
  std::vector<char> cozuldu(n, 0);
  cozuldu[0] = 1;
  for (std::size_t tur = 0; tur < n; ++tur) {
    bool buyudu = false;
    for (std::size_t i = 0; i < mafsallar.size(); ++i) {
      const Mafsal& m = mafsallar[i];
      if (m.ana >= n || m.cocuk >= n || agacta[i]) continue;
      if (!cozuldu[m.ana] || cozuldu[m.cocuk]) continue;
      agacta[i] = 1;
      cozuldu[m.cocuk] = 1;
      buyudu = true;
    }
    if (!buyudu) break;
  }
  for (std::size_t i = 0; i < mafsallar.size(); ++i)
    if (!agacta[i] && mafsallar[i].ana < n && mafsallar[i].cocuk < n) halka.push_back(i);
}
}  // namespace

std::vector<std::size_t> Mekanizma::halkaKapatanlar() const {
  std::vector<char> agacta;
  std::vector<std::size_t> halka;
  agacAyristir(mafsallar_, uzuvlar_.size(), agacta, halka);
  return halka;
}

bool Mekanizma::kapaliZincirCoz(const std::vector<std::size_t>& surulen, int enFazlaAdim,
                                f64 tolerans, f64* artikCikti) {
  const std::size_t n = uzuvlar_.size();
  std::vector<char> agacta;
  std::vector<std::size_t> halka;
  agacAyristir(mafsallar_, n, agacta, halka);
  if (halka.empty()) { if (artikCikti) *artikCikti = 0.0; return true; }  // ağaç → iş yok

  // Bilinmeyenler: SÜRÜLMEYEN ve serbestliği olan TÜM mafsalların q'su —
  // ağaçtakiler VE HALKAYI KAPATANLAR. Halka mafsalının kendi değişkeni bilinmeyen
  // DEĞİLSE 6-DOF eşitlik kısıtı onun SERBEST ekseninde de eşitlik dayatır ve sistem
  // AŞIRI KISITLI olur (ölçüldü 07-21: 4-çubukta yalnız simetrik başlangıç açısı
  // çözülüyordu, krank dönünce yakınsamıyordu). Halka mafsalı FK'yı etkilemez, yalnız
  // artığa girer — kısıt böylece "kendi ekseni etrafında serbest" doğru modellenir.
  std::vector<std::size_t> bilinmeyen;
  for (std::size_t i = 0; i < mafsallar_.size(); ++i) {
    if (mafsallar_[i].ana >= n || mafsallar_[i].cocuk >= n) continue;
    if (std::find(surulen.begin(), surulen.end(), i) != surulen.end()) continue;
    if (mafsalSerbestlik(mafsallar_[i].tip) == 0) continue;  // Sabit → değişken yok
    bilinmeyen.push_back(i);
  }
  if (bilinmeyen.empty()) { if (artikCikti) *artikCikti = -1.0; return false; }

  // Artık: her halka mafsalı için beklenen çocuk çerçevesi ile gerçeği arasındaki
  // konum (mm) + yönelim (quaternion vektör parçası ×2 ≈ radyan) farkı.
  const auto artikHesapla = [&](std::vector<f64>& r) {
    const std::vector<Donusumd> dunya = ileriKinematik();
    r.assign(halka.size() * 6, 0.0);
    for (std::size_t h = 0; h < halka.size(); ++h) {
      const Mafsal& m = mafsallar_[halka[h]];
      const Donusumd beklenen = dunya[m.ana] * mafsalDonusumu(m);
      const Donusumd& gercek = dunya[m.cocuk];
      r[h * 6 + 0] = beklenen.konum.x - gercek.konum.x;
      r[h * 6 + 1] = beklenen.konum.y - gercek.konum.y;
      r[h * 6 + 2] = beklenen.konum.z - gercek.konum.z;
      Quatd fark = beklenen.yonelim * gercek.yonelim.eslenik();
      if (fark.w < 0) { fark.x = -fark.x; fark.y = -fark.y; fark.z = -fark.z; }  // kısa yol
      r[h * 6 + 3] = 2.0 * fark.x;
      r[h * 6 + 4] = 2.0 * fark.y;
      r[h * 6 + 5] = 2.0 * fark.z;
    }
  };
  const auto norm = [](const std::vector<f64>& v) {
    f64 s2 = 0;
    for (f64 x : v) s2 += x * x;
    return std::sqrt(s2);
  };

  std::vector<f64> r, rArti, J;
  artikHesapla(r);
  f64 son = norm(r);
  const std::size_t M = r.size(), N = bilinmeyen.size();
  constexpr f64 kEps = 1e-6;    // sayısal türev adımı
  constexpr f64 kSonum = 1e-6;  // Levenberg sönümü (tekil Jacobian koruması)
  for (int adim = 0; adim < enFazlaAdim && son > tolerans; ++adim) {
    J.assign(M * N, 0.0);
    for (std::size_t j = 0; j < N; ++j) {
      Mafsal& mf = mafsallar_[bilinmeyen[j]];
      const f64 eski = mf.q;
      mf.q = eski + kEps;
      artikHesapla(rArti);
      mf.q = eski;
      for (std::size_t i = 0; i < M; ++i) J[i * N + j] = (rArti[i] - r[i]) / kEps;
    }
    // Normal denklemler: (JᵀJ + λI) dq = −Jᵀr  → Gauss eliminasyonu (N küçük).
    std::vector<f64> A(N * N, 0.0), b(N, 0.0);
    for (std::size_t a = 0; a < N; ++a) {
      for (std::size_t c = 0; c < N; ++c) {
        f64 s2 = 0;
        for (std::size_t i = 0; i < M; ++i) s2 += J[i * N + a] * J[i * N + c];
        A[a * N + c] = s2 + (a == c ? kSonum : 0.0);
      }
      f64 s3 = 0;
      for (std::size_t i = 0; i < M; ++i) s3 += J[i * N + a] * r[i];
      b[a] = -s3;
    }
    for (std::size_t k = 0; k < N; ++k) {  // kısmi pivotlu eliminasyon
      std::size_t piv = k;
      for (std::size_t i = k + 1; i < N; ++i)
        if (std::fabs(A[i * N + k]) > std::fabs(A[piv * N + k])) piv = i;
      if (std::fabs(A[piv * N + k]) < 1e-14) { if (artikCikti) *artikCikti = son; return false; }
      if (piv != k) {
        for (std::size_t c = 0; c < N; ++c) std::swap(A[k * N + c], A[piv * N + c]);
        std::swap(b[k], b[piv]);
      }
      for (std::size_t i = k + 1; i < N; ++i) {
        const f64 f = A[i * N + k] / A[k * N + k];
        if (f == 0.0) continue;
        for (std::size_t c = k; c < N; ++c) A[i * N + c] -= f * A[k * N + c];
        b[i] -= f * b[k];
      }
    }
    std::vector<f64> dq(N, 0.0);
    for (std::size_t ki = N; ki-- > 0;) {
      f64 s4 = b[ki];
      for (std::size_t c = ki + 1; c < N; ++c) s4 -= A[ki * N + c] * dq[c];
      dq[ki] = s4 / A[ki * N + ki];
    }
    // Geri-izleme: adım artığı büyütüyorsa yarıla (ıraksama koruması).
    f64 olcek = 1.0;
    std::vector<f64> yedek(N);
    for (std::size_t j = 0; j < N; ++j) yedek[j] = mafsallar_[bilinmeyen[j]].q;
    bool iyilesti = false;
    for (int deneme = 0; deneme < 12; ++deneme) {
      for (std::size_t j = 0; j < N; ++j) mafsallar_[bilinmeyen[j]].q = yedek[j] + dq[j] * olcek;
      artikHesapla(rArti);
      const f64 yeni = norm(rArti);
      if (yeni < son) { r = rArti; son = yeni; iyilesti = true; break; }
      olcek *= 0.5;
    }
    if (!iyilesti) {
      for (std::size_t j = 0; j < N; ++j) mafsallar_[bilinmeyen[j]].q = yedek[j];
      break;  // daha iyisi bulunamadı → yerel minimum
    }
  }
  if (artikCikti) *artikCikti = son;
  return son <= tolerans;
}

namespace {
// Yerel AABB'yi dünya-hizalı AABB'ye çevir (8 köşe dönüştürülür → yeniden kutu).
Aabbd dunyaKutu(const Aabbd& yerel, const Donusumd& d) {
  Aabbd out;
  if (!yerel.gecerliMi()) return out;
  const Vec3d k[8] = {{yerel.asgari.x, yerel.asgari.y, yerel.asgari.z},
                      {yerel.azami.x, yerel.asgari.y, yerel.asgari.z},
                      {yerel.asgari.x, yerel.azami.y, yerel.asgari.z},
                      {yerel.azami.x, yerel.azami.y, yerel.asgari.z},
                      {yerel.asgari.x, yerel.asgari.y, yerel.azami.z},
                      {yerel.azami.x, yerel.asgari.y, yerel.azami.z},
                      {yerel.asgari.x, yerel.azami.y, yerel.azami.z},
                      {yerel.azami.x, yerel.azami.y, yerel.azami.z}};
  for (const Vec3d& p : k) out.genislet(d.noktaUygula(p));
  return out;
}

// İki kutunun çakışma hacmi / küçük kutunun hacmi (0 = değmiyor).
f64 cakismaOrani(const Aabbd& a, const Aabbd& b) {
  const f64 dx = std::min(a.azami.x, b.azami.x) - std::max(a.asgari.x, b.asgari.x);
  const f64 dy = std::min(a.azami.y, b.azami.y) - std::max(a.asgari.y, b.asgari.y);
  const f64 dz = std::min(a.azami.z, b.azami.z) - std::max(a.asgari.z, b.asgari.z);
  if (dx <= 0 || dy <= 0 || dz <= 0) return 0.0;  // temas (=0) çarpışma SAYILMAZ
  const Vec3d ba = a.boyut(), bb = b.boyut();
  const f64 ha = ba.x * ba.y * ba.z, hb = bb.x * bb.y * bb.z;
  const f64 kucuk = std::min(ha, hb);
  return kucuk > 1e-12 ? (dx * dy * dz) / kucuk : 0.0;
}
}  // namespace

std::vector<Carpisma> carpismaTara(Mekanizma& mek, const std::vector<Aabbd>& yerelKutu,
                                   f64 t0, f64 t1, int adim, bool komsuAtla) {
  std::vector<Carpisma> sonuc;
  const std::size_t n = mek.uzuvlar().size();
  if (n < 2 || yerelKutu.empty() || adim < 1) return sonuc;
  // Komşuluk: bir mafsalla doğrudan bağlı çiftler (montajda temas normaldir).
  std::vector<char> komsu(n * n, 0);
  if (komsuAtla)
    for (const Mafsal& m : mek.mafsallar())
      if (m.ana < n && m.cocuk < n) { komsu[m.ana * n + m.cocuk] = 1; komsu[m.cocuk * n + m.ana] = 1; }
  std::vector<char> bulundu(n * n, 0);  // aynı çift bir kez raporlanır
  const bool halkaVar = !mek.halkaKapatanlar().empty();
  for (int i = 0; i < adim; ++i) {
    const f64 t = (adim == 1) ? t0 : t0 + (t1 - t0) * (f64)i / (f64)(adim - 1);
    mek.zamanaAyarla(t);
    if (halkaVar) {
      std::vector<std::size_t> surulen;
      for (const MafsalYasa& y : mek.yasalar()) surulen.push_back(y.mafsal);
      mek.kapaliZincirCoz(surulen);
    }
    const std::vector<Donusumd> dunya = mek.ileriKinematik();
    std::vector<Aabbd> kutu(n);
    for (std::size_t a = 0; a < n; ++a)
      if (a < yerelKutu.size()) kutu[a] = dunyaKutu(yerelKutu[a], dunya[a]);
    for (std::size_t a = 0; a < n; ++a) {
      if (!kutu[a].gecerliMi()) continue;
      for (std::size_t b = a + 1; b < n; ++b) {
        if (!kutu[b].gecerliMi() || bulundu[a * n + b]) continue;
        if (komsuAtla && komsu[a * n + b]) continue;
        const f64 oran = cakismaOrani(kutu[a], kutu[b]);
        if (oran <= 0.0) continue;
        bulundu[a * n + b] = 1;
        sonuc.push_back({a, b, t, oran});
      }
    }
  }
  return sonuc;
}

bool Mekanizma::agacMi() const {
  const std::size_t n = uzuvlar_.size();
  if (n == 0) return false;
  // Her uzvun EN FAZLA bir ana mafsalı olmalı (çoklu ana = kapalı zincir).
  std::vector<int> anaSayi(n, 0);
  for (const Mafsal& m : mafsallar_) {
    if (m.ana >= n || m.cocuk >= n) return false;  // bozuk indeks
    if (m.cocuk == 0) return false;                // gövde çocuk olamaz
    if (++anaSayi[m.cocuk] > 1) return false;
  }
  // Gövdeden (0) BFS ile hepsi erişilebilir mi?
  std::vector<bool> ulasildi(n, false);
  ulasildi[0] = true;
  bool buyudu = true;
  while (buyudu) {
    buyudu = false;
    for (const Mafsal& m : mafsallar_)
      if (ulasildi[m.ana] && !ulasildi[m.cocuk]) { ulasildi[m.cocuk] = true; buyudu = true; }
  }
  return std::all_of(ulasildi.begin(), ulasildi.end(), [](bool b) { return b; });
}

std::vector<Donusumd> Mekanizma::ileriKinematik() const {
  const std::size_t n = uzuvlar_.size();
  std::vector<Donusumd> dunya(n);
  for (std::size_t i = 0; i < n; ++i) dunya[i] = uzuvlar_[i].taban;
  if (n == 0) return dunya;
  // Gövdeden yayarak zincirle; her tur en az bir uzuv çözülür → en çok n tur.
  std::vector<bool> cozuldu(n, false);
  cozuldu[0] = true;
  for (std::size_t tur = 0; tur < n; ++tur) {
    bool buyudu = false;
    for (const Mafsal& m : mafsallar_) {
      if (m.ana >= n || m.cocuk >= n) continue;
      if (!cozuldu[m.ana] || cozuldu[m.cocuk]) continue;
      dunya[m.cocuk] = dunya[m.ana] * mafsalDonusumu(m);
      cozuldu[m.cocuk] = true;
      buyudu = true;
    }
    if (!buyudu) break;
  }
  return dunya;
}

}  // namespace fiko::kinematik