#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <string>
#include <stdexcept>

class Pitot3 {
  
  static const int hoge = 10;

  public:
    struct Parameters {
      struct {
        double Cv, C23, Ct1, Ct2, rho, delay;
        bool use_lpf;
      } tube;
      struct {
        int C, S1, S2, A;
        int C_sign, S1_sign, S2_sign;
      } connection;
      bool has_alt_pressure;
      double sensitivity[4];
      double offset_calc_start;
      double t_start;
    };
  
  protected:
    Parameters params;
    typedef std::string string;
    typedef std::stringstream stringstream;
    
  public:
    Pitot3() : params() {
      // Parameters default setting
      params.has_alt_pressure = false;
      params.offset_calc_start = 0;
      params.tube.delay = 0;
      params.tube.use_lpf = true;
    }
    ~Pitot3(){}
    
    double &offset_calc_start() {
      return params.offset_calc_start;
    }
    
    double &t_start() {
      return params.t_start;
    }
    
    void read_config(std::istream &in) throw (std::exception) {
      
      char buf[256];
      int line_count(0);
      string label;
      
      // 行ごとに読み込んでいく
      while(!in.eof()){
        in.getline(buf, sizeof(buf));
        line_count++;
        
        stringstream ss;
        ss << buf; // 行で入力ストリームを作る
        
        string key;
        double value;
        
        ss >> key;
        ss >> value;
        
        
        if(!ss.fail()){
          // 行に名前と数字があった場合はパラメータがある行
          std::cerr << "found key:value @ " << line_count << ", "
              << key << " => " << value << std::endl;
          
          // 辞書に基づいてパラメータを設定する
          bool found_target(true);
          if(label == "Pitot_tube_parameters"){
            do{
              if(key == "Cv"){
                params.tube.Cv = value;
                break;
              }else if(key == "C23"){
                params.tube.C23 = value;
                break;
              }else if(key == "Ct1"){
                params.tube.Ct1 = value;
                break;
              }else if(key == "Ct2"){
                params.tube.Ct2 = value;
                break;
              }else if(key == "rho"){
                params.tube.rho = value;
                break;
              }else if(key == "delay"){
                params.tube.delay = value;
                break;
              }else if(key == "use_lpf"){
                params.tube.use_lpf = (value != 0);
                break;
              }
              found_target = false;
            }while(false);
          }else if(label == "Pitot_tube_Pressure_sensor_connection"){
            int abs_value(abs((int)value)), sign(value > 0 ? 1 : -1);
            if(abs_value > 4){
              stringstream what;
              what << "Index must be under 4 !! : "
                  << key << " _of_ "
                  << label << " _is_ "
                  << abs_value;
              throw std::runtime_error(what.str());
            }
            do{
              if(key == "C"){
                params.connection.C = abs_value - 1;
                params.connection.C_sign = sign;
                break;
              }else if(key == "S1"){
                params.connection.S1 = abs_value - 1;
                params.connection.S1_sign = sign;
                break;
              }else if(key == "S2"){
                params.connection.S2 = abs_value - 1;
                params.connection.S2_sign = sign;
                break;
              }else if(key == "A"){
                params.connection.A = abs_value - 1;
                params.has_alt_pressure = true;
                break;
              }
              found_target = false;
            }while(false);
          }else if(label == "Pressure_sensor_sensitivity"){
            do{
              if(key == "S1"){
                params.sensitivity[0] = value;
                break;
              }else if(key == "S2"){
                params.sensitivity[1] = value;
                break;
              }else if(key == "S3"){
                params.sensitivity[2] = value;
                break;
              }else if(key == "S4"){
                params.sensitivity[3] = value;
                break;
              }
              found_target = false;
            }while(false);
          }else if(label == "Analysis_start"){
            do{
              if(key == "T"){
                params.t_start = value;
                break;
              }
              found_target = false;
            }while(false);
          }
          
          if(!found_target){
            //std::cerr << key << "," << label << std::endl; 
            throw std::runtime_error(
                string("Unknown option !! : ")
                    .append(key).append(" _of_ ")
                    .append(label));
          }
        }else if(key.length()){
          label = key;
          // ない場合は設定対象項目の選定
          std::cerr << "found label @ " << line_count << ", "
              << label << std::endl;
        }
      }
    }
    
    struct input_t {
      bool analyzing;
      double time;
      double raw[4];
      double offset[4];
    };
    
    struct output_t {
      double time;
      double aspd, angle1, angle2, alt;
    };
    
