/*    Sontek Data Download Program SONTEK.C */

#include "sontek.h"
#include "cport.h"
#include <stdio.h>
#include <conio.h>
#include <dos.h>
#include <math.h>

/* DEFINITIONS */

COM 	comport;


/* Global Variables */

LongSample	samples[10];

union {
  LongSample 		sample_struct;
  unsigned char		sample_buf[33];
} sample_union;

unsigned g_num_samples = 0;


/* Function Prototypes */
void log_data(void);
void download(void);
void display(void);

/******************************************/

void main()
{
  char ch, inkey;
  int x;

  comport = ComOpen(COM1, B9600, W8|S1|NONE, 1024, 512);
  if(comport == NULL){
    printf("\nError opening comport...exiting program");
    exit(0);
  }

  while(1){

    printf("\n\n\nSontek Data Download Menu\n\n");
    printf("1 - Log Data\n");
    printf("2 - Download Data\n");
    printf("3 - Display\n");
    printf("9 - Quit\n\n");

    inkey = getch();
    switch(inkey){
      case '1': log_data();
	break;
      case '2': download();
	break;
      case '3': display();
	break;
      case '9':
	ComClose(comport);
	exit(0);
    }
  }
}

/***************************************/

void log_data()
{
  int x;
  char char_buf[80];

  printf("\nErasing memory...wait 2 min.");
  for(x=0;x<5;x++){
   ComPutc(comport, '+');
   delay(300);
  }
  ComPuts(comport, "\r");
  delay(2000);
  ComPuts(comport, "format\r");
  delay(1000);
  ComPuts(comport, "yes\r");
  for(x=0;x<120;x++){
    delay(1000);
    ComGets(comport, char_buf, 80, '\n');
    printf("\nchar_buf: %s", char_buf);
  }
  ComPuts(comport, "deploy\r");
  printf("\nLogging data...");
}

/****************************************/

void download()
{
  char char_buf[80];
  unsigned char byte;
  unsigned x, y, z, sample_num;

  printf("\nDownloading...");
  for(x=0;x<5;x++){
   ComPutc(comport, '+');
   delay(300);
  }
  ComPuts(comport, "\r");
  delay(2000);
  ComPuts(comport, "OD DEF001\r");
  delay(500);
  ComFlushRx(comport);
  ComPuts(comport, "NS\r");
  delay(500);
  ComGets(comport, char_buf, 80, '\n');
  ComGets(comport, char_buf, 80, '\n');
  printf("\nchar_buf: %s", char_buf);
  sscanf(char_buf, "Number of samples in file is %d.", &g_num_samples);
  printf("\nNumber of samples is: %d", g_num_samples);

  for(sample_num=0;sample_num<g_num_samples;sample_num++){
    ComFlushRx(comport);
    ComPuts(comport, "RSB 1\r");
    delay(200);
    y = 10;
    while(1){                  //Search for the sync character
      byte = ComGetc(comport);
      if(byte == 0xB0){
//	printf("\nsync_char");
	sample_union.sample_buf[0] = byte;
	ComIn(comport, &sample_union.sample_buf[1], 32);
	samples[sample_num] = sample_union.sample_struct;
	break;
      }
      y--;
      if(y == 0){
	printf("\nNo sync character found.");
	break;
      }
    }
    printf("\n");
    for(z=0;z<33;z++) printf("%02X ", sample_union.sample_buf[z]);
  }
}

/*******************************************/

void display(void){
  unsigned x;
  double double_speed;
  int speed, angle;

  for(x=0;x<g_num_samples;x++){
    printf("\n\n%d  East: %d", x, samples[x].Vel[0]);
    printf("\n%d  North: %d", x, samples[x].Vel[1]);
    printf("\n%d  Up: %d", x, samples[x].Vel[2]);

    double_speed = sqrt( (samples[x].Vel[0]*samples[x].Vel[0] + samples[x].Vel[1]*samples[x].Vel[1]) );
    if (double_speed == 0) double_speed = 1;      // Avoid divde by zero error
    speed = double_speed;
    printf("\n%d  Speed: %d", x, speed);

    angle = (acos(samples[x].Vel[1]/double_speed)/3.1415)*180;
    if(samples[x].Vel[0] < 0){
      angle += 180;
      angle %= 360;
    }
    printf("\n%d  Angle: %d", x, angle);
  }

}