#include "pch.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <cmath>
#include <math.h>
#include <iomanip>


const int wym = 20;
using namespace std;

struct n_p {
	int x, y;
}npoz;

//funkcja sprawdza mozliwy ruch po wczytanej tablicy od wskazanych wsp.
bool possible_motion(int x, int y, double **Tablica, double **TO, double**TR) {
	int poz_x = x;
	int poz_y = y;
	int end_x = 19, end_y = 19;
	double heurystyka =0;
	int kosztRuchu = 1; //x + y;
	double **Tablica1 = Tablica;
	double **Otwarta=TO;
	double **Rodzic = TR;
	Rodzic[poz_x][poz_y] = 3;


	if (poz_x - 1 >= 0) {
		if (Tablica1[poz_x - 1][poz_y] == 0 && Tablica1[poz_x - 1][poz_y] != 5 && Otwarta[poz_x-1][poz_y ] == 0) {		//sprawdz DO GÓRY
			heurystyka = kosztRuchu + sqrt((pow((poz_x - 1 - end_x), 2) + (pow((poz_y - end_y), 2))));
			Otwarta[poz_x - 1][poz_y] = heurystyka;
		}
	}
	if (poz_x + 1 < wym) {
		if (Tablica1[poz_x + 1][poz_y] == 0 && Tablica1[poz_x + 1][poz_y] != 5 && Otwarta[poz_x+1][poz_y ] == 0) {		//sprawdz DO DOŁU
			heurystyka = kosztRuchu + sqrt((pow((poz_x + 1 - end_x), 2) + (pow((poz_y - end_y), 2))));
			Otwarta[poz_x + 1][poz_y] = heurystyka;
		}
	}
	if (poz_y - 1 >= 0) {
		if (Tablica1[poz_x][poz_y - 1] == 0  && Tablica1[poz_x][poz_y - 1] != 5 && Otwarta[poz_x][poz_y - 1] == 0) {		//sprawdz W LEWO
			heurystyka = kosztRuchu + sqrt((pow((poz_x - end_x), 2) + (pow((poz_y - 1 - end_y), 2))));
			Otwarta[poz_x][poz_y - 1] = heurystyka;
		}
	}
	if (poz_y + 1 < wym) {
		if (Tablica1[poz_x][poz_y + 1] == 0 && Tablica1[poz_x][poz_y + 1] != 5 && Otwarta[poz_x][poz_y + 1] == 0) {		//sprawdz W PRAWO
			heurystyka = kosztRuchu + sqrt((pow((poz_x - end_x), 2) + (pow((poz_y + 1 - end_y), 2))));
			Otwarta[poz_x][poz_y + 1] = heurystyka;
		}
	}
	//blokuje, zwracajac false, gdy dalsze wyznaczanie heurystyki niemozliwe
	double temp=0;
	double temp1 = 0;
	if (temp!=heurystyka) {
		if (heurystyka != 0) temp = heurystyka;
		return true;
	}
	else{
		temp1++;
		if (temp1 >=10) return false;
	}
	return true;
}
//funkcja wyznacza najmniejsza heurystyke pola w tablicy otwartej, oraz wskazuje jej wsp.
void minimum(double **Otwarta, double **Rodzic) {
	double **TO = Otwarta;
	double **TR = Rodzic;
	double min = 40;
	int npoz_x=20, npoz_y=20;

	for (int i = 0; i < wym; i++) {
		for (int j = 0; j < wym; j++) {
			if (min > TO[i][j] && TO[i][j] != 0 && TR[i][j] != 3) {
				min = TO[i][j];
				npoz_x = i;
				npoz_y = j;
			}
		}
	}
	if(npoz_x!=20 ){
	npoz.x = npoz_x;
	npoz.y = npoz_y;
	}
}
//funkcja pobiera tablice rodzica i otwarta, na ich podstawie wyznacza najkrotsza trase do celu
void wyznaczNajkrotsza(double **Otwarta, double **Rodzic, double **Zamknieta) {
	double **TO = Otwarta;
	double **TZ = Zamknieta;
	double **TR = Rodzic;
	double max = 0;
	double min = 40;


	cout << "TRASA OD CELU DO STARTU:\n";


	for (int i = 19; i >=0; i--) {
		for (int j = 19; j >=0; j--) {
			if (max < TO[i][j] && TO[i][j] != 0 && TR[i][j] == 3 && TO[i][j] < max + 1.26) {
				max = TO[i][j];
				TZ[i][j] = 3;
				printf("-> (%d,%d) ", i, j);
			}
		}
	}
}