    bool analyze_1step(input_t &input, output_t &output){
      double press[4];
      
      if(input.time < params.offset_calc_start){
        return false;
      }
      
      if(input.time < params.t_start){
        if(params.tube.use_lpf){
          for(int i(0); i < 4; i++){  // LPFあり
            input.offset[i] = (input.offset[i] * 9 + input.raw[i]) / 10;
          }
        }else{
          for(int i(0); i < 4; i++){  // LPFなし
            input.offset[i] = input.raw[i];
          }
        }
        return false;
      }
      
      if(!input.analyzing){
        input.analyzing = true;
        std::cerr << "Offset: " << input.offset[0];
        for(int i(1); i < 4; i++){
          std::cerr << ", " << input.offset[i];
        }
        std::cerr << std::endl;
      }
      
      // 時刻の計算
      output.time = input.time - params.tube.delay;
      
      // 圧力の計算
      // 差圧か絶対圧かで場合分け
      for(int i(0); i < 4; i++){
        if(params.has_alt_pressure && (i == params.connection.A)){
          press[i] 
              = params.sensitivity[i] 
                  * (input.raw[i] - input.offset[i]) * (5000.0 / 2097151);
        }else{
          press[i] 
              = params.sensitivity[i] 
                  * (input.raw[i] - input.offset[i]) * (5000.0 / 65535);
        }
      }
      
      // 対気速度の計算
      double src_aspd(press[params.connection.C] * params.connection.C_sign);
      output.aspd = 0;
      if(src_aspd > 0){
        output.aspd = sqrt(2. * src_aspd / (params.tube.rho * (params.tube.Cv + params.tube.C23)));
      }
      
      // 迎角、横滑り角の計算
      // 機速があまりにも遅いときははじく
      output.angle1 = 0;
      output.angle2 = 0;
      if(output.aspd > 5){
        output.angle1 
            = press[params.connection.S1] * params.connection.S1_sign / src_aspd / params.tube.Ct1;
        output.angle2
            = press[params.connection.S2] * params.connection.S2_sign / src_aspd / params.tube.Ct2;
      }
      
      // 高度
      output.alt = 0;
      if(params.has_alt_pressure){
        output.alt = press[params.connection.A];
      }
      
      return input.analyzing;
    }

    void analyze(std::istream &in, std::ostream &out){
      input_t input;
      output_t output;
      char  buf[256];
      int line_count(0);
      
      for(int i(0); i < sizeof(input.offset) / sizeof(input.offset[0]); i++){
        input.offset[i] = 0;
      }
      input.analyzing = false;
      
      out << std::setprecision(10);
      
      std::cerr << std::endl << "analyzing";
      
      while(!in.eof()){
        in.getline(buf, sizeof(buf));
        
        if((++line_count) % 100 == 0){
          std::cerr << ".";
        }
        
        stringstream ss;
        ss << buf;
        
        bool read_complete(true);
        double value;
        string delimiter;
        for(int i(0); i < 6; i++){
          // "値, "のパターンが区切り
          ss >> value;
          if((i < 5) && !ss.good()){
            read_complete = false;
            break;
          }
          switch(i){
            case 0:
              input.time = value;
              break;
            case 1:
              input.time += value * 0.02;
              break;
            default:
              input.raw[i - 2] = value;
          }
          ss >> delimiter;
        }
        if(!read_complete){continue;}
        
        if(!analyze_1step(input, output)){continue;}
        
        // 出力
        out << output.time << "," 
            << output.aspd << "," 
            << output.angle1 << "," << output.angle2 << "," 
            << output.alt << std::endl;
      }
      
      std::cerr << " done." << std::endl;
    }
};

#ifndef USE_AS_HEADER

using namespace std;

int main(int argc, char *argv[]){
  
  if((argc < 3) || (argc > 4)){
    cerr << " Usage: " << argv[0] 
        << " data_filename configuration_filename [output_filename]" 
        << endl;
    cerr << "(if filename = \"-\" of (data/output) then (cin/cout) is used)" << endl;
    return -1;
  }
  
  string input_fn(argv[1]), config_fn(argv[2]), output_fn("output.csv");
  if(argc == 4){
    output_fn = string(argv[3]); 
  }
  
  istream *in;
  bool in_is_fstream(true);
  if(input_fn == "-"){
    in = &cin;
    input_fn = "[cin]";
    in_is_fstream = false;
  }else{
    in = new ifstream(input_fn.c_str());
  }
  ostream *out;
  bool out_is_fstream(true);
  if(output_fn == "-"){
    out = &cout;
    output_fn = "[cout]";
    out_is_fstream = false;
  }else{
    out = new ofstream(output_fn.c_str());
  }
  
  cerr << "Data file name: " << input_fn << endl
      << "Configuration filename: " << config_fn << endl
      << "Output filename: " << output_fn << endl;
  
  int return_code(-1);
  do{
    if(in->fail()){
      cerr << "File open error - " << input_fn << endl;
      break;
    }
    if(out->fail()){
      cerr << "File open error - " << output_fn << endl;
      break;
    }
    
    fstream config(config_fn.c_str());
    if(config.fail()){
      cerr << "File open error - " << config_fn << endl;
      break;
    }
    
    Pitot3 pitot3;
    
    cerr << "opeining files complete." << endl;
    try{
      pitot3.read_config(config);
    }catch(exception &e){
      cerr << "Configuration file read error" << endl;
      cerr << e.what() << endl;
      break;
    }
    
    pitot3.analyze(*in, *out);
    
    return_code = 0;
  }while(false);
  
  if(in_is_fstream){
    static_cast<ifstream *>(in)->close();
    delete in;
  }
  if(out_is_fstream){
    static_cast<ofstream *>(out)->close();
    delete out;
  }
  
  return return_code;
}
#endif