// C++ headers
#include <string>           // String
#include <iostream>         // String streams
#include <fstream>          // File operations
#include <unordered_map>

#include <Obs_Calibration_Vector.hpp>
#include <stdio.h>

Obs_Calibration_Vector::Obs_Calibration_Vector(double offset, double scale){

	for (int i=0;i<3;i++){
		this->offset[i]=0.0;
		this->scale[i]=1.0;
	}

	this->offset[0] = offset;
	this->scale[0]  = scale;

	if (offset==0 && scale==1){
		do_cal=0;
	} else {
		do_cal=1;
	}

}

Obs_Calibration_Vector::Obs_Calibration_Vector(double* offset, double* scale){

	for (int i=0;i<3;i++){
		this->offset[i]=offset[i];
		this->scale[i]=scale[i];
		if (do_cal==0 && (offset[i]!=0 || scale[i]!=1)){
			do_cal=1;
		}
	}

}

Obs_Calibration_Vector::Obs_Calibration_Vector(
		double scale_X, double scale_Y, double scale_Z,
        double offset_X, double offset_Y, double offset_Z){

	do_cal=0;

	this->scale[0]=scale_X;
	this->scale[1]=scale_Y;
	this->scale[2]=scale_Z;
	this->offset[0]=offset_X;
	this->offset[1]=offset_Y;
	this->offset[2]=offset_Z;

	for (int i=0; i<3; i++){
		if (do_cal==0 && (offset[i]!=0 || scale[i]!=1)){
			do_cal=1;
			break;
		}
	}

}

/****************************************************************************
 ****************************************************************************
 *																			*
 * 				PUBLIC METHODS												*
 *																			*
 ****************************************************************************
 ***************************************************************************/

/****************************************************************************
 *                                                                                                                                                      *
 * Calibrate vector data                                                                                                        *
 *                                                                                                                                                      *
 ***************************************************************************/
int Obs_Calibration_Vector::calibrate(data_obs_vector* vdata){

        if (do_cal){

                double data[5];

                vdata->get_data(data);

                for (int i=0;i<3;i++){
                        //printf("SCALE: %f ",data[i]);
                        data[i]=data[i]*scale[i]+offset[i];
                        //printf("%f %f\n",data[i],scale[i]);
                }

                vdata->set_data(data);

        }

        return(0);



}


/****************************************************************************
 *                                                                                                                                                      *
 * Calibrate vector data                                                                                                        *
 *                                                                                                                                                      *
 ***************************************************************************/
int Obs_Calibration_Vector::calibrate(data_obs_scalar* sdata){

        if (do_cal){

                double data;

                data=sdata->get_value();

                data=data*scale[0]+offset[0];

                sdata->set_value(data);

        }

        return(0);

}




/****************************************************************************
 *																		 	*
 * Calibration constants for the ADC													*
 *																			*
 ***************************************************************************/
int Obs_Calibration_Vector::get_adc_calibrate(std::string config,
        std::string &offset, std::string &scale){

    std::unordered_map<std::string, struct adc_cal>::iterator it;
    struct adc_cal cal;
    
    it = map_ADC.find(config);
    
    if (it != map_ADC.end()) {
        cal     = it->second;
        scale   = cal.scale;
        offset  = cal.offset;
    } else {
        scale  = "";
        offset = "";
        return(-1);
    }
    
    return(0);

}

/****************************************************************************
 *																		 	*
 * Calibration constants for the ADC													*
 *																			*
 ***************************************************************************/
int Obs_Calibration_Vector::add_adc_calibrate(std::string config, 
        std::string offset, std::string scale){

    struct adc_cal cal;
    
    cal.offset = offset;
    cal.scale  = scale;
    
    // Check if already exists
    
    if (map_ADC.count(config)>0) {
        map_ADC[config]=cal;
    } else {
        std::cerr << "Calibration for " << config << " overwritten.";
    }

    return(0);

}

