#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>

typedef struct _ampd_data_ {
 float *data;
 int len;
 float avg;
 int hpWin_min;
 float **peaks;
 int peakCnt;
}AmpdData;

typedef struct _data_node_ {
 uint64_t tsMs;
 float value;
}DataNode;

class PeakDetector
{
public:
 PeakDetector(int period) :
 _len(0),
 _sum(0.0),
 _avg(0.0),
 positive(0){
 hpWin_min = period/4; // 12 * 10; // ms
 this->posPeak.value = this->negPeak.value = 0.0;
 }
 ~PeakDetector() { }

 void prepareDetector(const std::vector<DataNode> &samp) {
 int i, len;
 len = samp.size();
 for (i = 0; i < len; i++) {
 this->_sum += samp[i].value;
 if (this->posPeak.value > samp[i].value) {
 this->posPeak = samp[i];
 }
 if (this->negPeak.value < samp[i].value) {
 this->negPeak = samp[i];
 }
 }
 this->_len += len;
 this->_avg = this->_sum / this->_len;

 if (samp[i-1].value > this->_avg) {
 this->positive = 1;
 } else {
 this->positive = -1;
 }

 this->staPoint = samp[i-1];
 }
 void cleanDetector(std::vector<DataNode> &peaks) {
 if ((this->lastPoint.tsMs - staPoint.tsMs) < this->hpWin_min) {
 return;
 }
 if (positive == 1) {
 if (fabs((this->posPeak.value - this->_avg) / this->_avg) < 0.001) {
 return;
 }
 peaks.push_back(posPeak);
 } else {
 if (fabs((this->_avg - this->negPeak.value) / this->_avg) < 0.001) {
 return;
 }
 peaks.push_back(negPeak);
 }
 printf("\n");
 }
 void detectPeaks(const std::vector<DataNode> &samp, std::vector<DataNode> &peaks) {
 int i, len;
 len = samp.size();
 if (this->_len > 7200*10) { // 波形限制,最大周期长度内,未找到明显波动波形
 this->_avg = this->_sum / this->_len;
 this->_sum = 0.0;
 this->_len = 0;
 }
 for (i = 0; i < len; i++) {
 this->lastPoint = samp[i];
 if (this->positive == 1) {
 if (this->posPeak.value < samp[i].value) { // 查找当前正峰值
 this->posPeak = samp[i];
 }
 do {
 if (samp[i].value >= this->_avg) break; // 正周期继续
 if (fabs((this->posPeak.value - this->_avg) / this->_avg) < 0.001) break; // 波形幅值限制,幅值不明显,不认为是有效波动

 if ((this->lastPoint.tsMs - staPoint.tsMs) > this->hpWin_min) {
 peaks.push_back(posPeak);
 staPoint = samp[i];
 } else {
 }
 this->positive = -1; // 切换波峰符号
 this->negPeak = samp[i];
 } while(0);
 } else {
 if (this->negPeak.value > samp[i].value) { // 查找当前负峰值
 this->negPeak = samp[i];
 }
 do {
 if (samp[i].value <= this->_avg) break; // 负周期继续
 if (fabs((this->_avg - this->negPeak.value) / this->_avg) < 0.001) break; // 波形幅值限制,幅值不明显,不认为是有效波动

 if ((this->lastPoint.tsMs - staPoint.tsMs) > this->hpWin_min) {
 peaks.push_back(negPeak);
 staPoint = samp[i];
 } else {
 }
 this->positive = 1; // 切换波峰符号
 this->posPeak = samp[i];
 this->_avg = this->_sum / this->_len; // 计算上一个周期均值
 this->_sum = 0.0; // 准备下一个周期累计
 this->_len = 0;
 } while(0);
 }
 this->_sum += samp[i].value; // 累计当前周期
 this->_len++;
 }
 }

