#include "stdio.h"
#include "math.h"

void eingabe(float *);
void status(float*, float*,float*,int );   //Funktionsdeklarationen
float abstand(float*, float*);
void polkart(float* ,  char );
float degrad(float, char);



int main()
{
float w, ist[3], soll[3], greifer[3]={1.0,0.0,0.0};
int i, gr_stat=0;


printf("Roboter Teaching\n\n\n");

printf("koordinaten des gegenstands mit >>/<< bei eingabe trennen!\n\n");

printf("ist-wert: ");              //Werteeingabe

eingabe(ist);

printf("\n\nsoll-wert: ");

eingabe(soll);

status(ist, soll, greifer, gr_stat);



while(gr_stat != -1)
{
printf("\n\nmotor:\n1) vertikal, 2) horiontal, 3) länge, 4) greifer ? ");
scanf("%d", &i);


if(i==1)
{
printf("\nwinkeländerung in deg: ");
scanf("%f",&w);
polkart(greifer,'p');       //kart. greiferko. in polarkord. umrechnen
greifer[2]+=w;              //winkel dazuzählen
polkart(greifer,'k');       //zurück in kart.
}


if(i==2)
{
printf("\nwinkeländerung in deg: ");    //analog
scanf("%f",&w);
polkart(greifer,'p');
greifer[1]+=w;
polkart(greifer,'k');
}


if(i==3)
{
printf("\nlängenänderung: ");        //analog
scanf("%f",&w);
polkart(greifer,'p');
greifer[0]+=w;
polkart(greifer,'k');
}


if(i==4)
{
if(gr_stat)         //prüfen ob greifer am ziel
{
	if(abstand(soll, greifer)<0.01)
		{printf("\n\ngegenstand am ziel!"); gr_stat= -1; continue;}
	else printf("\n\nsoll ich den gegenstand wirklich auf den boden schmeißen, alte pfeife ;-)");

}

if(abstand(ist, greifer)<0.01)gr_stat = 1;      //greifer schließen
else printf("\n\ngegenstand nicht in reichweite");
}                                                //wenn kleiner 0.01



status(ist, soll, greifer, gr_stat);
}



}  //Ende main



//******************Funktionsdefinitionen*******************************


void eingabe(float *feld)
{
char eingabe[40], aux[40];
int i=0, d;

scanf("%s", &eingabe);

while(eingabe[i]!='/')    //erster teil
{
aux[i]=eingabe[i];
i++;
}
aux[i]='\0';
feld[0]= atof(aux);    //wandelt ascii in float um
d=++i;
while(eingabe[i]!='/')       //zweiter teil
{
aux[i-d]=eingabe[i];
i++;
}
aux[i-d]='\0';
feld[1]=atof(aux);
d=++i;
while(eingabe[i]!='/')        //dreitter teil
{
aux[i-d]=eingabe[i];
i++;
}
aux[i-d]='\0';
feld[2]=atof(aux);
}


void status(float* ist, float* soll, float* greifer, int gr_stat)  //Zeiger
{                                                           //auf ko-felder
printf("\n\nist:\t\t%f %f %f",ist[0],ist[1],ist[2]);
printf("\nsoll:\t\t%f %f %f",soll[0],soll[1],soll[2]);
printf("\ngreifer:\t%f %f %f",greifer[0],greifer[1],greifer[2]);
if(gr_stat)printf("\ngeschlossen");
else printf("\noffen");
}


float abstand(float*a,float*b)     //abstand zweier punkte
{
return fabs(sqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) +
	(a[2]-b[2])*(a[2]-b[2]) ) );
}

void polkart(float* hin,  char s)
{

int i;

float rueck[3];  //hilfsfeld, rueckgabe erfolgt über pointer auf "hin"

if(s=='p')        //in polarkort
{
	 float aux[3]={0.0,0.0,0.0};
	 rueck[0] = abstand(hin, aux);   //abstand zum ursprung
	 rueck[1] = degrad(atan(hin[1]/hin[0]), 'd');      //phi + umrechnung in deg
	 rueck[2] = degrad(asin(hin[2]/rueck[0]), 'd');    //eta + umrechnung in deg
}
else           //in kart
{
	hin[1]=degrad(hin[1], 'r');     //in rad
	hin[2]=degrad(hin[2], 'r');     //in rad
	rueck[0] = hin[0]*cos(hin[2])*cos(hin[1]);
	rueck[1] = hin[0]*cos(hin[2])*sin(hin[1]);
	rueck[2] = hin[0]*sin(hin[2]);
}
for(i=0; i<=2; i++)hin[i]=rueck[i];

}


float degrad(float x,  char s)
{

if(s=='d')
{
	return x/3.1415927*180;        //in deg umrechnen
}
else
{
	return x*3.1415927/180;        //in rad umrechnen
}

}