//#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;

int StartX=0, StartY=0;
int FinishX=2, FinishY=8;
/*
#define Lx 9
#define Ly 9

const unsigned short int PROGMEM Peta[Ly][Lx]=
{//0 1 2 3 4 5 6 7 8    // Line Tracing
  {0,0,1,1,1,0,1,1,0},  // 8
  {0,0,0,1,0,0,0,1,0},  // 7
  {1,1,1,1,1,1,1,1,1},  // 6
  {0,0,1,0,0,0,1,0,0},  // 5
  {0,0,1,0,0,0,1,0,1},  // 4
  {0,0,1,0,0,0,1,1,1},  // 3
  {1,1,1,1,1,1,1,0,0},  // 2
  {1,0,0,0,0,1,0,0,0},  // 1
  {1,0,0,0,1,1,0,0,0}   // 0
};

/*

const unsigned short int PROGMEM Peta[Ly][Lx]=
{
  {0,0,0,1,1,1,0,0,0},  // Map Tracing
  {0,0,0,1,1,1,0,0,0},
  {0,0,0,1,1,1,0,0,0},
  {1,1,1,1,1,1,1,1,1},
  {1,1,1,1,1,1,1,0,0},
  {1,1,1,1,1,1,1,1,1},
  {1,1,1,0,0,0,1,1,1},
  {1,1,1,0,0,0,1,1,1},
  {1,1,1,0,0,0,1,1,1}
};
*/
#define Lx 24
#define Ly 24

const unsigned short int PROGMEM Peta[Ly][Lx]=
{
  {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0}, //y=23
  {0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0},  // 22
  {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0},  // 21
  {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1},  // 20 
  {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0},  // 19
  {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0},  // 18
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 17
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 16
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 15
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 14
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 13
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 12
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 11
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 10
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1},  // 9
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},  // 8
  {0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0},  // 7
  {0,0,0,1,1,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},  // 6
  {0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},  // 5
  {0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},  // 4
  {0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},  // 3
  {0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},  // 2
  {0,0,0,1,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0},  // 1
  {0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0}   // 0 
// 0 1 2 3 4 5 6 7 8 9 1011121314151617181920212223
};
//*/

int LintasanX[100];
int LintasanY[100];
int nLintasan=0;

int Fill[Lx][Ly];
int SimpanganX[100], SimpanganY[100];
int PanjangS=0, PosisiS=0;

int Cek(int x, int y, int F)
{
  if(x>=0&&y>=0&&x<Lx&&y<Ly)
    return pgm_read_byte(&(Peta[Ly-1-y][x]))==1&&Fill[x][y]>(F+4);
  else return false;
}

int CekPeta(int x, int y)
{
  if(x>=0&&y>=0&&x<Lx&&y<Ly)
    return pgm_read_byte(&(Peta[Ly-1-y][x]))==1;
  else return false;
}

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

int CekJalan(int x, int y)    // Periksa kemungkinan percagangan
{
  int s=0;
  int F=Fill[x][y];
  if(Cek(x-1,y-1,F)&&CekPeta(x-1,y)&&CekPeta(x,y-1)) s++;
  if(Cek(x,y-1,F)) s++;
  if(Cek(x+1,y-1,F)&&CekPeta(x,y-1)&&CekPeta(x-1,y)) s++;
  if(Cek(x-1,y,F)) s++;
  if(Cek(x+1,y,F)) s++;
  if(Cek(x-1,y+1,F)&&CekPeta(x-1,y)&&CekPeta(x,y+1)) s++;
  if(Cek(x,y+1,F)) s++;
  if(Cek(x+1,y+1,F)&&CekPeta(x,y+1)&&CekPeta(x+1,y)) s++;
  return s;
}

void CariJalan(int x, int y, int *X, int *Y)  // Ambil satu jalan dari percagangan
{
  int F=Fill[x][y];
  if(Cari(x,y-1,F,X,Y)) return;
  if(Cari(x-1,y,F,X,Y)) return;
  if(Cari(x+1,y,F,X,Y)) return;
  if(Cari(x,y+1,F,X,Y)) return;
  if(CekPeta(x-1,y)&&CekPeta(x,y-1)&&Cari(x-1,y-1,F,X,Y)) return;
  if(CekPeta(x,y-1)&&CekPeta(x-1,y)&&Cari(x+1,y-1,F,X,Y)) return;
  if(CekPeta(x-1,y)&&CekPeta(x,y+1)&&Cari(x-1,y+1,F,X,Y)) return;
  if(CekPeta(x,y+1)&&CekPeta(x+1,y)&&Cari(x+1,y+1,F,X,Y)) return;
}

