File content as of revision 2:1b02b22417bd:

#pragma once
#include "mbed.h"
#include <cmath>

const int MAX_SENSORS = 10;
const int ITERATIONS = 3;
/** A
*      }
* }
* @endcode
*/

class AoA_Est {

public:
    /** Create a
    *
    * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
    *
    * @note Supported types of sensors:
    */
    AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq);
    float estimate(float phases[], float amp[]);
    bool calibrate();


    int confidence;

private:
    float estimate_Theoretical(float phases[], float amp[]);
    float estimate_Calibrated(float phases[], float amp[]);
    void comparative_Phases(float phas[]);
    float angle_Resolver();
    float distanceFinder(float phase);
    int sensors;

    int x[MAX_SENSORS];
    int y[MAX_SENSORS];
    float phases[MAX_SENSORS - 1];
    float sensorSep[MAX_SENSORS];
    float sensorAngles[MAX_SENSORS];
    float amplitudes[MAX_SENSORS];
    int z[2];
    float ambigAngles[2][MAX_SENSORS+ITERATIONS];
    float wavelength;

};
AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors)
{
    wavelength = (338.4 / freq)*1000;
    for (short i = 0; i < sensors-1; i++) {
        x[i] = xPassed[i];
        y[i] = yPassed[i];
        sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i]));
        sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535;
    }
}

float AoA_Est::distanceFinder(float phase) {
    return phase / 360 * wavelength;
}

float AoA_Est::estimate_Theoretical(float phases[], float amp[]) {
    float distance = 0;
    float angle = 0;

    for (int i = 0; i < sensors-1; i++) {
        distance = distanceFinder(phases[i]);
        angle = acos(distance / sensorSep[i])*180/3.1415923535;
        ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/-
        ambigAngles[1][i] = sensorAngles[i] + angle;
    //  if (distance > 0) {
            //ambigAngles[0][i] = (int(ambigAngles[0][i]) + 180) % 360;  //Check
            //ambigAngles[1][i] = (int(ambigAngles[1][i]) + 180) % 360; //Not sure
    //      ambigAngles[0][i] = angle - sensorAngles[i];
    //      ambigAngles[1][i] = sensorAngles[i] + angle;
    //  }
        ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i];
        ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i];
    }
    
    float phas_diff = 0;
    float relative_angle = 0;
    float relative_dist = 0;
    for (int i = 1; i < sensors - 1; i++) {
        while (i < sensors - 1) {
            phas_diff = phases[i - 1] - phases[i];
            if (abs(phas_diff) > 180) {
                phas_diff = (phas_diff < 0) ? phas_diff + 360 : phas_diff - 360;
            }
            distance = distanceFinder(phas_diff);
            relative_angle = atan2(float(y[i - 1] - y[i]), float((x[i - 1] - x[i]))) * 180 / 3.1415926535;
            relative_dist = sqrt(float((x[i - 1] - x[i]) *(x[i - 1] - x[i])) + float((y[i - 1] - y[i]) *(y[i - 1] - y[i])));
            angle = acos(distance / relative_dist) * 180 / 3.1415923535;
            ambigAngles[0][sensors - 2 + i] = relative_angle - angle;
            ambigAngles[1][sensors - 2 + i] = relative_angle + angle;

            ambigAngles[0][sensors - 2 + i] = (ambigAngles[0][sensors - 2 + i] < 0) ? ambigAngles[0][sensors - 2 + i] + 360 : ambigAngles[0][sensors - 2 + i];
            ambigAngles[1][sensors - 2 + i] = (ambigAngles[1][sensors - 2 + i] < 0) ? ambigAngles[1][sensors - 2 + i] + 360 : ambigAngles[1][sensors - 2 + i];
            i++;
        }
    }
    angle = angle_Resolver();
    return angle;
}

float AoA_Est::angle_Resolver() {
    float angle = ambigAngles[0][0];
    float avg = angle;
    bool flag = false;
    confidence = 0;
    for (short i = 1; i <= ITERATIONS; i++) {
        if (abs(angle - ambigAngles[0][i]) < abs(angle-ambigAngles[1][i]) && abs(angle-ambigAngles[0][i]) < 30) {
            angle = ambigAngles[0][i];
            avg += ambigAngles[0][i];
            confidence++;
        }
        else if (abs(angle - ambigAngles[1][i]) < abs(angle-ambigAngles[0][i]) && abs(angle - ambigAngles[1][i]) < 30) {
            angle = ambigAngles[1][i];
            avg += ambigAngles[1][i];
            confidence++;
        }
        else if (i == 1 && flag == false) {
            angle = ambigAngles[1][0];
            avg = angle;
            i = 0;
            flag = true;
        }

    }

    return avg / (ITERATIONS);//change when comute the other way
}

void AoA_Est :: comparative_Phases(float phas[]) {
    for (int i = 0; i < sensors - 1; i++) {
        phases[i] = phas[0] - phas[i + 1];
        if (abs(phases[i]) > 180) {
            phases[i] = (phases[i] < 0) ? phases[i] + 360 : phases[i] - 360;
        }

    }

}

float AoA_Est::estimate(float phas[], float amplitudes[]) {
    float angle;
    comparative_Phases(phas);
    angle = estimate_Theoretical(phases, amplitudes);

    return angle;

}