// bewegungssteuerung.h
#pragma once
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

//#ifdef _DEBUG
#define DEBUG
//#endif

const char *url1="192.168.4.1";
const char *url2="192.168.0.109";
static const char *url=NULL;
static char path[800];
const int portNumber=80;
#define portNumberString "80"

bool wifi_init()
{
 //test ob WLAN eingeschaltet ist und nano-rp2040 erreichbar:
 char pingstring[80];
 sprintf(pingstring,"ping %s -W 5 -c1",url=url1);
#ifdef DEBUG
 printf("pingstring=\"%s\"\n",pingstring);//test
#endif
 int ret=system(pingstring);
 if(ret!=0)
  {
   sprintf(pingstring,"ping %s -W 5 -c1",url=url2);
#ifdef DEBUG
   printf("pingstring=\"%s\"\n",pingstring);//test
#endif
   ret=system(pingstring);
  }
 if(ret!=0)
  {
   printf("ping() --> ret=%d\n",ret);//test
   printf("zuerst WLAN einschalten und mit nano-rp2040 verbinden\n");
   url=NULL;
   return false;
  }
#ifdef DEBUG
 printf("wifi_init() erfolgreich, ip=%s\n",url);//test
#endif
 return true;
}

class SensorData {
public:
 float ax,ay,az; //Beschleunigung
 float gx,gy,gz; //Gyroskop
 float rx,ry,rz; //Rotation
 unsigned long zeit; //Zeit auf dem Controller in Millisekunden seit Start
 SensorData() {ax=ay=az=0; gx=gy=gz=0; rx=ry=rz=0; zeit=0;}
};

int httpclient_start();
int httpclient_read_data(SensorData &data);
void httpclient_stop();

int wifi_read_sensor(SensorData& data) //Rueckgabewert=Fehlernummer, 0=keine Fehler
{
 if(url==NULL && wifi_init()==false) return 1;
 int err=httpclient_start();
 if(err) {printf("httpclient_start() err=%d\n",err); return err;}
 err=httpclient_read_data(data);
 httpclient_stop();
 if(err) {printf("httpclient_read_data() err=%d\n",err); return err;}
 return 0;
}

unsigned int myinet_addr(const char *url)
{
 //auf Zahlen beschraenkt, Vermeidung von <arpa/inet.h>
 unsigned int c1,c2,c3,c4;
 sscanf(url,"%d.%d.%d.%d",&c1,&c2,&c3,&c4);
 return (c4<<24)|(c3<<16)|(c2<<8)|c1;
}

#define BUF_SIZE 1024
static int sockfd;
static char buffer[BUF_SIZE]; 

int get_request(const char *url, const char *port);

int httpclient_start() //bei Erfolg rueckgabe von 0, sonst Fehlernummer
{
 int ret; 
 char http_ok[] = "HTTP/1.0 200 OK";
 
 sockfd = get_request(url, portNumberString);
 memset(&buffer, 0, BUF_SIZE);
 ret = recv(sockfd, buffer, BUF_SIZE, 0);
 if(ret<0) {printf("Error receiving HTTP status!\n"); return 2;}
#ifdef DEBUG
 printf("buffer=\"%s\"\n", buffer);//test
#endif
 if(strstr(buffer, http_ok) == NULL)
  {close(sockfd); printf("Error HTTP status!\n"); return 3;}
 //test send(sockfd, "OK", 2, 0);
 return 0;
}

int pars_data(char *buffer, SensorData &data);

int httpclient_read_data(SensorData &data)
{
 int err=0;
 memset(&buffer, 0, BUF_SIZE);
 int size=BUF_SIZE;
 int ret=0, i,j, imax=5000; //Schlaufenabbruch nach 5000 Millisekunden
 for(i=j=0; (ret = recv(sockfd, &buffer[j], size, 0)) > 0 && i<imax; i++)
  {
   if(strstr(buffer,"\n\r\n")!=NULL || strstr(buffer,"\n\n")!=NULL)
      {break;}//fertig wenn Leerzeile erkannt
   j += ret;
   size -= ret; if(size <= 0) {break;}
   usleep(1000); // 1 Millisekunde warten
  }
 if(i>=imax) {err=1; printf("Fehler: recv() timeout\n");}
 else
  {err=pars_data(buffer, data);
   if(err) printf("Fehler in pars_data() buffer=\n\"%s\"\n",buffer);//test
  }
 send(sockfd, "OK", 2, 0); //? etwas anderes senden falls err!=0 ?
 return err;
}

void httpclient_stop()
{
 close(sockfd);
}

int get_request(const char *url, const char *port)
{
 int sockfd;
 char getrequest[1024];
 struct sockaddr_in addr;
 
 sprintf(getrequest, "GET %s HTTP/1.0\nHOST: %s\n\n", path, url);
#ifdef DEBUG
 printf("getrequest=\"%s\"\n",getrequest);//test
#endif

 // creates a socket to the host
 sockfd = socket(AF_INET, SOCK_STREAM, 0);
 if(sockfd < 0) {printf("Error creating socket!\n"); return 1;}
#ifdef DEBUG
 printf("Socket created...\n");
#endif
 
 memset(&addr, 0, sizeof(addr));  
 addr.sin_family = AF_INET;
 //addr.sin_addr.s_addr = inet_addr(url);
 addr.sin_addr.s_addr = myinet_addr(url);
 addr.sin_port = htons(atoi(port));

#ifdef DEBUG
 printf("starte connect(sockfd ...)\n");//test
#endif
 /** Abbruch wenn Verbindungsaufbau zu lange dauert: **/
 //TODO
 /**/
 //dauert lange wenn keine Verbindung:
 if(connect(sockfd, (struct sockaddr *) &addr, sizeof(addr)) < 0 )
  {printf("Connection Error!\n"); return 1;}
#ifdef DEBUG
 printf("Connection successful...\n\n");
#endif
 // writes the HTTP GET Request to the sockfd
#ifdef DEBUG
 printf("starte write(sockfd, getrequest ...)\n");//test
#endif
 write(sockfd, getrequest, strlen(getrequest));
 return sockfd;
}

int pars_data(char *buffer, SensorData &data)
{
 const char *st;
 st = strstr(buffer,"Tim:");
 if(st!=NULL) sscanf(&st[4],"%ld",&data.zeit); else data.zeit=0;
 st = strstr(buffer,"Acc:"); if(st==NULL) return 1;
 sscanf(&st[4],"%f %f %f\n", &data.ax, &data.ay, &data.az);
 st = strstr(st,"Gyr:"); if(st==NULL) return 2;
 sscanf(&st[4],"%f %f %f\n", &data.gx, &data.gy, &data.gz);
 st = strstr(st,"Rot:"); if(st==NULL) return 3;
 int n=sscanf(&st[4],"%f %f %f\n", &data.rx, &data.ry, &data.rz);
 if(n!=3) {printf("Fehler in pars_data(): fehlende Daten.\n"); return 4;}
 return 0;
}