void TampilPeta();

void LebihKecil(int x, int y, int *X, int *Y)
{
  int b=pgm_read_byte(&(Peta[Ly-1-y][x]));
  if(x>=0&&x<Lx&&y>=0&&y<Ly&&b==1&&Fill[x][y]<Fill[*X][*Y])
  {
    *X=x;
    *Y=y;
    //Serial.print(x);
    //Serial.print(',');
    //Serial.print(y);
    //Serial.print(' ');
    LintasanX[nLintasan]=x;
    LintasanY[nLintasan]=y;
    nLintasan++;
  }
}

void LacakJalan()
{
  Serial.print("StartX=");
  Serial.print(StartX);
  Serial.print(" StartY=");
  Serial.print(StartY);
  Serial.print(" Ke FinishX=");
  Serial.print(FinishX);
  Serial.print(" FinisY=");
  Serial.println(FinishY);
  int x=StartX;
  int y=StartY;
  int Selesai=false;
  LintasanX[0]=StartX;
  LintasanY[0]=StartY;
  nLintasan=1;
  while(!Selesai)
  {
    int X=x; int Y=y;
    int F=Fill[x][y];
      if(CekPeta(x-1,y)&&CekPeta(x,y+1))
        LebihKecil(x-1,y+1,&X,&Y);  // diagonal dulu ?
      if(CekPeta(x,y-1)&&CekPeta(x-1,y))
        LebihKecil(x+1,y-1,&X,&Y);
      if(CekPeta(x-1,y)&&CekPeta(x,y-1))
        LebihKecil(x-1,y-1,&X,&Y);
      if(CekPeta(x,y+1)&&CekPeta(x+1,y))
        LebihKecil(x+1,y+1,&X,&Y);

    LebihKecil(x,y+1,&X,&Y);  // Lurus
    LebihKecil(x,y-1,&X,&Y);
    LebihKecil(x-1,y,&X,&Y);
    LebihKecil(x+1,y,&X,&Y);
    //if(x==X&&y==Y)          // diagonal
    //{
//      LebihKecil(x-1,y+1,&X,&Y);
//      LebihKecil(x+1,y-1,&X,&Y);
//      LebihKecil(x-1,y-1,&X,&Y);
//      LebihKecil(x+1,y+1,&X,&Y);
    //}
    if(x==X&&y==Y) Selesai=true;
    else
    {
      x=X; y=Y;
    }
  }
  for(int i=0;i<nLintasan;i++)
  {
    Serial.print(LintasanX[i]);
    Serial.print(',');
    Serial.print(LintasanY[i]);
    Serial.println();
  }
}

void Jalan()
{
  int x=FinishX;
  int y=FinishY;
  int Selesai=false;
  Fill[x][y]=0;
  while(!Selesai)
  {
    int s;
    int X, Y;
    Serial.print("x=");
    Serial.print(x);
    Serial.print(" y=");
    Serial.println(y);
    if(!(x==StartX&&y==StartY)&&(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;
      x=X; y=Y;
      TampilPeta();
    }
    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
        TampilPeta();
        LacakJalan();
      }
    }
  }
}

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

void TampilPeta()           // Tampilkan Peta dan Fill
{
  for(int y=Ly-1;y>=0;y--)
  {
    for(int x=0;x<Lx;x++)
    {
//      Kotak(x,y,Peta[x][Ly-1-y]==1?ILI9341_RED:ILI9341_BLUE);
      int b=pgm_read_byte(&(Peta[Ly-1-y][x]));
      Serial.print(b);
      Serial.print(' ');
    }
    Serial.print("    ");
    for(int x=0;x<Lx;x++)
    {
      int b=Fill[x][y];
      Serial.print(b);
      Serial.print('\t');
    }
    Serial.println();
  }
  //while (Serial.available()==0); Serial.flush();
}

void RobotJalan()
{
  for(int i=1;i<nLintasan;i++)
  {

  }
  for(int i=nLintasan-1;i>0;i--)
  {

  }
}

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);
//  }
  

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

  Mx=StartX;
  My=StartY;
  Mv=0;
  Ms=90;
  for(int x=0; x<Lx; x++)
    for(int y=0; y<Ly; y++)
      Fill[x][y]=255;
  
  TampilPeta();
  Jalan();
  RobotJalan();
}

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);
}