#ifndef MANIPULATOR_HH
#define MANIPULATOR_HH
#include "wektor.hh"
#include "ogniwo.hh"
#include "macierz.hh"

class manipulator
{
  public:
  ogniwo p[100];
  wektor efektor;
  int ilosc_ogniw;
  public:
  //bool policz_poczatki(int);
  int zwroc_ilosc() { return ilosc_ogniw; }
  macierz rq(int);
  wektor translacja(int);
  void wstaw_efektor(wektor p) { efektor = p; }
  ogniwo pobierz(int i) { return p[i]; }
  //void oblicz_wsp(int);
  wektor wsp_punktow(macierz, wektor, wektor);
  wektor punkt(int);
  ogniwo zwroc(int i) { return p[i]; }
  //void wsp_punktow(int);
 // void wstaw(ogniwo, int); 
  bool plik();
  manipulator rowne(manipulator);
};
#endif
  



#include "manipulator.hh"
#include <cmath>
#include <iostream>

using namespace std;
/*
void manipulator::wsp_punktow(int ilosc_przegubow)
{
  wektor punkt, t, pomocnik;
  macierz Rq;
  int i, j;

  p[0].wstaw(punkt);
  for (i = 1; i <= ilosc_przegubow; i++)
  {  
    punkt.zeruj();
    t.zeruj();
    pomocnik.zeruj();
    Rq.zeruj();
    for (j = i-1; i < ilosc_przegubow-1; j++)
    {
      Rq = this->rq(j);
      t = this->translacja(j+1);
      punkt = p[j+1].zwroc_wsp();
      pomocnik = Rq.mac_x_wektor((t + punkt));
    }
    if (i = ilosc_przegubow)
      efektor = pomocnik;
    else
      p[i].wstaw(pomocnik);
  }
}*/  
/*
bool manipulator::policz_poczatki(int ilosc_ogniw)
{
    wektor pop;
    wektor poczatek;
    unsigned int i=1;
    int j;
    double qj;
    double Tj;
    double R[2][2];
    while (i<=ilosc_ogniw)
    {
        poczatek.wstaw_x(0)
        poczatek.wstaw_y(0);
        pop.wstaw_x(0);
        pop.wstaw_y(0);
        j= i-1;
        while (j>=0)
        {
            /*
            cout    << "P " << i << " " << j << " = "
                    << "R(q" << j << ") * ("
                    << "P " << i << " " << j << " + T" << j << ")" << endl;
            *//*
            qj=tab_ogniw[j].zwroc_kat_radiany();
            R[0][0]=cos(qj); R[0][1]=(-sin(qj));
            R[1][0]=sin(qj); R[1][1]=cos(qj);
            Tj=tab_ogniw[j].zwroc_dlugosc();
            poczatek.x=(R[0][0]*(pop.wsp_x+Tj))+(R[0][1]*pop.wsp_y);
            poczatek.y=(R[1][0]*(pop.wsp_x+Tj))+(R[1][1]*pop.wsp_y);
            pop.wsp_x=poczatek.wsp_x;
            pop.wsp_y=poczatek.wsp_y;
            j--;
        }
        tab_ogniw[i].ustaw_poczatek(poczatek);
        i++;
    }
    zapisz_wspolrzedne_do_pliku();
    return true;
}*/
bool manipulator::plik()
{
  ofstream strm;
  double x, y;
  wektor poczatek;
  strm.open("manipulator.dat");
  if(strm.is_open()) 
  {
    strm << "0 0" << endl;
    for(int i = 0; i < ilosc_ogniw; i++) 
    {
      x = poczatek.zwroc_x();
      y = poczatek.zwroc_y(); 
      cout << x << " " << y << " x    y\n";
      poczatek = p[i].zwroc_wsp();
      strm << poczatek.zwroc_x() << '\t' 
           << poczatek.zwroc_y() << endl;
  strm.close();
  cout << "Zapisano ustawienie do pliku!" << endl;
  return true;
  }
  }
  else 
  {
    cerr << "Zapis do pliku sie nie powiodl" << endl; 
    return true;
  }
}
manipulator manipulator::rowne(manipulator M)
{
  manipulator W;
  for (int i = 0; i <= 101; i++)
  {
    W.p[i] = M.p[i];
  }
  W.efektor = M.efektor;
  W.ilosc_ogniw = M.ilosc_ogniw;
  return W;
}