/* SAP emulator */

#include <stdio.h>

/* nasty globalness here - but it works better for things like the SAP anyway... */
#define BUF_SIZE 32
char buffer_in[BUF_SIZE];
char in_index;
char buffer_out[BUF_SIZE];
char out_index;

/* our status holders... */
unsigned char laser = 0x80;
unsigned char display = 0x2d;
struct status_byte {
  unsigned char battery : 1;
  unsigned char temperature : 1;
  unsigned char leg: 1;
};
float Gc[12] = {1.1,1.2,1.3,
		1.4,1.5,1.6,
		1.7,1.8,1.9,
		-0.5,-0.3,-0.2};
float Mc[12] = {1.2,0.8,-1.2,
		1.9,-0.001,0.02,
		1.4,0.8,1.2,
		0,0,0};
int grav_offset = 3;
int mag_offset = 0;
int compass = 90;
int clino = -5;
/* load up sensor data */
#include "sensors.c" 		
int *G = sensor_data+3;
int *M = sensor_data;
struct status_byte status = {0,0,0};

/* utility calls */
unsigned char readHexByte(const char *text, char *result) {
  unsigned char tempa,tempb;
  if (text[0] == '\0') return 0; /* return no error if no arg supplied */
  tempa = text[0] - 0x30;
  if (tempa > 9) tempa -= 7; // A..F
  if (tempa > 15) tempa -= 0x20; // a..f
  if (tempa > 15) return -1; // invalid conversion
  tempb = text[1] - 0x30;
  if (tempb > 9) tempb -= 7;
  if (tempb > 15) tempb -= 0x20; 
  if (tempb > 15) return -1; // invalid conversion
  *result = (tempa << 4) + tempb;
  return 0;
}
      
unsigned char readHexWord(const char *text, signed short *result) {
  unsigned char tempa, tempb;
  if (text[0] == '\0') return 0; /* return no error if no arg supplied */
  if (readHexByte(text,&tempa)) return -1;
  if (readHexByte(text+2,&tempb)) return -1;
  *result = tempa * 256 + tempb;
  return 0;
}

unsigned char readBoolean(const char *text, char *result) {
  if (text[0] == '\0') return 0; /* return no error if no arg supplied */
  if (text[0]=='0') {
    *result = 0;
    return 0;
  }
  if (text[0]=='1') {
    *result = 1;
    return 0;
  }
  return -1;
}


/* these should vary depending on whether this is a SAP or emulator code */  
int get_data() {
  if (fgets(buffer_in + in_index, BUF_SIZE-1-in_index,stdin))
    return strlen(buffer_in) - in_index;
  return 0;
}

void put_data() {
  printf(buffer_out);
  fflush(NULL);
}

char* find_token() {
  char *index = buffer_in;
  while (isalnum(*index)) ++index; /* get to the end of the command word (eg "Read"); */
  while (isspace(*index)) ++index; /* get to next actual thing, either \0 or something interesting */
  return index; 
}

char 
service_command() {
  char retval = -1; /* assume bad transaction until shown good */
  char temp;
  signed short tempw;
  /* vary tasks based on first character of command */
  switch(buffer_in[0]) {
  case 'H':
    retval = 0;
    sprintf(buffer_out,"Hiya");
    break;
  case 'L':
    retval = readHexByte(find_token(),&laser);
    sprintf(buffer_out,"%02hhX",laser);
    break;
  case 'D':
    retval = readHexByte(find_token(),&display);
    sprintf(buffer_out,"%02hhX",display);
    break;
  case 'B':
    temp = status.battery;
    retval = readBoolean(find_token(),&temp);
    if (!retval) status.battery = temp;
    sprintf(buffer_out,"%d",status.battery);
    break;
  case 'T':
    temp = status.temperature;
    retval = readBoolean(find_token(),&temp);
    if (!retval) status.temperature = temp;
    sprintf(buffer_out,"%d",status.temperature);
    break;
  case 'G':
    retval = -1;
    if (buffer_in[1]=='G') {
      retval = 0;
      sprintf(buffer_out,"%04hX %04hX %04hX",G[0],G[1],G[2]);
      G += 6;
    }
    if (buffer_in[1]=='M') {
      retval = 0;
      sprintf(buffer_out,"%04hX %04hX %04hX",M[0],M[1],M[2]);
      M += 6;
    }
    break;
  case 'S':
    retval = -1;
    temp = buffer_in[2] - 0x30;
    if (temp > 9) temp -= 7;
    if (temp > 15) temp -= 0x20;
    if (temp < 16) {
      if (buffer_in[1]=='G') {
	tempw = Gc[temp]*0x4000;
	retval = readHexWord(find_token(), &tempw);
	Gc[temp] = ((float)tempw)/0x4000;
	sprintf(buffer_out,"%04hX",tempw);
      }
      if (buffer_in[1]=='M') {
	tempw = Mc[temp]*0x4000;
	retval = readHexWord(find_token(), &tempw);
	Mc[temp] = ((float)tempw)/0x4000;
	sprintf(buffer_out,"%04hX",tempw);
      }
    }
    break;
  case 'R':
    retval = 0;
    sprintf(buffer_out,"%03d %+02d",compass,clino);
    break;
  }
  return retval;
}

void do_comms() {
  char offset;
  offset = get_data();
  if (offset) {
    /* we've got some new data */
    char* fin = strchr(buffer_in,'\n');
    in_index += offset;
    if (in_index == BUF_SIZE-1) {
      fin = buffer_in + BUF_SIZE-1;
    }
    if (fin) {
      /* buffer is either full or we have a newline. fin points to end of text... */
      *fin = '\0'; /* make sure that our text has newline stripped and zero-terminated*/
      if (service_command()) {
	/* failed read */
	strcat(buffer_out,"\nKO\n");
      } else {
	strcat(buffer_out,"\nOK\n");
	/* successful read */
      }
      put_data();
      in_index = 0;
      buffer_in[0] = '\0';
      buffer_out[0] = '\0';
    }
  }
}

int main() {
  /* set up our buffers */
  buffer_in[0] = '\0';
  buffer_out[0] = '\0';
  in_index = out_index = 0;
  while(!feof(stdin)) {
    do_comms();
    sleep(1);
  }
}