#include <DPM8600.h>       //DPM DCDC library
//#include <DPM8600N.h>       //DPM DCDC library

#include <SoftwareSerial.h> //SoftwarSerial library to adress more Serial ports
#define debugMode 1
#define DPM8624_1 Serial1
#define DPM8624_2 Serial2
//SoftwareSerial DPM8624_1(50, 51);
//SoftwareSerial DPM8624_2(52, 53);
DPM8600 DPM_1;
DPM8600 DPM_2;

const int   nMaxValues         = 6;               // maximum number of values per label stored in one read-cycle

int nDCDCCounter[2][2];                                         // counter to count number of values measured per label in one read cycle, 2 labels (I, V)
float flgPowerDCDC[2];                                          // power status of DPMs
float aFeedCurrentArr[2][nMaxValues];                           // array to store DPM measured values of the label 'I'
float vFeedVoltageArr[2][nMaxValues];                           // array to store DPM measured values of the label 'V'
float aFeedCurrent[2];                                          // mean value of measured load current values
float vFeedVoltage[2];                                          // mean value of measured load voltage values
float wFeedPower[2];                                            // calculated charging power based on measured values
float wFeedPowerSumMeas;                                        // sum of feed power - measured every reread timeslot
float wFeedPowerSumLow;                                         // sum of feed power - lowest value in recalc timeslot
unsigned long tDPMReadTime     = 5000;            // max read-time to get DPM data in ms


void setup() {
  // put your setup code here, to run once:
  Serial.begin(19200);     // communication and debugging
  DPM8624_1.begin(9600);
  DPM8624_2.begin(9600);
  DPM_1.begin(DPM8624_1);
  DPM_2.begin(DPM8624_2);
  delay(1000);

}

void loop() {
  // put your main code here, to run repeatedly:
  readDPM();
  delay(3000);

  DPM_2.write('p',true);
  DPM_1.write('p',true);
}

void readDPM()
{
  DPM_2.write('p',true);
  delay(500);
  DPM_1.write('p',true);
  initializeDPMArrays();
  unsigned long startTimeDPM = millis();
  unsigned long currentTimeDPM = startTimeDPM;

  while (currentTimeDPM - startTimeDPM < tDPMReadTime) {

   // DPM8624_1.listen();
    flgPowerDCDC[0] = DPM_1.read('p');
    Serial.println(flgPowerDCDC[0]);
    if (flgPowerDCDC[0] > 0) 
    {
      if (nDCDCCounter[0][0] < nMaxValues) {
			  vFeedVoltageArr[0][nDCDCCounter[0][0]] = DPM_1.read('v');
				nDCDCCounter[0][0]++;
			}
      if (nDCDCCounter[0][1] < nMaxValues) {
			  aFeedCurrentArr[0][nDCDCCounter[0][1]] = DPM_1.read('c');
				nDCDCCounter[0][1]++;
			}
    }  
  
    delay(500);
    
    //DPM8624_2.listen();
    delay(500);
    flgPowerDCDC[1] = DPM_2.read('p');
    Serial.println(flgPowerDCDC[1]);
    if (flgPowerDCDC[1] > 0) 
    {
      if (nDCDCCounter[1][0] < nMaxValues) {
			  vFeedVoltageArr[1][nDCDCCounter[1][0]] = DPM_2.read('v');
				nDCDCCounter[1][0]++;
			}
      if (nDCDCCounter[1][1] < nMaxValues) {
			  aFeedCurrentArr[1][nDCDCCounter[1][1]] = DPM_2.read('c');
				nDCDCCounter[1][1]++;
			}
    }
    currentTimeDPM  = millis();
  }

  for (int j = 0 ; j < 2; j++){
    for (int i = 0 ; i < nMaxValues; i++){
      if (i < nDCDCCounter[j][0]){
        vFeedVoltage[j] = vFeedVoltage[j] + vFeedVoltageArr[j][i];
      }
      if (i < nDCDCCounter[j][1]){
        aFeedCurrent[j] = aFeedCurrent[j] + aFeedCurrentArr[j][i];
      }
    }
    if (nDCDCCounter[j][0] > 0) {
      vFeedVoltage[j] = vFeedVoltage[j]/nDCDCCounter[j][0];
    }
    if (nDCDCCounter[j][1] > 0) {    
      aFeedCurrent[j] = aFeedCurrent[j]/nDCDCCounter[j][1];
    }
    wFeedPower[j] = wFeedPower[j] + vFeedVoltage[j]*aFeedCurrent[j];
    wFeedPowerSumMeas = wFeedPowerSumMeas + wFeedPower[j];
  }

  wFeedPowerSumMeas = max( wFeedPowerSumMeas, 0.0 );

  if (debugMode)
  {
    Serial.println(" Calculated DPM values");
    Serial.print(" DPM1: v = ");
    Serial.print(vFeedVoltage[0]);
    Serial.print("; a = ");
    Serial.print(aFeedCurrent[0]);
    Serial.print("; w = ");
    Serial.println(wFeedPower[0]);
    Serial.print(" DPM2: v = ");
    Serial.print(vFeedVoltage[1]);
    Serial.print("; a = ");
    Serial.print(aFeedCurrent[1]);
    Serial.print("; w = ");
    Serial.println(wFeedPower[1]);        
    Serial.print(" wFeedPowerSumMeas = ");
    Serial.println(wFeedPowerSumMeas);
  }
  DPM_2.write('p',false);
  DPM_1.write('p',false);
  delay(1000);

}

void initializeDPMArrays()
{
    memset(aFeedCurrentArr, 0.0, sizeof(float) * 2 * nMaxValues );
    memset(vFeedVoltageArr, 0.0, sizeof(float) * 2 * nMaxValues );
    memset(aFeedCurrent, 0.0, sizeof(float) * 2 );
    memset(vFeedVoltage, 0.0, sizeof(float) * 2 );
    memset(wFeedPower, 0.0, sizeof(float) * 2 );
    memset(nDCDCCounter, 0, sizeof(int) * 2 * 2 );
    wFeedPowerSumMeas = 0.0;
}