#include <iostream>
#include <cstdio>
#include "Scena.hh"
#include "Przeszkoda.hh"
#include <unistd.h>
#include <iomanip>
#include <fstream>
#include <ctime>
#include <vector>
#include <memory>

using namespace std;

Scena::Scena ()
{

  Wektor2D P1A,P1C,P2A,P2C,P3A,P3C,P4A,P4C,wekR1,wekR2,wekR3;

  P1A[0]=100; P1A[1]=380;  P1C[0]=50;  P1C[1]=320;
  P2A[0]=300; P2A[1]=390; P2C[0]=200; P2C[1]=200;
  P3A[0]=70;  P3A[1]=70;   P3C[0]=10;  P3C[1]=10;
  P4A[0]=350; P4A[1]=150;  P4C[0]=230;  P4C[1]=50;

  wekR1[0]=100; wekR1[1]=100;
  wekR2[0]=50;  wekR2[1]=200;
  wekR3[0]=150; wekR3[1]=70;


  ObiektGraficzny *R= new Robot(wekR1,0);
  ObiektGraficzny *R2=new Robot(wekR2,280);
  ObiektGraficzny *R3=new Robot(wekR3,140);
  ObiektGraficzny *P= new Przeszkoda (P1A,P1C);//undefined reference Przeszkoda::Przeszkoda(Wektor2D,Wektor2D)
  ObiektGraficzny *P2=new Przeszkoda (P2A,P2C);//undefined reference Przeszkoda::Przeszkoda(Wektor2D,Wektor2D)
  ObiektGraficzny *P3=new Przeszkoda (P3A,P3C);//undefined reference Przeszkoda::Przeszkoda(Wektor2D,Wektor2D)
  ObiektGraficzny *P4=new Przeszkoda (P4A,P4C);//undefined reference Przeszkoda::Przeszkoda(Wektor2D,Wektor2D)
  ObiektGraficzny *S1=new Sciezka(wekR1);
  ObiektGraficzny *S2=new Sciezka(wekR2);
  ObiektGraficzny *S3=new Sciezka(wekR3);
  roboty.push_back(R);
  roboty.push_back(R2);
  roboty.push_back(R3);
  obiekty.push_back(P);
  obiekty.push_back(P2);
  obiekty.push_back(P3);
  obiekty.push_back(P4);
  obiekty.push_back(S1);
  obiekty.push_back(S2);
  obiekty.push_back(S3);
  Lacze.DodajNazwePliku("Robot1.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Robot2.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Robot3.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Przeszkoda1.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Przeszkoda2.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Przeszkoda3.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Przeszkoda4.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Sciezka1.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Sciezka2.dat",PzG::RR_Ciagly,2);
  Lacze.DodajNazwePliku("Sciezka3.dat",PzG::RR_Ciagly,2);
  roboty[0]->PrzykladZapisuWspolrzednychDoPliku("Robot1.dat");
  roboty[1]->PrzykladZapisuWspolrzednychDoPliku("Robot2.dat");
  roboty[2]->PrzykladZapisuWspolrzednychDoPliku("Robot3.dat");
  obiekty[0]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda1.dat");
  obiekty[1]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda2.dat");
  obiekty[2]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda3.dat");
  obiekty[3]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda4.dat");
  obiekty[4]->PrzykladZapisuWspolrzednychDoPliku("Sciezka1.dat");
  obiekty[5]->PrzykladZapisuWspolrzednychDoPliku("Sciezka2.dat");
  obiekty[6]->PrzykladZapisuWspolrzednychDoPliku("Sciezka3.dat");
  Lacze.UstawZakresX(-30, 400);
  Lacze.UstawZakresY(-30, 400);
  Lacze.ZmienTrybRys(PzG::TR_2D);
  Lacze.Rysuj();
}

Scena::~Scena()
{

}

void Scena::Przesun(Wektor2D Wek)
{
  int ind;
  for(ind=0; ind < 7; ind++)
    {
      obiekty[ind]->Przesun()=Wek;
    }
  for(ind=0; ind < 3; ind++)
    {
      roboty[ind]->Przesun()=Wek;
    }

}
void Scena::Rysuj()
{
  roboty[0]->PrzykladZapisuWspolrzednychDoPliku("Robot.dat");
  roboty[1]->PrzykladZapisuWspolrzednychDoPliku("Robot2.dat");
  roboty[2]->PrzykladZapisuWspolrzednychDoPliku("Robot3.dat");
  obiekty[0]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda1.dat");
  obiekty[1]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda2.dat");
  obiekty[2]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda3.dat");
  obiekty[3]->PrzykladZapisuWspolrzednychDoPliku("Przeszkoda4.dat");
  obiekty[4]->PrzykladZapisuWspolrzednychDoPliku("Sciezka1.dat");
  obiekty[5]->PrzykladZapisuWspolrzednychDoPliku("Sciezka2.dat");
  obiekty[6]->PrzykladZapisuWspolrzednychDoPliku("Sciezka3.dat");
  Lacze.Rysuj();
}

ObiektGraficzny* Scena::ZwrocRobota (int ind) const
{
  return roboty[ind];
}

void Scena::AnimacjaJazdy (float droga, int ind)
{
  double kat=roboty[ind]->Kat()*M_PI/180;
  Wektor2D Wek;
  Wek[0]=cos(kat);Wek[1]=sin(kat);
  //Wektor2D Wek(cos(kat),sin(kat));
  float i;
  for (i=0; i< droga; i++)
    {
      roboty[ind]->PrzesunOG(Wek);
      Rysuj();
      Lacze.Rysuj();
      usleep(2000);
    }
}

void Scena::AnimacjaObrotu (double _kat, int ind)
{
  double i;
  for(i=0; i<_kat; i++)
    {
      roboty[ind]->Obroc(1);
      Rysuj();
      Lacze.Rysuj();
      usleep(2000);
    }
}