 void debug() {
 printf("\nwin: %ld; avg: %.3f %.3f %.3f; positive: %d\n", hpWin_min, _avg, this->posPeak.value, this->negPeak.value, this->positive);
 }
private:
 int _len; // 当前周期累计计数
 float _sum; // 当前周期累计值
 float _avg; // 上一周期平均值
 int positive; // 1: 当前检测正周期;-1: 当前检测负周期
 uint64_t hpWin_min; // ms 最小检测窗口尺寸(4分之一波)
 DataNode posPeak, negPeak, staPoint, lastPoint;
};

void ampd(AmpdData *samp)
{
 int i, sta = 0;
 int positive = 0;
 float sum = 0;
 float *posPeak, *negPeak;
 
 for (i = 0; i < samp->len; i++) {
 sum += samp->data[i];
 }
 samp->avg = sum / samp->len;
 
 if (samp->data[0] > samp->avg) {
 positive = 1;
 } else {
 positive = -1;
 }

 posPeak = negPeak = &samp->data[0];

 for (i = 0; i < samp->len; i++) {
 if (positive == 1) {
 if (*posPeak < samp->data[i]) {
 posPeak = &samp->data[i];
 }
 if (samp->data[i] < samp->avg) {
 if ((i - sta) > samp->hpWin_min) {
 samp->peaks[samp->peakCnt] = posPeak;
 samp->peakCnt++;
 sta = i;
 } else {
 }
 positive = -1;
 negPeak = &samp->data[i];
 }
 } else {
 if (*negPeak > samp->data[i]) {
 negPeak = &samp->data[i];
 }
 if (samp->data[i] > samp->avg) {
 if ((i - sta) > samp->hpWin_min) {
 samp->peaks[samp->peakCnt] = negPeak;
 samp->peakCnt++;
 sta = i;
 } else {
 }
 positive = 1;
 posPeak = &samp->data[i];
 }
 }
 }
 if (positive == 1) {
 samp->peaks[samp->peakCnt] = posPeak;
 samp->peakCnt++;
 } else {
 samp->peaks[samp->peakCnt] = negPeak;
 samp->peakCnt++;
 }
}

int main(int argc, char *argv[])
{
 AmpdData samp;
 int i;
 float noise = 0;
 int period, cycle, N;
 
 period = 20;
 cycle = 2;
 N = period*cycle;

 samp.len = N;
 samp.data = (float*)malloc(samp.len * sizeof(float));
 samp.peaks = (float**)malloc(samp.len / 4 * sizeof(float));
 samp.peakCnt = 0;
 samp.hpWin_min = period/4;

 srand((unsigned)time( NULL ));
 for (i = 0; i < N; i++) {
 noise = ((float)rand() / RAND_MAX - 0.5f) * 0.2f;
 samp.data[i] = 5.0f * sin(2.0*M_PI*cycle / N * i) + noise;
 }

 ampd(&samp);

 for (i = 0; i < samp.peakCnt; i++) {
 printf("%p; %02d; %.3f\n", samp.peaks[i], (int)((samp.peaks[i]-samp.data)), *samp.peaks[i]);
 }

 PeakDetector peakDtor(period*10);
 std::vector<DataNode> sampData;
 std::vector<DataNode> peaks;
 DataNode node;

 for (i = 0; i < N; i++) {
 noise = ((float)rand() / RAND_MAX - 0.5f) * 0.2f;
 node.value = 5.0f * sin(2.0*M_PI*cycle / N * i) + noise;
 node.tsMs = 10*i;
 sampData.push_back(node);
 }

 peakDtor.prepareDetector(sampData);
 peakDtor.debug();

 for (i = 0; i < N; i++) {
 sampData[i].tsMs = 10*i+10*N;
 }
 for (i = 0; i < N; i++) {
 peakDtor.detectPeaks({sampData[i]}, peaks);
 }
 for (i = 0; i < N; i++) {
 sampData[i].tsMs = 10*i+10*N*2;
 }
 peakDtor.detectPeaks(sampData, peaks);

 peakDtor.cleanDetector(peaks);
 peakDtor.debug();
 for (auto &p : peaks) {
 printf("p: %02ld; %.3f\n", p.tsMs, p.value);
 }

 return 0;
}