#include <stdio.h>
#include <math.h>

#define Normdistance 720.0
#define NormXmax 524.0
#define Xmax 320.0
/* distance between laser and camera in mm */ 
#define las2cam 238.0
 

int i,X[641],Xrel,messcount;
float distance,a,b;
float alpha,gamma,gammamax_,gamma_;
float beta[641];
FILE *dat;

main () {

  gammamax_ = atan ( NormXmax / Normdistance );	// gammamax_ in radians

  if ((dat=fopen("tmp/imgdata/x.line","r+"))==NULL) { perror("fopen"); exit(-1);}
  fscanf(dat,"%d",&messcount);
  printf("x.line (%d):",messcount);
  for(i=0;i<messcount;i++){
    fscanf(dat,"%d",&X[i]);		// laser X position in picture  
    printf("%d ",X[i]);
  }
  fclose(dat);
  printf("\n");
  
  if ((dat=fopen("tmp/imgdata/beta.line","r+"))==NULL) { perror("fopen"); exit(-1);}
  fscanf(dat,"%d",&messcount);
  printf("beta.line (%d):",messcount);
  for(i=0;i<messcount;i++){
    fscanf(dat,"%f",&beta[i]);		// angle at laser motor
    printf("%f ",beta[i]);
  }
  fclose(dat);
  printf("\n distance :");

  
  if ((dat=fopen("tmp/imgdata/dist.line","w+"))==NULL) { perror("fopen"); exit(-1);}
  fprintf(dat,"%d",&messcount);

  for(i=0;i<messcount;i++){
    if(X[i] != -1){
      Xrel = X[i] - 320;		// Laser X coordinate at grabed picture (in pixel)
      beta[i] = beta[i] * (M_PI/180);  // degree -> radians

      gamma_ = ( gammamax_ / Xmax ) * ( (float)Xrel ) ; // gamma_ in radians
      //printf ("gammamax_ = %f,gamma_ = %f \n",gammamax_*180.0/M_PI,gamma_*180.0/M_PI);
      gamma = 90 * (M_PI/180) + gamma_ ;  // in degree
      //printf("gamma = %f \n",gamma*(180/M_PI));
      alpha = 180.0 * (M_PI/180) - gamma - beta[i] ;
      //printf("alpha = %f \n",alpha*(180/M_PI));
      b = ( sin(beta[i]) * las2cam ) / sin(alpha); 
      distance = sin(gamma) * b ;
      printf(" %f" , distance);
      fprintf(dat," %f",distance);

    } else {
      printf(" -1");
      fprintf(dat," -1");
    }
  }
  printf("\n");
  fclose(dat);
}