1 /*
2  * Copyright (C) 2021 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <iostream>
18 #include <vector>
19 
20 constexpr int kMinLoopLimitValue = 1;
21 constexpr int kNumPeaks = 3;
22 
23 /*!
24   \brief           Compute the length normalized correlation of two signals
25 
26   \sigX            Pointer to signal 1
27   \sigY            Pointer to signal 2
28   \len             Length of signals
29   \enableCrossCorr Flag to be set to 1 if cross-correlation is needed
30 
31   \return          First value is vector of correlation peak indices
32                    Second value is vector of correlation peak values
33 */
34 
correlation(const int16_t * sigX,const int16_t * sigY,int len,int16_t enableCrossCorr)35 static std::pair<std::vector<int>, std::vector<float>> correlation(const int16_t* sigX,
36                                                                    const int16_t* sigY, int len,
37                                                                    int16_t enableCrossCorr) {
38     float maxCorrVal = 0.f, prevCorrVal = 0.f;
39     int peakIndex = 0, flag = 0;
40     int loopLim = (1 == enableCrossCorr) ? len : kMinLoopLimitValue;
41     std::vector<int> peakIndexVect(kNumPeaks, 0);
42     std::vector<float> peakValueVect(kNumPeaks, 0.f);
43     for (int i = 0; i < loopLim; i++) {
44         float corrVal = 0.f;
45         for (int j = i; j < len; j++) {
46             corrVal += (float)(sigX[j] * sigY[j - i]);
47         }
48         corrVal /= len - i;
49         if (corrVal > maxCorrVal) {
50             maxCorrVal = corrVal;
51         }
52         // Correlation peaks are expected to be observed at equal intervals. The interval length is
53         // expected to match with wave period.
54         // The following block of code saves the first kNumPeaks number of peaks and the index at
55         // which they occur.
56         if (peakIndex < kNumPeaks) {
57             if (corrVal > prevCorrVal) {
58                 peakIndexVect[peakIndex] = i;
59                 peakValueVect[peakIndex] = corrVal;
60                 flag = 0;
61             } else if (0 == flag) {
62                 peakIndex++;
63                 flag = 1;
64             }
65         }
66         if (peakIndex == kNumPeaks) break;
67         prevCorrVal = corrVal;
68     }
69     return {peakIndexVect, peakValueVect};
70 }
71 
printUsage()72 void printUsage() {
73     printf("\nUsage: ");
74     printf("\n     correlation <firstFile> <secondFile> [enableCrossCorr]\n");
75     printf("\nwhere, \n     <firstFile>       is the first file name");
76     printf("\n     <secondFile>      is the second file name");
77     printf("\n     [enableCrossCorr] is flag to set for cross-correlation (Default 1)\n\n");
78 }
79 
main(int argc,const char * argv[])80 int main(int argc, const char* argv[]) {
81     if (argc < 3) {
82         printUsage();
83         return EXIT_FAILURE;
84     }
85 
86     std::unique_ptr<FILE, decltype(&fclose)> fInput1(fopen(argv[1], "rb"), &fclose);
87     if (fInput1.get() == NULL) {
88         printf("\nError: missing file %s\n", argv[1]);
89         return EXIT_FAILURE;
90     }
91     std::unique_ptr<FILE, decltype(&fclose)> fInput2(fopen(argv[2], "rb"), &fclose);
92     if (fInput2.get() == NULL) {
93         printf("\nError: missing file %s\n", argv[2]);
94         return EXIT_FAILURE;
95     }
96     int16_t enableCrossCorr = (4 == argc) ? atoi(argv[3]) : 1;
97 
98     fseek(fInput1.get(), 0L, SEEK_END);
99     unsigned int fileSize1 = ftell(fInput1.get());
100     rewind(fInput1.get());
101     fseek(fInput2.get(), 0L, SEEK_END);
102     unsigned int fileSize2 = ftell(fInput2.get());
103     rewind(fInput2.get());
104     if (fileSize1 != fileSize2) {
105         printf("\nError: File sizes different\n");
106         return EXIT_FAILURE;
107     }
108 
109     size_t numFrames = fileSize1 / sizeof(int16_t);
110     std::unique_ptr<int16_t[]> inBuffer1(new int16_t[numFrames]());
111     std::unique_ptr<int16_t[]> inBuffer2(new int16_t[numFrames]());
112 
113     if (numFrames != fread(inBuffer1.get(), sizeof(int16_t), numFrames, fInput1.get())) {
114         printf("\nError: Unable to read %zu samples from file %s\n", numFrames, argv[1]);
115         return EXIT_FAILURE;
116     }
117 
118     if (numFrames != fread(inBuffer2.get(), sizeof(int16_t), numFrames, fInput2.get())) {
119         printf("\nError: Unable to read %zu samples from file %s\n", numFrames, argv[2]);
120         return EXIT_FAILURE;
121     }
122 
123     auto pairAutoCorr1 = correlation(inBuffer1.get(), inBuffer1.get(), numFrames, enableCrossCorr);
124     auto pairAutoCorr2 = correlation(inBuffer2.get(), inBuffer2.get(), numFrames, enableCrossCorr);
125 
126     // Following code block checks pitch period difference between two input signals. They must
127     // match as AGC applies only gain, no frequency related computation is done.
128     bool pitchMatch = false;
129     for (unsigned i = 0; i < pairAutoCorr1.first.size() - 1; i++) {
130         if (pairAutoCorr1.first[i + 1] - pairAutoCorr1.first[i] !=
131             pairAutoCorr2.first[i + 1] - pairAutoCorr2.first[i]) {
132             pitchMatch = false;
133             break;
134         }
135         pitchMatch = true;
136     }
137     if (pitchMatch) {
138         printf("Auto-correlation  : Pitch matched\n");
139     } else {
140         printf("Auto-correlation  : Pitch mismatch\n");
141         return EXIT_FAILURE;
142     }
143 
144     if (enableCrossCorr) {
145         auto pairCrossCorr =
146                 correlation(inBuffer1.get(), inBuffer2.get(), numFrames, enableCrossCorr);
147 
148         // Since AGC applies only gain, the pitch information obtained from cross correlation data
149         // of input and output is expected to be same as the input signal's pitch information.
150         pitchMatch = false;
151         for (unsigned i = 0; i < pairCrossCorr.first.size() - 1; i++) {
152             if (pairAutoCorr1.first[i + 1] - pairAutoCorr1.first[i] !=
153                 pairCrossCorr.first[i + 1] - pairCrossCorr.first[i]) {
154                 pitchMatch = false;
155                 break;
156             }
157             pitchMatch = true;
158         }
159         if (pitchMatch) {
160             printf("Cross-correlation : Pitch matched for AGC\n");
161             if (pairAutoCorr1.second[0]) {
162                 printf("Expected gain     : (maxCrossCorr / maxAutoCorr1) = %f\n",
163                        pairCrossCorr.second[0] / pairAutoCorr1.second[0]);
164             }
165         } else {
166             printf("Cross-correlation : Pitch mismatch\n");
167             return EXIT_FAILURE;
168         }
169     }
170 
171     return EXIT_SUCCESS;
172 }
173