/****************************************************************************
 *
 * Save the calibration constants to file
 *
 ***************************************************************************/
 int Obs_Calibration_Vector::save_adc_calibrate(std::string filename) {
     
     /* PROBLEM: Save the comments in ADC file when new configurations are
      * stored. Cannot simply append, as duplicates need to be treated.
      * SOLUTION: Save whole file in string before writing. Then, rewrite
      * file. Comment out duplicates instead of overwriting.
      */
     
    std::ifstream  file_buf(filename);
    std::ofstream  file_out;
    std::string    line, config, comment;
    
    size_t pos_c;
    
    struct adc_cal cal;
     
    // Copy the hash map
    // The copy will be used to track what has been written to file
    std::unordered_map<std::string, struct adc_cal> map_ADC_cpy(map_ADC);
     
    // Open file as new blank file
    file_out.open(filename,std::ios::out | std::ios::trunc);
    
    // Read from buffered file
      while ( getline (file_buf,line) )  {
          
        // Parse from line
          
        if (parse_adc_calibration(line,config,cal,comment) < 0){
        
            file_out << line << std::endl;
        
        } else {
            
            // Configuration exists and is overwritten
            if (map_ADC_cpy.count(config)>0) {
                file_out << "#" << line << std::endl;
                cal=map_ADC_cpy[config];
                file_out << config << "," << cal.offset << "," << 
                            cal.scale << ";" << comment << std::endl;

            // Configuration doesn't exist
            } else {
                file_out << line << std::endl;
            }
        
        }
        
      }
    
     file_out.close();
    
     
     
     return(0);
 }
 
 int Obs_Calibration_Vector::save_adc_calibrate() {
 
     if (file_ADC.empty()) return(-1);
     
     return(save_adc_calibrate(file_ADC));
     
 }
        
/****************************************************************************
 *
 * Read the calibration constants to file
 *
 ***************************************************************************/
 int Obs_Calibration_Vector::read_adc_calibrate(std::string filename) {
     
    std::ifstream  file;
    std::string    line;
    
    std::string     config; // configuration string used as key
    struct adc_cal  cal;    // calibration constants used as value
    
    // Open file
    file.open(filename,std::ios::out);
    
    // Read from file
    if (file.is_open()) {
      while ( getline (file,line) )  {
        // Parse from line
        if (parse_adc_calibration(line,config,cal)==0) {
            // Save key-value pair
            map_ADC[config]=cal;
            std::cout << "Read ADC: " << config << " " << cal.offset << 
            " " << cal.scale << std::endl;
        }
      }
      file.close();
    } else {
      std::cout << "Unable to open file: " << filename << std::endl;
    }


    return 0;
     
 }
 
 int Obs_Calibration_Vector::read_adc_calibrate() {
     
     if (file_ADC.empty()) return(-1);
     
     return(read_adc_calibrate(file_ADC));
     
 }
 
 
 /****************************************************************************
 *
 * Parse an ADC calibration line
 *
 ***************************************************************************/
 int Obs_Calibration_Vector::parse_adc_calibration(std::string line, 
         std::string& config, struct adc_cal& cal, std::string& comment) {
         
    size_t pos_c;
    
    comment="";
     
    // Find comment token ("#")
    if (line.length()<=0) return(-1);
    pos_c=line.find('#');
    if (pos_c < std::string::npos){
        comment=line.substr(pos_c);
        line=line.substr(0,pos_c);
    }
    if (line.size()==0) return(-1);
    std::cerr << "PARSE: " << line << std::endl;
    // Find configuration string
    pos_c=line.find(',');
    config=line.substr(0,pos_c);
    // Find calibration string
    line=line.substr(pos_c+1);
    pos_c=line.find(',');
    cal.offset = line.substr(0,pos_c);
    line=line.substr(pos_c+1);
    pos_c=line.find(';');
    cal.scale  = line.substr(0,pos_c);
    return(0);
 }

  int Obs_Calibration_Vector::parse_adc_calibration(std::string line, 
         std::string& config, struct adc_cal& cal) {
      
      std::string comment;
      
      return(parse_adc_calibration(line,config,cal,comment));
      
  }
  
  int Obs_Calibration_Vector::set_adc_calibrate(std::string filename){
      
      file_ADC=filename;
      
      read_adc_calibrate(filename);
      
      return(0);
      
  }