/****************************************************************************/
/* Copyright (c) 2018 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy Controller Settings                                */
/* Filename : PbConSettings.cpp                                             */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 05/04/2018                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

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

#include "PbConSettings.hpp"

PbConSettings::PbConSettings(const char* filename)
{
   cfg_file = strdup(filename);

   rpid_kp = rpid_ki = rpid_kd = rpid_min = rpid_max = rpid_target = 0.;
   dpid_kp = dpid_ki = dpid_kd = dpid_min = dpid_max = dpid_target = 0.;
   re_sigma = re_cov1 = re_cov2 = re_force_slope = re_force_offset = 0.;

   load(filename);
}

PbConSettings::~PbConSettings()
{
   save(cfg_file);
   delete cfg_file;
}

// Load a configuration.
// Open filename and parse each key = value line in filename.
// Set each recognized key, flag the others.
// Close filename.
// 
int PbConSettings::load(const char* filename)
{
   char line[120];
   FILE *f = fopen(filename, "r");

   if (!f)
      return -1;

   // For all non-comment lines, parse the key = value pairs
   // and load known key values into the attributes
   // 
   char key[50], value[50];
   size_t k = sizeof(key);
   while (fgets(line, sizeof(line)-1, f))
   {
      if (line[0] == '#') continue;  // Skip comment lines
      if (2 != sscanf(line, "%s %s", key, value))
      {
         fprintf(stderr, "PbConSettings::load() - failed to parse %s in %s\n",
            filename, line);
      }

      // Load recognized key values
      // 
      if      (!strncmp(key,"rpid_kp",k))     rpid_kp = atof(value);
      else if (!strncmp(key,"rpid_ki",k))     rpid_ki = atof(value);
      else if (!strncmp(key,"rpid_kd",k))     rpid_kd = atof(value);
      else if (!strncmp(key,"rpid_min",k))    rpid_min = atof(value);
      else if (!strncmp(key,"rpid_max",k))    rpid_max = atof(value);
      else if (!strncmp(key,"rpid_target",k)) rpid_target = atof(value);

      else if (!strncmp(key,"dpid_kp",k))     dpid_kp = atof(value);
      else if (!strncmp(key,"dpid_ki",k))     dpid_ki = atof(value);
      else if (!strncmp(key,"dpid_kd",k))     dpid_kd = atof(value);
      else if (!strncmp(key,"dpid_min",k))    dpid_min = atof(value);
      else if (!strncmp(key,"dpid_max",k))    dpid_max = atof(value);
      else if (!strncmp(key,"dpid_target",k)) dpid_target = atof(value);

      else if (!strncmp(key,"re_sigma",k))        re_sigma = atof(value);
      else if (!strncmp(key,"re_cov1",k))         re_cov1 = atof(value);
      else if (!strncmp(key,"re_cov2",k))         re_cov2 = atof(value);
      else if (!strncmp(key,"re_force_slope",k))  re_force_slope = atof(value);
      else if (!strncmp(key,"re_force_offset",k)) re_force_offset = atof(value);

      else fprintf(stderr, "PbConSettings::load() - unknown setting %s in %s\n",
                 key, filename);
   }

   fclose(f);
}

// Save the current configuration.
// Open filename and write each attribute in key = value lines.
// Close filename.
// 
int PbConSettings::save(const char* filename)
{
   char line[120];
   FILE *f = fopen(filename, "w");

   if (!f)
      return -1;

   // Write comment line with the current date and time
   // 
   time_t t = time(NULL);
   snprintf(line, sizeof(line), "# last updated %s", ctime(&t)); fputs(line, f);

   // Write range pid settings
   // 
   fputs("# range pid stuff\n", f);
   snprintf(line, sizeof(line), "rpid_kp = %.4f\n", rpid_kp); fputs(line, f);
   snprintf(line, sizeof(line), "rpid_ki = %.4f\n", rpid_ki); fputs(line, f);
   snprintf(line, sizeof(line), "rpid_kd = %.4f\n", rpid_kd); fputs(line, f);
   snprintf(line, sizeof(line), "rpid_min = %.4f\n", rpid_min); fputs(line, f);
   snprintf(line, sizeof(line), "rpid_max = %.4f\n", rpid_max); fputs(line, f);
   snprintf(line, sizeof(line), "rpid_target = %.4f\n", rpid_target); fputs(line, f);

   // Write range pid settings   
   // 
   fputs("# depth pid stuff\n", f);
   snprintf(line, sizeof(line), "dpid_kp = %.4f\n", dpid_kp); fputs(line, f);
   snprintf(line, sizeof(line), "dpid_ki = %.4f\n", dpid_ki); fputs(line, f);
   snprintf(line, sizeof(line), "dpid_kd = %.4f\n", dpid_kd); fputs(line, f);
   snprintf(line, sizeof(line), "dpid_min = %.4f\n", dpid_min); fputs(line, f);
   snprintf(line, sizeof(line), "dpid_max = %.4f\n", dpid_max); fputs(line, f);
   snprintf(line, sizeof(line), "dpid_target = %.4f\n", dpid_target); fputs(line, f);

   // Write range est setting
   // 
   fputs("# range est stuff\n", f);
   snprintf(line, sizeof(line), "re_sigma = %.4f\n", re_sigma); fputs(line, f);
   snprintf(line, sizeof(line), "re_cov1 = %.7f\n", re_cov1); fputs(line, f);
   snprintf(line, sizeof(line), "re_cov2 = %.4f\n", re_cov2); fputs(line, f);
   snprintf(line, sizeof(line), "re_force_slope = %.4f\n", re_force_slope); fputs(line, f);
   snprintf(line, sizeof(line), "re_force_offset = %.4f\n", re_force_offset); fputs(line, f);

   fclose(f);
}

#ifdef TEST_PBS
int main(int argc, char** argv)
{
   PbConSettings pbs("./pbcon.cfg");

   pbs.rpid_target = 100.;
}
#endif