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

/* 非依存ヘッダ */
#include "common.h"
#include <util/fifo_num.h>
#include <util/conv.h>

/* 依存ヘッダ */
#include <AKI3069.h>
#include <misc.h>
#include <sci.h>
#include <itu.h>
#include <AD7708.h>
#include <gps.h>

/* 変数 */
/* A/D変換対象、加速度(3)、ジャイロ(3)、温度(1) */
#define AD_ACCEL   3
#define AD_GYRO    3
#define AD_TARGET  (AD_ACCEL + AD_GYRO)
#define AD_FREQUENCY 20
int ad_target[AD_TARGET] = {1, 2, 3, 0, 6, 7};
char *ad_label[] = {"Accel_X", "Accel_Y", "Accel_Z", "Gyro_X", "Gyro_Y", "Gyro_Z"};

int ad_tmp = 4;

/* タイマ */
long timer;

/* パケット */

#define GPS_PACKET_SIZE GPS_MAX_LENGTH /* 48 */
/**
 * GPS受信情報をパケットに
 * buffer = NULLのときは内部クリア
 * 
 */
char *gps2packet(char *buffer){
 
  // SCI2(GPS)、GPSデータがやってくる
  static char *buffer_marked = NULL;
  static char *buffer_current;
  
  if(!buffer){buffer_marked = NULL; return NULL;}
  
  char check;
  while(sci2_read(&check, sizeof(check)) > 0){

    if(buffer_marked && ((buffer_current - buffer_marked) < GPS_PACKET_SIZE)){

      *(buffer_current++) = check;
      continue;
    }else if(check == 0xE0){

      sci_dump("GPS");
      buffer_marked = buffer_current = buffer;
      *(buffer_current++) = check;
      buffer += GPS_PACKET_SIZE;
    }
  }
  return buffer;
}

#define AD_PACKET_SIZE (1 + AD_TARGET * sizeof(num_t))
/**
 * AD変換結果をパケットに
 * 
 */
char *ad2packet(char *buffer){
  
  num_t ad;
  
  char *buffer_top = buffer;
  *(buffer++) = 'A';
  int i;
  for(i = 0; i < AD_TARGET; i++){
    while(!fifo_num_read(&fifo_ad7708[ad_target[i]], &ad));
    *(buffer++) = (char)(ad >> 8);
    *(buffer++) = (char)(ad & 0xFF);
  }

  return buffer_top + AD_PACKET_SIZE;
}

/**
 * ADパケットを解く
 * 
 */
static char *dump_ad_packet(char *packet){
  
  char *packet_index = packet;
  if(*(packet_index++) != 'A'){return NULL;}
  
  unsigned int ad;
  char buffer[256];
  char *buffer_limit = buffer;
  int i;
  
  *(buffer_limit++) = 'A';
  *(buffer_limit++) = '\t';
  for(i = 0; i < AD_TARGET; i++){
    ad = *(packet_index++);
    buffer_limit = uint2string((ad << 8) | *(packet_index++), buffer_limit);
    *(buffer_limit++) = '\t';
  }
  buffer_limit = crlf(buffer_limit);
  
  char *buffer_index = buffer;
  while(buffer_index < buffer_limit){
    buffer_index += sci1_write(buffer_index, buffer_limit - buffer_index);
  }
  
  return packet + AD_PACKET_SIZE;
}

/**
 * GPSパケットを解く
 * 
 */
static char *dump_gps_packet(char *packet){
  
  long gps_data[GPS_DATA_KIND];
  int i;
  char buffer[256];
  char *buffer_limit = buffer;
  
 
  *(buffer_limit++) = 'G';
  *(buffer_limit++) = '\t';
  
  // GPSデータが取れたかどうか
  if(gps2value(packet, gps_data) != NULL){
    for(i = 0; i < GPS_DATA_KIND; i++){
      buffer_limit = long2string(gps_data[i], buffer_limit);
      *(buffer_limit++) = '\t';
    }
  }else{
    
    *(buffer_limit++) = 'F';
    *(buffer_limit++) = '\t';
    memcpy(buffer_limit, packet, GPS_PACKET_SIZE);
    buffer_limit += GPS_PACKET_SIZE;
  }
  buffer_limit = crlf(buffer_limit);
  
  char *buffer_index = buffer;
  while(buffer_index < buffer_limit){
    buffer_index += sci1_write(buffer_index, buffer_limit - buffer_index);
  }
  
  return packet + GPS_PACKET_SIZE;
}

/* データプール */
#define STRAGE_PERIOD (60 * 30)
#define STRAGE_SIZE ((GPS_PACKET_SIZE + AD_PACKET_SIZE * AD_FREQUENCY) * STRAGE_PERIOD)
char strage[STRAGE_SIZE];
char *strage_index;

void strage_init(){
  strage_index = strage;
  while(strage_index < strage + STRAGE_SIZE) *(strage_index++) = '\0';
  strage_index = strage;
}

/* 処理ルーチン */

/**
 * インターバルタイマによって呼び出される関数
 * 10ms間隔で呼び出してもらえる
 * itu_init(interval_10ms)のように使う。
 * 
 */
void interval_10ms(){
  
  static int loop = 0;

  if(loop % (100 / AD_FREQUENCY) == 0) ad7708_convert(AD_TARGET, ad_target);

  if(++loop == 100){
    char buffer[16];
    null_char(long2string(++timer, buffer));
    sci_dump(buffer);
    sci_dump("sec\r\n");
    loop = 0;
  }
}

/**
 * インタープリタによる実行
 * 
 */