int main()
{
	cout << "Wczytuje tablice z pliku";
	string nazwap = "grid.txt";

	//deklaruje tablice
	double **Tablica;
	double **TZ;
	double **TO;
	double **TR;


	Tablica = new double *[wym];
	for (int i = 0; i < wym; i++)
		Tablica[i] = new double[wym];

	TZ = new double *[wym];
	for (int i = 0; i < wym; i++)
		TZ[i] = new double[wym];

	TO = new double *[wym];
	for (int i = 0; i < wym; i++)
		TO[i] = new double[wym];

	TR = new double *[wym];
	for (int i = 0; i < wym; i++)
		TR[i] = new double[wym];

	std::ifstream plik(nazwap.c_str());
	//przygotowuję pustą tablicę Główną
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			plik >> Tablica[i][j];
		}
	}
	plik.close();

	//przygotowuję pustą tablicę Zamknieta
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			TZ[i][j] = Tablica[i][j];
		}
	}
	TZ[0][0] = 3;

	//przygotowuję pustą tablicę Rodzic
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			TR[i][j] = 0;
		}
	}

	//przygotowuję pustą tablicę Otwartą
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			TO[i][j] = 0;
		}
	}
	//--------------------------------------------------------------------------------------------------------------------------------

	cout << "\nWypisujemy na ekran nasza tablice z przeszkodami\n\n";
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			cout << " " << Tablica[i][j];
		}cout << "\n";
	}
	//====================================================================================================================================

	bool sukces;
	sukces=possible_motion(0, 0, Tablica, TO, TR);
	minimum(TO,TR);

		do
		{

			sukces = possible_motion(npoz.x, npoz.y, Tablica, TO, TR);
			minimum(TO, TR);
			if (sukces == false) break;

		} while (TR[19][19]==0 );




	cout << "\nWypisujemy na ekran nasza tablice otwarta\n\n";
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j <wym; j++)
		{
			cout <<"  " << setprecision(3) << TO[i][j];
		}cout  << "\n " /*<< i << "  "*/;
	}

	cout << "\nWypisujemy na ekran nasza tablice rodzic\n\n";
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			cout << "  " << TR[i][j];
		}cout << "\n ";
	}

	wyznaczNajkrotsza(TO, TR, TZ);	//<-wyznaczam najkrótszą scieżkę do celu !!!!

	cout << "\n\nWypisujemy na ekran nasza tablice zamknieta z wyznaczona trasa i przeszkodami\n\n";
	for (int i = 0; i < wym; i++)
	{
		for (int j = 0; j < wym; j++)
		{
			cout << "  " << TZ[i][j];
		}cout << "\n ";
	}
	if (TR[19][19] == 0)cout << "NIE MOZNA DOTRZEC DO CELU";

	//--------------------------------------------------------------------------------------------------------------------------------
	//czyścimy pamięć po naszej tablicy
	for (int i = 0; i < wym ; i++)
	{
		delete[] Tablica[i];
	}
	delete[] Tablica;

	for (int i = 0; i < wym; i++)
	{
		delete[] TZ[i];
	}
	delete[] TZ;

	for (int i = 0; i < wym; i++)
	{
		delete[] TO[i];
	}
	delete[] TO;

	for (int i = 0; i < wym; i++)
	{
		delete[] TR[i];
	}
	delete[] TR;

	cout << "\n\nNacisnij ENTER aby zakonczyc";
	getchar();

	return 0;

}