#include <Arduino.h>
#include <INA226.h>
#include <EEPROM.h>
#include <Wire.h>

#define  OP_PIN_01  8 
#define  OP_PIN_02  9 
#define  OP_PIN_03  10 

/************************************* INA226 ******************************/

INA226 INA(0x40); // A0=GND,A1=GND 对应地址0x40

#define INA_SAMPLE_COUNT 12

struct
{
  float current;
  float voltage;
  float power;
  float currents[INA_SAMPLE_COUNT];
  float voltages[INA_SAMPLE_COUNT];
  float current_min;
  float current_max;
  float voltage_min;
  float voltage_max;
  float current_coeffA;
  float current_coeffB;
  float voltage_coeffA;
  float voltage_coeffB;
} ina;

void ina226_init()
{
  Wire.begin();
  if (!INA.begin() )
  {
    Serial.println("could not connect. Fix and Reboot");
  }
  else
  {
    INA.setAverage(10);
    delay(100);
    INA.setMaxCurrentShunt(1, 0.002);
  }
}

void ina226_read()
{
  float fVoltage = INA.getBusVoltage()*ina.voltage_coeffA+ina.voltage_coeffB;
  float fCurrent = INA.getCurrent_mA()*ina.current_coeffA+ina.current_coeffB;
  
  for (int i = 0; i < INA_SAMPLE_COUNT - 1; i++)
  { 
    ina.currents[i] = ina.currents[i+1]; 
    ina.voltages[i] = ina.voltages[i+1];
  }
  
  ina.currents[INA_SAMPLE_COUNT - 1] = fCurrent; 
  ina.voltages[INA_SAMPLE_COUNT - 1] = fVoltage;
  
  float voltage_sum=0.0;
  float current_sum=0.0;

  for (int i = 0; i < INA_SAMPLE_COUNT; i++)
  { 
    if(ina.currents[i]<ina.current_min)ina.current_min=ina.currents[i];
    if(ina.voltages[i]<ina.voltage_min)ina.voltage_min=ina.voltages[i];

    if(ina.currents[i]>ina.current_max)ina.current_max=ina.currents[i];
    if(ina.voltages[i]>ina.voltage_max)ina.voltage_max=ina.voltages[i];

    voltage_sum+=ina.voltages[i];
    current_sum+=ina.currents[i];
  }

  ina.current = (current_sum-ina.current_min-ina.current_max)/(INA_SAMPLE_COUNT-2);
  ina.voltage = (voltage_sum-ina.voltage_min-ina.voltage_max)/(INA_SAMPLE_COUNT-2);
  ina.power=ina.current*ina.voltage;
}

/************************************* 参数保存 ***********************************/

#define   EEPROM_SIZE             128
#define   EEPROM_CHECK_LEN        11
#define   INA_COEFF_LEN           16  // 2*2*4

typedef union
{
  float f_buff;
  uint8_t u8_buff[4];
  uint16_t u16_buff[2];
} float_convert;

struct
{
  bool    init;
  bool    change;
  int     address;
  uint8_t check;
  uint8_t check_param[EEPROM_CHECK_LEN] = { 'a', 'b', 'c', 'd', 'e', 'f','g', 'h', 'i', 'j', 'k' }; 
} eeprom;

void float2u8Array(uint8_t *array, float fValue)
{
  float_convert result;
  result.f_buff=fValue;
  for (uint8_t i = 0; i < 4; i++)
  {
    array[i]=result.u8_buff[i];
  }
}

void u8Array2float(uint8_t *array, float *fValue)
{
  float_convert result;
  for (uint8_t i = 0; i < 4; i++)
  {
    result.u8_buff[i]=array[i];
  }
  *fValue=result.f_buff;
}