void run(void (*function)(void)){
 
  // 割り込み禁止
  // SCI0.SCR.BYTE &= 0x0f;  
  // SCI2.SCR.BYTE &= 0x0f;
  
  sci_dump("STRAT\r\n");
  
  function();
  
  char *c = "STOP\r\n";
  char *index = c;
  while(index < c + strlen(c)){
    index += sci1_write(index, c + strlen(c) - index);
  }
}

/**
 * 記録モード
 * 
 */
void record(){
  
  timer = 0;
  itu0_init(interval_10ms);
  
  char check;
  unsigned long loop = 0;
  
  gps_init();
  
  //GPSデータのクリア
  gps2packet(NULL);
  while(sci2_read(&check, sizeof(check)) > 0);

  while(strage_index < strage + STRAGE_SIZE - max(GPS_PACKET_SIZE, AD_PACKET_SIZE)){

    if(sci1_read(&check, sizeof(check)) > 0 && check == 'C'){break;}

    strage_index = gps2packet(strage_index);
    strage_index = ad2packet(strage_index);
    
    if(++loop % 100 == 0){sci_dump("100LOOP,OK\r\n");}
  }
  
  itu0_stop();
}

/**
 * アラン分散用モード
 * 
 */
void allan(){
  timer = 0;
  itu0_init(interval_10ms);
  
  char check;
  
  // 取得
  while(strage_index < strage + STRAGE_SIZE - max(GPS_PACKET_SIZE, AD_PACKET_SIZE)){
    
    if(sci1_read(&check, sizeof(check)) > 0 && check == 'C'){break;}
    
    strage_index = ad2packet(strage_index);
  }
  
  itu0_stop();
}

/**
 * GPSモード
 * 
 */
void gps(){
  
  char check;
  
  gps_init();
  
  //GPSデータのクリア
  gps2packet(NULL);
  while(sci2_read(&check, sizeof(check)) > 0);
  
  // 取得
  while(strage_index < strage + STRAGE_SIZE - max(GPS_PACKET_SIZE, AD_PACKET_SIZE)){
    
    if(sci1_read(&check, sizeof(check)) > 0 && check == 'C'){break;}
    
    strage_index = gps2packet(strage_index);
  }
}

/**
 * ダンプモード
 * 
 */
void dump(){
  
  // 書き出し
  char *index = strage;
  while(index < strage_index){
    
    switch(*index){
      case 'A':   if((index = dump_ad_packet(index)) != NULL) break;
      case 0xE0:  if((index = dump_gps_packet(index)) != NULL) break;
      default: sci_dump("ERROR!\r\n"); index = strage_index;
    }
  }
}

/**
 * テストモード
 * 
 */
void test(){
  
  char buffer[64];
  
  // 加速度、ジャイロ
  int i;
  for(i = 0; i < AD_TARGET; i++){
    sci_dump(ad_label[i]);
    sci_dump(": ");
    null_char(uint2string(ad7708_get(ad_target[i]), buffer));
    sci_dump(buffer);
    sci_dump("\r\n");
  }
  
  // 温度
  sci_dump("TEMP: ");
  null_char(uint2string(ad7708_get(ad_tmp), buffer));
  sci_dump(buffer);
  sci_dump("\r\n");
  
  // バッファ管理
  sci_dump("Buffer margin: ");
  null_char(long2string((long)(strage + STRAGE_SIZE - strage_index), buffer));
  sci_dump(buffer);
  sci_dump("\r\n");
  
  // GPS
  gps_init();
  sci_dump("GPS\r\n");
  gps2packet(NULL);
  while(sci2_read(buffer, sizeof(buffer)) > 0);
  while(buffer == gps2packet(buffer));
  while(buffer == gps2packet(buffer));
  sci1_write(buffer, GPS_MAX_LENGTH);
  sci_dump("\r\n");
  dump_gps_packet(buffer);
}

/**
 * インタープリタ
 * 
 */
void ineterpreter(){

  //それまでの文字をクリア
  char check;
  while(sci1_read(&check, sizeof(check)) > 0);
    
  sci_dump("Data Logger Ver.A by fenrir\r\n");
  sci_dump(" 'D'\tDump stored data\r\n");
  sci_dump(" 'E'\tErase stored data\r\n");
  sci_dump(" 'R'\tRecord start\r\n");
  sci_dump(" 'A'\tADC only mode\r\n");
  sci_dump(" 'G'\tGPS only mode\r\n");
  sci_dump(" 'T'\tTest mode\r\n");
    
  sci_dump("$ ");
  
  char buffer[8];
  char *buffer_index = buffer;
  while(sci1_read(&check, sizeof(check)) == 0);
  *(buffer_index++) = check;
  null_char(crlf(buffer_index));
  sci_dump(buffer);

  switch(check){
    case 'R': case 'r': run(record);      break;
    case 'E': case 'e': run(strage_init); break;
    case 'D': case 'd': run(dump);        break;
    case 'A': case 'a': run(allan);       break;
    case 'G': case 'g': run(gps);       break;
    case 'T': case 't': run(test);        break;
    default: sci_dump("Void OP!!\r\n");
  }
}

/**
 * メイン関数
 * 
 */
int main(){
 
  // Wait
  int i;
  for(i = 0; i < 100000; i++);
 
  // 初期設定
  strage_init();
  sci1_init();
  sci2_init();
  ad7708_init();

  enableIRQ(); /* 割り込み有効 */
  
  while(1) ineterpreter();
 
  disableIRQ(); /* 割り込み無効 */
  
  return 0;
}