// Dieses Programm liest seemicro.adam-Logdaten -
// entweder aus einer normalen Datei oder aus einem
// fifo [siehe fifo(4)] und Berechnet mittels alpha
// die Radgeschwindigkeit. Außerdem wird für mitte,
// radius und alpha die Erkennungsrate angezeigt.
// Diese Erkennungsraten basieren auf den Logdaten und sind
// eine untere Grenze für die Erkennungsgenauigkeit.
// Auf wenn ein alpha als ungültig markeirt ist, wird es
// zur Geschwindigkeitsberechnung verwendet!

// Kompilieren mit `gcc logdata2speed.c -o logdata2speed`

#define _GNU_SOURCE
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <math.h>

// for __flbf()
#include <stdio_ext.h>

char defaultFilename[] = "adam.log";

void openStream(FILE **f, char *filename)
{
  printf("Opening '%s'...\n", filename);
  *f = fopen(filename, "r");

  if(*f==NULL)
  {
    perror("Konnte f nicht öffnen");
    exit(1);
  }
  
  printf("Opened f.\n");
  
  // set line buffering mode for this stream
  if(setvbuf(*f, NULL, _IOLBF, BUFSIZ)!=0)
   perror("setvbuf failed. Could not set line buffering mode for input stream");
  
  char bmodes[2][20] = { "non line-buffered", "line-buffered" };
  printf("Buffering mode: %s\n", bmodes[ (__flbf(*f)==0) ? 0 : 1 ]);
}

float framerate = 25;// frames/s
int rememberS = 10;// Aufsummierung der Geschwindigkeit über die letzten rememberS Frames
int rememberR = 100;// Aufsummierung der Reliability über die letzten rememberR Frames
float speed, mitterate, radiusrate, alpharate;

#define MAXALPHA 100
#define MAXVALID 1000

// Ringbuffer
float alphas[MAXALPHA];
int mittevalids[MAXVALID], radiusvalids[MAXVALID], alphavalids[MAXVALID];
int lastA = 0, lastV = 0;// nächster freier Index

void getspeed(float alpha, int mittevalid, int radiusvalid, int alphavalid)
{
  // neue Werte speichern
  alphas[lastA++] = alpha;
  mittevalids[lastV] = mittevalid;
  radiusvalids[lastV] = radiusvalid;
  alphavalids[lastV++] = alphavalid;

  if(lastA>=MAXALPHA) lastA = 0;
  if(lastV>=MAXVALID) lastV = 0;
 
  // Geschwindigkeit ermitteln
  int i = lastA, summanden;
  float prevA;
  for(summanden=0; summanden<rememberS; summanden++)
  {
    i--;
    if(i<0) i = MAXALPHA;
    float diff = alphas[i]-prevA;
    // sonderfall bei ~359 / 1 °?
    if(diff> M_PI) diff -= 2*M_PI;
    if(diff<-M_PI) diff += 2*M_PI;
    if(summanden>0) speed += diff;
    prevA = alphas[i];
  }
  speed /= (rememberS-1);

  // Erkennungsraten ermitteln
  i = lastV;
  int mitteN = 0, radiusN = 0, alphaN = 0;
  for(summanden=0; summanden<rememberR; summanden++)
  {
    i--;
    if(i<0) i = MAXVALID;
    if(mittevalids[i]) mitteN++;
    if(radiusvalids[i]) radiusN++;
    if(alphavalids[i]) alphaN++;
  }
  mitterate = (float)mitteN / (float)rememberR;
  radiusrate = (float)radiusN / (float)rememberR;
  alpharate = (float)alphaN / (float)rememberR;
}

int main(int argc, char **argv)
{
  char *filename;
  if(argc>2)
  {
    fprintf(stderr, "Aufruf mit %s [<logfile>]\nDefault logfile is 'adam.log'\n", argv[0]);
    exit(1);
  }
  if(argc==1) filename = defaultFilename;
  else filename = argv[1];
  
  printf("Framerate=%3.1f Frames/Sekunde\n", framerate);
  printf("Aufsummierung der Geschwindigkeit über die letzten %i Frames.\n", rememberS);
  printf("Aufsummierung der Erkennungsrate über die letzten %i Frames.\n", rememberR);
  
  FILE *f;
  openStream(&f, filename);
  
  char *line = NULL;
  size_t len = 0;
  float alpha, beta;
  int frame, mitteValid, x, y, radiusValid, radius, alphaValid;
  while(1)
  {
    if(feof(f)) continue;
    if(line) { free(line); line = NULL; }
    if(getline(&line, &len, f) ==-1)
    {
      fprintf(stderr, "getline() failed. reopening stream.\n");
      fclose(f);
      openStream(&f, filename);
      continue;
    }
     
    if(!sscanf(line, "frame=%i mittevalid=%i mitte=(%i %i) radiusvalid=%i radius=%i alphavalid=%i alpha=%f beta=%f\n",
	 &frame, &mitteValid, &x, &y, &radiusValid, &radius, &alphaValid, &alpha, &beta))
    {
      fprintf(stderr, "Konnte Zeile nicht verstehen: %s", line);
      continue;
    }
    getspeed(alpha, mitteValid, radiusValid, alphaValid);
    printf("\rAlpha=%3.0f° speed=% 5.1f°/frame=% 6.1f°/s=% 4.1fu/s mittevalid=%3.0f%% radiusvalid=%3.0f%% alphavalid=%3.0f%%",
       alpha*180/M_PI, speed*180/M_PI, speed*180/M_PI*framerate, speed*framerate/2/M_PI,
       mitterate*100, radiusrate*100, alpharate*100);
  }
  fclose(f);
  return 0;
}
