#include <Adafruit_GFX.h>      // Library grafis untuk ILI9341
#include <Adafruit_ILI9341.h>  // Library untuk layar ILI9341
#include <Adafruit_FT6206.h>   // Library untuk touch panel capacitive FT6206

// Pin untuk SPI dan TFT ILI9341
#define TFT_CS 15
#define TFT_DC 2
#define TFT_RST 4

// Inisialisasi objek ILI9341 dan touch screen
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
Adafruit_FT6206 ts = Adafruit_FT6206();

//typedef tMobileRobot
//{
  int Mx, My;
  float Mv, Ms;
//} mb;

const unsigned char PROGMEM Peta[24][24]=
{
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1}, 
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}, 
};

int StartX=2, StartY=0;
int FinishX=10, FinishY=23;

int Fill[24][24];
//typedef tSimpangan
//{
  int SimpanganX[100], SimpanganY[100];
  //int Nilai;
//} Simpangan[100];
int PanjangS=0, PosisiS=0;

int Cek(int x, int y)
{
  return Peta[x][y]==1&&Fill[x][y]==0;
}

int Cari(int x, int y, int *X, int *Y)
{
  if(Cek(x,y))
  {
    *X=x;
    *Y=y;
    return true;
  }
  return false;
}

int CekJalan(int x, int y)
{
  int s=0;
  if(Cek(x-1,y-1)) s++;
  if(Cek(x,y-1)) s++;
  if(Cek(x+1,y-1)) s++;
  if(Cek(x-1,y)) s++;
  if(Cek(x+1,y)) s++;
  if(Cek(x-1,y+1)) s++;
  if(Cek(x,y+1)) s++;
  if(Cek(x+1,y+1)) s++;
}

void CariJalan(int x, int y, int *X, int *Y)
{
  if(Cari(x-1,y-1,X,Y)) return;
  if(Cari(x,y-1,X,Y)) return;
  if(Cari(x+1,y-1,X,Y)) return;
  if(Cari(x-1,y,X,Y)) return;
  if(Cari(x+1,y,X,Y)) return;
  if(Cari(x-1,y+1,X,Y)) return;
  if(Cari(x,y+1,X,Y)) return;
  if(Cari(x+1,y+1,X,Y)) return;
}

void Jalan()
{
  int x=FinishX;
  int y=FinishY;
  int Selesai=false;
  while(!Selesai)
  {
    int s;
    int X, Y;
    if(s=CekJalan(x,y))     // ada berapa kemungkinan simpangan/jalan
    {
      if(s>1)
      {
        SimpanganX[PosisiS]=x;
        SimpanganY[PosisiS]=y;
        PanjangS++;
        if(PanjangS>100) PanjangS=100;
        PosisiS++;
        if(PosisiS>=100) PosisiS=0;
      }
      CariJalan(x,y,&X,&Y);         // ambil satu jalan
      Fill[X][Y]=Fill[x][y]+1;
    }
    else                            // tidak ada jalan/jalan buntu
    {
      if(PanjangS>0)                // apakah masih ada simpangan ?
      {
        PosisiS--;
        if(PosisiS<0) PosisiS=99;
        x=SimpanganX[PosisiS];
        y=SimpanganY[PosisiS];
        PanjangS--;
      }
      else  // Finish
      {
        Selesai=true;  // keluar dari while
      }
    }
  }
}

void Kotak(int x, int y, uint16_t Warna)
{
  tft.fillRect(y*10,x*10, 10, 10, Warna);
}

void TampilPeta()
{
  for(int y=0;y<24;y++)
  {
    for(int x=0;x<24;x++)
    {      
      Kotak(x,y,Peta[x][23-y]==1?ILI9341_RED:ILI9341_BLUE);
    }
  }
}

void setup() {
  Serial.begin(115200);
  // Inisialisasi TFT
  tft.begin();
  tft.setRotation(1);  // Atur rotasi layar (sesuaikan sesuai orientasi yang diinginkan)
  tft.fillScreen(ILI9341_BLACK);  // Bersihkan layar dengan warna hitam

  // Inisialisasi touch panel
  if (!ts.begin(40)) { // Sensitivitas touch panel
    Serial.println("Touchscreen tidak terdeteksi!");
    while (1);
  }
  Serial.println("Touchscreen siap!");
  TampilPeta();
  Kotak(0,0, ILI9341_CYAN);
  Kotak(12,23, ILI9341_GREEN);
  Kotak(23,12, ILI9341_PINK);

  Kotak(StartX,StartY, ILI9341_YELLOW);
  Kotak(FinishX,FinishY, ILI9341_WHITE);

  Mx=StartX;
  My=StartY;
  Mv=0;
  Ms=90;
  for(int x=0; x<24; x++)
    for(int y=0; y<24; y++)
      Fill[x][y]=0;
}

void loop() {
  // Periksa apakah ada input sentuhan
  if (ts.touched()) {
    // Jika ada input sentuhan, baca posisi x dan y
    TS_Point p = ts.getPoint();

    // Ubah koordinat agar sesuai dengan orientasi layar
    int x = map(p.y, 0, 240, 0, tft.width());
    int y = map(p.x, 0, 320, 0, tft.height());

    // Tampilkan informasi koordinat yang ditekan
    Serial.print("X: "); Serial.print(x);
    Serial.print(" Y: "); Serial.println(y);

    // Gambarkan kotak di posisi yang ditekan
    drawSquare(x*320/426, (180-y)*240/180);
    Jalan();
  }
  delay(50); // Tambahkan sedikit delay untuk menghindari pembacaan berulang terlalu cepat
}

// Fungsi untuk menggambar kotak di posisi yang ditekan
void drawSquare(int x, int y) {
  int size = 20;  // Ukuran kotak

  // Gambarkan kotak dengan pusat di posisi (x, y)
  tft.fillRect(x - size / 2, y - size / 2, size, size, ILI9341_RED);
}