void eeprom_write_ina_data()
{
  eeprom.address = EEPROM_CHECK_LEN;
  uint8_t array2d[4][4];
  float2u8Array(array2d[0], ina.current_coeffA);
  float2u8Array(array2d[1], ina.current_coeffB);
  float2u8Array(array2d[2], ina.voltage_coeffA);
  float2u8Array(array2d[3], ina.voltage_coeffB);
  int index=0;
  for (uint8_t i = 0; i < 4; ++i)
  {
    for (uint8_t j = 0; j < 4; ++j)
    {
      EEPROM.write(eeprom.address + index, array2d[i][j]); 
      // EEPROM.commit();
      delay(10);
      index++;
    } 
  } 
}

void eeprom_write_all_data()
{
  eeprom.address = 0;
  for (uint8_t i = 0; i < EEPROM_CHECK_LEN; ++i)    
  {
    EEPROM.write(eeprom.address + i, eeprom.check_param[i]); 
    //EEPROM.commit();
    delay(10);
  } 
  eeprom_write_ina_data();
}

void eeprom_read_ina_data()
{
  eeprom.address = EEPROM_CHECK_LEN;
  uint8_t array2d[4][4];
  int index=0;
  for (uint8_t i = 0; i < 4; ++i)
  {
    for (uint8_t j = 0; j < 4; ++j)
    {
      array2d[i][j]=EEPROM.read(eeprom.address + index);
      index++;
    } 
  } 
  u8Array2float(array2d[0], &ina.current_coeffA);
  u8Array2float(array2d[1], &ina.current_coeffB);
  u8Array2float(array2d[2], &ina.voltage_coeffA);
  u8Array2float(array2d[3], &ina.voltage_coeffB);
}

void eeprom_read_all_data()
{
  eeprom_read_ina_data();
}

void eeprom_param_init()
{
  ina.voltage_coeffA=1;
  ina.voltage_coeffB=0;
  ina.current_coeffA=1;
  ina.current_coeffB=0;
}

void eeprom_init()
{
  //EEPROM.begin();
  eeprom.check = 0;
  eeprom.address = 0; 
  for (uint8_t i = 0; i < EEPROM_CHECK_LEN; ++i)  
  {
    if (EEPROM.read(eeprom.address + i) != eeprom.check_param[i])  eeprom.check ++;
  }

  if (eeprom.check <= 1) 
  {
    eeprom_read_all_data();  
  }
  else 
  {
    eeprom_param_init();
    eeprom_write_all_data();
  }
}

/************************************* serial port ******************************/

#define  SP_DATA_LEN   4
#define  OP_COUNT      3

bool outputs[OP_COUNT];

struct
{
  String    command;
  bool      read_mark;
  String    datas[SP_DATA_LEN];
} sp;

void read_outputs()
{
  outputs[0]=digitalRead(OP_PIN_01);
  outputs[1]=digitalRead(OP_PIN_02);
  outputs[2]=digitalRead(OP_PIN_03);
}

void sp_print_header()
{
  for(uint16_t i=0;i<2;i++)
  {
    Serial.print(sp.datas[i]);
    Serial.print(",");
  }
}

void write_io_low(uint8_t index)
{
  switch(index)
  {
    case 0: digitalWrite(OP_PIN_01, LOW);break;
    case 1: digitalWrite(OP_PIN_02, LOW);break;
    case 2: digitalWrite(OP_PIN_03, LOW);break;
  }
}

void write_io_high(uint8_t index)
{
  switch(index)
  {
    case 0: digitalWrite(OP_PIN_01, HIGH);break;
    case 1: digitalWrite(OP_PIN_02, HIGH);break;
    case 2: digitalWrite(OP_PIN_03, HIGH);break;
  }
}

void write_io(uint8_t index)
{
  if(sp.datas[2]=="1")        write_io_high(index);
  else if(sp.datas[2]=="0")   write_io_low(index);
}

