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

/************************************* 管脚定义 *************************************/

#define ADC_PIN_01  32
#define ADC_PIN_02  33
#define ADC_PIN_03  34
#define ADC_PIN_04  35
#define UP_PIN_01   19
#define UP_PIN_02   18
#define UP_PIN_03   5
#define UP_PIN_04   17
#define DOWN_PIN_01 16
#define DOWN_PIN_02 4
#define DOWN_PIN_03 0
#define DOWN_PIN_04 2

/************************************* INA 电流电压功率 ******************************/

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

struct
{
  float current;
  float voltage;
  float power;
  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()
{
  ina.voltage = INA.getBusVoltage()*ina.voltage_coeffA+ina.voltage_coeffB;
  ina.current = INA.getCurrent_mA()*ina.current_coeffA+ina.current_coeffB;
  //ina.power = INA.getPower_mW();
  ina.power=ina.current*ina.voltage;
}

/************************************* ADC *************************************/

#define ADC_COUNT 4
#define ADC_AVERAGE 10 

struct
{
  float voltages[ADC_COUNT];
  float coeffA[ADC_COUNT];
  float coeffB[ADC_COUNT];
} adc;

void adc_analog(float *vol,uint8_t index)
{
  int value;
  switch (index)
  {
    case 0: value=analogRead(ADC_PIN_01); break;
    case 1: value=analogRead(ADC_PIN_02); break;
    case 2: value=analogRead(ADC_PIN_03); break;
    case 3: value=analogRead(ADC_PIN_04); break;
    default: break;
  }
  *vol =  value*(10.0/5822.0);
}

void adc_read()
{
  float values[ADC_AVERAGE][ADC_COUNT];
  float sum_values[ADC_COUNT];

  for (uint8_t i = 0; i < ADC_AVERAGE; i++)
  {
    for (uint8_t j = 0; j < ADC_COUNT; j++)
    {
      adc_analog(&values[i][j],j);
      sum_values[j]+=values[i][j];
    }
  }
  for (uint8_t i = 0; i < ADC_COUNT; i++)
  {
    adc.voltages[i]=(sum_values[i]/ADC_AVERAGE)*adc.coeffA[i]+adc.coeffB[i];
  }
}

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

#define   EEPROM_SIZE             128
#define   EEPROM_CHECK_LEN        11
#define   INA_COEFF_LEN           16  // 2*2*4
#define   ADC_COEFF_LEN           32  // 4*2*4
#define   MODBUS_LEN              7   // 3+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;

struct
{
  uint8_t baudrate_index=0;
  uint8_t slave_id=1;
  uint8_t pwm_count=5;
  float   pwm_timeout=5;
} modbus_param;

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 float2u16Array(uint16_t *array, float fValue)
{
  float_convert result;
  result.f_buff=fValue;
  for (uint8_t i = 0; i < 2; i++)
  {
    array[i]=result.u16_buff[i];
  }
}

void u16Array2float(uint16_t *array, float *fValue)
{
  float_convert result;
  for (uint8_t i = 0; i < 2; i++)
  {
    result.u16_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_adc_data()
{
  eeprom.address = EEPROM_CHECK_LEN+INA_COEFF_LEN;
  uint8_t array2d[8][4];
  for (uint8_t i = 0; i < 4; ++i)
  {
    float2u8Array(array2d[i], adc.coeffA[i]);
    float2u8Array(array2d[i+4], adc.coeffB[i]);
  }

  int index=0;
  for (uint8_t i = 0; i < 8; ++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_modbus_data()
{
  eeprom.address = EEPROM_CHECK_LEN+INA_COEFF_LEN+ADC_COEFF_LEN;
  EEPROM.write(eeprom.address + 0, modbus_param.baudrate_index); 
  EEPROM.write(eeprom.address + 1, modbus_param.slave_id); 
  EEPROM.write(eeprom.address + 2, modbus_param.pwm_count); 
  uint8_t array[4];
  float2u8Array(array, modbus_param.pwm_timeout);
  for (uint8_t j = 0; j < 4; ++j)
  {
    EEPROM.write(eeprom.address + 3+j, array[j]); 
    EEPROM.commit();
    delay(10);
  }
}

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();
  eeprom_write_adc_data();
  eeprom_write_modbus_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_adc_data()
{
  eeprom.address = EEPROM_CHECK_LEN+INA_COEFF_LEN;
  uint8_t array2d[8][4];
  int index=0;
  for (uint8_t i = 0; i < 8; ++i)
  {
    for (uint8_t j = 0; j < 4; ++j)
    {
      array2d[i][j]=EEPROM.read(eeprom.address + index);
      index++;
    } 
  } 
  for (uint8_t i = 0; i < 4; ++i)
  {
    u8Array2float(array2d[i], &adc.coeffA[i]);
    u8Array2float(array2d[i+4], &adc.coeffB[i]);
  }
}

void eeprom_read_modbus_data()
{
  eeprom.address = EEPROM_CHECK_LEN+INA_COEFF_LEN+ADC_COEFF_LEN;
  modbus_param.baudrate_index=EEPROM.read(eeprom.address + 0); 
  modbus_param.slave_id=EEPROM.read(eeprom.address + 1); 
  modbus_param.pwm_count=EEPROM.read(eeprom.address + 2); 
  uint8_t array[4];
  float2u8Array(array, modbus_param.pwm_timeout);
  for (uint8_t j = 0; j < 4; ++j)
  {
    array[j]=EEPROM.read(eeprom.address + 3+j); 
  }
  u8Array2float(array, &modbus_param.pwm_timeout);
}

void eeprom_read_all_data()
{
  eeprom_read_ina_data();
  eeprom_read_adc_data();
  eeprom_read_modbus_data();
}

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

  for (uint8_t i = 0; i < ADC_COUNT; i++)
  {
    adc.coeffA[i]=1.0;
    adc.coeffB[i]=0.0;
  }

  modbus_param.baudrate_index=0;
  modbus_param.slave_id=1;
  modbus_param.pwm_count=5;
}

void eeprom_init()
{
  EEPROM.begin(EEPROM_SIZE);
  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();
  }
}

/************************************* Modbus 模块 ******************************/

#define MODBUS_COIL_COUNT  10
#define MODBUS_HOLDING_COUNT 70

const byte dePin = 23;
struct
{
  bool     pwm_read_flag;
  bool     pwm_read_last_flag;
  uint16_t pwm_read_mode; // 0: None; 0000 0000, 右往左依次 Down01..Down04, Up01..Up04
  uint8_t  pwm_read_counts[8];
  bool     pwm_read_vals[8];
} run_param;

bool coils[MODBUS_COIL_COUNT];
uint16_t holdingRegisters[MODBUS_HOLDING_COUNT];

ModbusRTUSlave modbus(Serial, dePin); // serial port, driver enable pin for rs-485

void modbus_update_holding_register(float value,uint8_t start_index)
{
  float_convert convert;
  convert.f_buff=value;
  for (uint8_t i = 0; i < 2; i++)
  {
    holdingRegisters[start_index+i]=convert.u16_buff[i];
  }
}

void modbus_get_holding_register_value_float(float *value,uint8_t start_index)
{
  float_convert convert;

  for (uint8_t i = 0; i < 2; i++)
  {
    convert.u16_buff[i]=holdingRegisters[start_index+i];
  }

  *value=convert.f_buff;
}

void modbus_param_update()
{
  holdingRegisters[0]=modbus_param.baudrate_index;
  holdingRegisters[1]=modbus_param.slave_id;
  holdingRegisters[2]=modbus_param.pwm_count;
  modbus_update_holding_register(modbus_param.pwm_timeout,3);

  modbus_update_holding_register(ina.current_coeffA,10);
  modbus_update_holding_register(ina.current_coeffB,12);
  modbus_update_holding_register(ina.voltage_coeffA,14);
  modbus_update_holding_register(ina.voltage_coeffB,16);
  
  for (uint8_t i = 0; i < 4; i++)
  {
    modbus_update_holding_register(adc.coeffA[i],20+i*2);
    modbus_update_holding_register(adc.coeffB[i],30+i*2);
  }
}

void modbus_param_change_check()
{
  bool need_update=false;
  if(holdingRegisters[0]!=modbus_param.baudrate_index)
  {
    modbus_param.baudrate_index=holdingRegisters[0];
    need_update=true;
  }
  if(holdingRegisters[1]!=modbus_param.baudrate_index)
  {
    modbus_param.slave_id=holdingRegisters[1];
    need_update=true;
  }
  if(holdingRegisters[2]!=modbus_param.baudrate_index)
  {
    modbus_param.pwm_count=holdingRegisters[2];
    need_update=true;
  }
  float value;
  modbus_get_holding_register_value_float(&value,3);
  if (modbus_param.pwm_timeout!=value)
  {
    modbus_param.pwm_timeout=value;
    need_update=true;
  }
  modbus_get_holding_register_value_float(&value,10);
  if (ina.current_coeffA!=value)
  {
    ina.current_coeffA=value;
    need_update=true;
  }
  modbus_get_holding_register_value_float(&value,12);
  if (ina.current_coeffB!=value)
  {
    ina.current_coeffB=value;
    need_update=true;
  }
  modbus_get_holding_register_value_float(&value,14);
  if (ina.voltage_coeffA!=value)
  {
    ina.voltage_coeffA=value;
    need_update=true;
  }
  modbus_get_holding_register_value_float(&value,16);
  if (ina.voltage_coeffB!=value)
  {
    ina.voltage_coeffB=value;
    need_update=true;
  }

  for (uint8_t i = 0; i < 4; i++)
  {
    float valueA,valueB;
    modbus_get_holding_register_value_float(&valueA,20+i*2);
    modbus_get_holding_register_value_float(&valueB,30+i*2);
    if (adc.coeffA[i]!=valueA)
    {
      adc.coeffA[i]=valueA;
      need_update=true;
    }
    if (adc.coeffB[i]!=valueB)
    {
      adc.coeffB[i]=valueB;
      need_update=true;
    }
  }

  if (need_update)
  {
    eeprom_write_all_data();
  }
}

void modbus_value_update()
{
  modbus_update_holding_register(ina.current,40);
  modbus_update_holding_register(ina.voltage,42);
  modbus_update_holding_register(ina.power,44);
  for (uint8_t i = 0; i < 4; i++)
  {
    modbus_update_holding_register(adc.voltages[i],50+i*2);
  }
}

void modbus_init()
{
  modbus.configureCoils(coils, MODBUS_COIL_COUNT); 
  modbus.configureHoldingRegisters(holdingRegisters, MODBUS_HOLDING_COUNT); 
  unsigned long baudrate=9600;
  switch (modbus_param.baudrate_index)
  {
    case 0:baudrate=9600; break;
    case 1:baudrate=19200; break;
    case 2:baudrate=38400; break;
    case 3:baudrate=115200; break;
    default: break;
  }
  modbus.begin(modbus_param.slave_id, baudrate);
  modbus_param_update();
}

/************************************* pwm ******************************/

void pwm_inter_index(uint8_t index)
{
  if(run_param.pwm_read_flag)
  {
    run_param.pwm_read_counts[index]++;
    if(run_param.pwm_read_counts[index]>=modbus_param.pwm_count)
    {
      run_param.pwm_read_counts[index]=modbus_param.pwm_count;
      coils[index+1]=true;
    }
  }
}

void pwm_inter_01()
{
  pwm_inter_index(0);
}
void pwm_inter_02()
{
  pwm_inter_index(1);
}
void pwm_inter_03()
{
  pwm_inter_index(2);
}
void pwm_inter_04()
{
  pwm_inter_index(3);
}
void pwm_inter_05()
{
  pwm_inter_index(4);
}
void pwm_inter_06()
{
  pwm_inter_index(5);
}
void pwm_inter_07()
{
  pwm_inter_index(6);
}
void pwm_inter_08()
{
  pwm_inter_index(7);
}

void pwm_init()
{
  for (uint8_t i = 0; i < 8; i++)
  {
    coils[i+1]=false;
    run_param.pwm_read_counts[i]=0;
    run_param.pwm_read_vals[i]=false;
  }
  coils[0]=false;
  run_param.pwm_read_flag = false;
  run_param.pwm_read_last_flag = false;
}

void pwm_loop()
{
  run_param.pwm_read_flag = coils[0];
  if (run_param.pwm_read_flag !=run_param.pwm_read_last_flag)
  {
    if (!run_param.pwm_read_flag)
    {
      for (uint8_t i = 0; i < 8; i++)
      {
        coils[i+1]=false;
        run_param.pwm_read_counts[i]=0;
        run_param.pwm_read_vals[i]=false;
      }
      // Serial.println("Reset PWM  Flag");
    }
    else
    {
      // Start PWM 
      // Serial.println("Start PWM Inspection");
    }

    run_param.pwm_read_last_flag = run_param.pwm_read_flag;
  }
}

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

TaskHandle_t Task_Data_Loop;

void gpio_init()
{
  pinMode(ADC_PIN_01,INPUT);
  pinMode(ADC_PIN_02,INPUT);
  pinMode(ADC_PIN_03,INPUT);
  pinMode(ADC_PIN_04,INPUT);

  pinMode(UP_PIN_01,INPUT_PULLUP);
  pinMode(UP_PIN_02,INPUT_PULLUP);
  pinMode(UP_PIN_03,INPUT_PULLUP);
  pinMode(UP_PIN_04,INPUT_PULLUP);
  pinMode(DOWN_PIN_01,INPUT_PULLUP);
  pinMode(DOWN_PIN_02,INPUT_PULLUP);
  pinMode(DOWN_PIN_03,INPUT_PULLUP);
  pinMode(DOWN_PIN_04,INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(UP_PIN_01), pwm_inter_01, RISING);
  attachInterrupt(digitalPinToInterrupt(UP_PIN_02), pwm_inter_02, RISING);
  attachInterrupt(digitalPinToInterrupt(UP_PIN_03), pwm_inter_03, RISING);
  attachInterrupt(digitalPinToInterrupt(UP_PIN_04), pwm_inter_04, RISING);
  
  attachInterrupt(digitalPinToInterrupt(DOWN_PIN_01), pwm_inter_05, RISING);
  attachInterrupt(digitalPinToInterrupt(DOWN_PIN_02), pwm_inter_06, RISING);
  attachInterrupt(digitalPinToInterrupt(DOWN_PIN_03), pwm_inter_07, RISING);
  attachInterrupt(digitalPinToInterrupt(DOWN_PIN_04), pwm_inter_08, RISING);
}

void data_loop()
{
  ina226_read();
  adc_read();
}

void task_data_loop(void *pvParameters)
{
  for (;;)
  {
    data_loop();
  }
}

void setup() {
  Serial.begin(115200);
  gpio_init();
  eeprom_init();
  ina226_init();
  modbus_init();
  pwm_init();
  xTaskCreatePinnedToCore(task_data_loop, "Task_Data_Loop", 10000, NULL, 1, &Task_Data_Loop, 0);
}

void loop() {
  modbus.poll();
  modbus_param_change_check();
  modbus_value_update();
  pwm_loop();
}