/*
	** Creates a synthetic scale weight image video using the real
	** 1D over time scale data and a time<->space mapping supplied
	** in a text file.
	** The scale weight image video can be visualized in a fork of
	** CafeView.
	*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define	MAX_DATA	100000	/* max number of raw data */
#define	ROWS		8	/* size of weight image */
#define	COLS		12	/* size of weight image */

int main (int argc, char *argv[])

{
FILE		*fpt;
int		i,j,k;
int		TotalData;
double		ax,ay,az,yaw,pitch,roll,scale;
double		RawData[MAX_DATA];
int		TotalAreas;
int		TotalCells[10];		/* area */
int		Cells[10][100];		/* area, 1D index */
int		TotalSpans;
int		SpanArea[500];
int		SpanStart[500],SpanEnd[500];
double		ScaleMin,ScaleMax;
unsigned char	ScaleImage[ROWS*COLS];
double		GramsImage[ROWS*COLS];
double		GramsScaled;


if (argc != 3)
  {
  printf("Usage:  sim-scale [datafile.txt] [mapping.txt]\n");
  exit(0);
  }

	/* Read data file.  7 columns separated by spaces.  15 Hz.
	** 7th column is scale, other 6 are ignored. */

if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",argv[1]);
  exit(0);
  }
TotalData=0;
while (1)
  {
  i=fscanf(fpt,"%lf %lf %lf  %lf %lf %lf  %lf",
	&ax,&ay,&az,&yaw,&pitch,&roll,&scale);
  if (i != 7)
    break;
  RawData[TotalData]=scale;
  TotalData++;
  if (TotalData >= MAX_DATA)
    {
    printf("MAX_DATA (%d) exceeded in this file\n",MAX_DATA);
    exit(0);
    }
  }
fclose(fpt);

	/* Because the scale data was oversampled, it sometimes contains
	** a value of 0.0.  This is erroneous.  Search for and replace
	** these with the average of surrounding values. */

for (i=0; i<TotalData; i++)
  if (RawData[i] < 1.0)		/* will be zero, but use < 1.0 for rounding */
    {
    for (j=i; j>=0; j--)
      if (RawData[j] >= 1.0)
        break;
    for (k=i; k<TotalData; k++)
      if (RawData[k] >= 1.0)
        break;
    if (j >= 0  &&  k < TotalData)
      RawData[i]=(RawData[j]+RawData[k])/2.0;
    else if (j >= 0)
      RawData[i]=RawData[j];
    else
      RawData[i]=RawData[k];
    }

	/* Read mapping file.
	** First row is #areas=N.
	** Next N rows are #cells, then cells (1D indices) of each area.
	** Following rows are area start_index end_index for mapping
	** weight changes in that timespan to that area.
	*/

if ((fpt=fopen(argv[2],"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",argv[2]);
  exit(0);
  }
fscanf(fpt,"%d",&TotalAreas);
for (i=0; i<TotalAreas; i++)
  {
  fscanf(fpt,"%d",&(TotalCells[i]));
  for (j=0; j<TotalCells[i]; j++)
    fscanf(fpt,"%d",&(Cells[i][j]));
  }
TotalSpans=0;
while (1)
  {
  i=fscanf(fpt,"%d %d %d",&(SpanArea[TotalSpans]),&(SpanStart[TotalSpans]),
	&(SpanEnd[TotalSpans]));
  if (i != 3)
    break;
  TotalSpans++;
  }
fclose(fpt);

	/*
	** Find min/max of scale data to set 0-255 range of image
	*/

ScaleMin=ScaleMax=RawData[0];
for (j=0; j<TotalData; j++)
  {
  if (RawData[j] < ScaleMin)
    ScaleMin=RawData[j];
  if (RawData[j] > ScaleMax)
    ScaleMax=RawData[j];
  }
printf("grams %lf to %lf\n",ScaleMin,ScaleMax);

	/* init grams image to first weight */
for (i=0; i<ROWS*COLS; i++)
  {
  GramsImage[i]=RawData[0];	/* start unloaded */
  ScaleImage[i]=0;
  }

fpt=fopen("scale.vid","w");
fwrite(ScaleImage,ROWS*COLS,1,fpt);

for (i=1; i<TotalData; i++)
  {
	/* see if the current index is in any span or not */
  for (j=0; j<TotalSpans; j++)
    if (i >= SpanStart[j]  &&  i <= SpanEnd[j])
      break;
  if (j == TotalSpans)
    {	/* not in a span, so distribute weight change across whole image */
    for (k=0; k<ROWS*COLS; k++)
      GramsImage[k]+=(RawData[i]-RawData[i-1]);
    }
  else
    {	/* in a span, only change weight in area */
    for (k=0; k<TotalCells[SpanArea[j]]; k++)
      GramsImage[Cells[SpanArea[j]][k]]+=(RawData[i]-RawData[i-1]);
    }
  for (k=0; k<ROWS*COLS; k++)
    {
    GramsScaled=(GramsImage[k]-ScaleMin)/(ScaleMax-ScaleMin);
    if (GramsScaled <= 0.0)
      ScaleImage[k]=0;
    else if (GramsScaled >= 1.0)
      ScaleImage[k]=255;
    else
      ScaleImage[k]=(unsigned char)(GramsScaled*255.0);
    }
  fwrite(ScaleImage,ROWS*COLS,1,fpt);
  }
fclose(fpt);

}