void sp_read_value()
{
  bool ret=false;
  sp_print_header();
  if(sp.datas[1]=="Current")      { Serial.print(ina.current); ret=true;}
  else if(sp.datas[1]=="Voltage") { Serial.print(ina.voltage); ret=true;}
  else if(sp.datas[1]=="Output") 
  {
    read_outputs();
    for(uint8_t i=0;i<OP_COUNT;i++) Serial.print((outputs[i]?1:0));
    ret=true;
  }
  else if(sp.datas[1]=="Setting") 
  {
    if     (sp.datas[2]=="CA") { Serial.print("CA,"); Serial.print(ina.current_coeffA); ret=true;}
    else if(sp.datas[2]=="CB") { Serial.print("CB,"); Serial.print(ina.current_coeffB); ret=true;}
    else if(sp.datas[2]=="VA") { Serial.print("VA,"); Serial.print(ina.voltage_coeffA); ret=true;}
    else if(sp.datas[2]=="VB") { Serial.print("VB,"); Serial.print(ina.voltage_coeffB); ret=true;}
  }
  if(ret) Serial.print(",OK"); else Serial.print(",NG"); 
}

void sp_write_value()
{
  bool ret=false;
  sp_print_header();
  if     (sp.datas[1]=="D1" && (sp.datas[2]=="1" || sp.datas[2]=="0")) { write_io(0); ret=true;}
  else if(sp.datas[1]=="D2" && (sp.datas[2]=="1" || sp.datas[2]=="0")) { write_io(1); ret=true;}
  else if(sp.datas[1]=="D3" && (sp.datas[2]=="1" || sp.datas[2]=="0")) { write_io(2); ret=true;}
  if(ret) Serial.print(",OK"); else Serial.print(",NG"); 
}

void sp_setting()
{
  bool ret=false;
  sp_print_header();
  char *pOutStr;
  float value = strtod(sp.datas[2].c_str(),&pOutStr);
  if     (sp.datas[1]=="CA") { ina.current_coeffA=value; ret=true;}
  else if(sp.datas[1]=="CB") { ina.current_coeffB=value; ret=true;}
  else if(sp.datas[1]=="VA") { ina.voltage_coeffA=value; ret=true;}
  else if(sp.datas[1]=="VB") { ina.voltage_coeffB=value; ret=true;}
  if(ret) ; // Update eeprom value
  if(ret) Serial.print(",OK"); else Serial.print(",NG"); 
}

/*
R,Current
R,Voltage
R,Output
R,Setting,CA/CB/VA/VB
W,D1/D2/D3,1/0
S,CA/CB/VA/VB,value
*/
void run_command()
{
  if(sp.datas[0]=="R") sp_read_value();  // Read value
  if(sp.datas[0]=="W") sp_write_value(); // Write value
  if(sp.datas[0]=="S") sp_setting();  // Setting
}

void read_command()
{
  while(Serial.available()>0)
  {
    sp.command += char(Serial.read());
    delay(2);
    sp.read_mark = true;
  }
  if(sp.read_mark)
  {
    // Get datas from command string.
    uint8_t j=0;
    for(uint8_t i=0;i<sp.command.length();i++)
    {
      if(sp.command[i]==',')
      {
        j++;
      }
      else
      {
        if(j<sizeof(sp.datas)/sizeof(sp.datas[0]))
        {
          sp.datas[j]+=sp.command[i];
        }
        else
        {
          break;
        }
      }
    }

    // Run command
    run_command();
    
    // Initial sp data struct
    sp.read_mark = false;
    sp.command="";
    for(uint8_t i=0;i<SP_DATA_LEN;i++)
    {
      sp.datas[i]="";
    }
  }
}

/************************************* setup & loop ******************************/

void gpio_init()
{
  pinMode(OP_PIN_01,OUTPUT);
  pinMode(OP_PIN_02,OUTPUT);
  pinMode(OP_PIN_03,OUTPUT);

  digitalWrite(OP_PIN_01, LOW);
  digitalWrite(OP_PIN_02, LOW);
  digitalWrite(OP_PIN_03, LOW);
}

void setup() 
{
  Serial.begin(115200);
  gpio_init();
  eeprom_init();
  ina226_init();
}

void loop() 
{
  read_command();
  ina226_read();
  if (!INA.begin() )
  {
    ina226_init();
  }
}