#include <MemoryFree.h>

#define FASTADC 1
#define SAMPLES 50

#include <math.h>

#define INPUT_NEURONS 1
#define HIDDEN_NEURONS 1
#define OUTPUT_NEURONS 1

// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

struct input_neuron{
        double value;
} input_layer[INPUT_NEURONS];

struct hidden_neuron{
    double value;
    double weights[INPUT_NEURONS+1]; //+1 bias
} hidden_layer[HIDDEN_NEURONS];

struct output_neuron{
    double value;
    double weights[HIDDEN_NEURONS]; //+1 bias
} output_layer[OUTPUT_NEURONS];

MemoryFree mf;
// MATLAB FOR 2 MICS SO FREQ IS 31250 Hz
// [bC,aC]=impinvar([62312.8,0],[1,8769.08,4.26601e8,1.78616e12,2.91404e16],31250);
// [bI,aI]=impinvar([0.000560749,2.45862,114218,0],[1,8769.08,4.26601e8,1.78616e12,2.91404e16],31250);
// bC_int = round(bC*1e12);
// aC_int = round(aC*2^12);
// bI_int = round(bI*1e12);
// aI_int = round(aI*2^12);
//long bI_int[] = {17944L, -46322L, 42690L, -14076L};
//long aI_int[] = {4096L, -13753L, 18621L, -11953L, 3094L};
//long bC_int[] = {0L, 898L, -80L, -818L};
//long aC_int[] = {4096L, -13753L, 18621L, -11953L, 3094L};
//float bI_float[] = {4.3809, -11.3091, 10.4224, -3.4365};
//float aI_float[] = {1, -3.3577, 4.5461, -2.9182, 0.7554};
//float bC_float[] = {0, 0.2192, -0.0195, -0.1997};
//float aC_float[] = {1, -3.3577, 4.5461, -2.9182, 0.7554};

// FOR 3 MICS SO FREQ IS 20833 Hz
float bI_float[] = {6.5713, -14.4548, 12.4941, -4.2151};
float aI_float[] = {1, -2.8159, 3.5573, -2.2815, 0.6564};
float bC_float[] = {0, 0.6755, -0.0872, -0.5884};
float aC_float[] = {1, -2.8159, 3.5573, -2.2815, 0.6564};
//SOUND SOURCE LEFT
//float mic0[SAMPLES] = {398,359,316,276,231,193,167,151,147,163,184,219,259,305,345,384,415,432,437,430,409,376,339,295,251,210,176,155,147,153,173,204,240,284,327,367,401,423,435,434,417,391,353,313,268,227,188,161,147,147};
//float mic1[SAMPLES] = {368,327,286,246,207,177,158,153,158,176,204,243,281,323,359,391,413,423,420,404,380,345,305,263,223,189,164,152,152,166,192,225,264,303,344,378,404,420,423,412,391,360,323,280,242,204,175,155,151,159};
//float mic0[SAMPLES] = {170,151,146,163,192,213,255,304,341,374,407,423,426,425,399,375,334,297,251,216,184,162,148,154,177,204,238,278,323,359,389,417,431,420,404,381,355,307,271,233,185,159,156,148,159,182,217,254,304,344};
//float mic1[SAMPLES] = {160,159,164,178,224,255,291,327,370,390,405,417,409,387,363,327,291,249,211,184,161,153,161,172,205,240,275,315,344,385,406,407,409,399,376,337,303,265,225,192,171,155,152,163,184,213,248,291,329,368};
//SOUND SOURCE RIGHT
float mic0[SAMPLES];// = {412,419,415,401,371,344,305,268,230,199,172,157,160,167,189,222,262,302,339,361,396,403,398,399,385,355,320,284,242,213,176,162,157,159,175,209,241,281,321,361,382,401,406,411,396,371,339,302,264,224};
float mic1[SAMPLES];// = {371,388,401,400,386,376,339,305,274,241,207,184,172,170,172,190,217,241,283,313,344,365,376,385,384,375,353,324,287,252,220,188,179,167,170,179,199,231,263,302,335,364,375,390,391,377,363,329,310,267};
float mic2[SAMPLES];
float mic1_I[SAMPLES];
float mic1_C[SAMPLES];
float mic0_I[SAMPLES];
float mic0_C[SAMPLES];

// SOME VARIABLES TO CONTROL THE SERVO
int servoPin = 2;     // Control pin for servo motor
int minPulse = 900;   // Minimum servo position
int maxPulse = 2100;  // Maximum servo position
int pulse = 1500;        // Amount to pulse the servo, starts in the center
int refreshTime = 20; // the time needed in between pulses

// to store 10 values to calculate the standard deviation.
float std_values0[10];
float std_values1[10];
float std_values2[10];
int counter_values0 = 0;
int counter_values1 = 0;
int counter_values2 = 0;

void setup() {

  #if FASTADC
  // set prescale to 16
  sbi(ADCSRA,ADPS2) ;
  cbi(ADCSRA,ADPS1) ;
  cbi(ADCSRA,ADPS0) ;
  #endif

  Serial.begin(9600);
  //Serial.print("memory ini1 ");
  //Serial.println(mf.freeMemory());

  double hidden_weights[] = {0.90359398, -0.12097949};
  double output_weights[] = {-1.3631867, 0.3051893};

  set_weights(hidden_weights, output_weights);
  Serial.println("ini");
  Serial.println(output_layer[0].weights[0]);
Serial.println(output_layer[0].weights[1]);
}


void loop() {
  //unsigned long time1, time2;
  //time1= micros();
  for (int i = 0; i<SAMPLES; i++) {
   mic0[i] = 0;
   mic1[i] = 0;
   mic2[i] = 0;
  }

  for (int i=0; i<SAMPLES; i++) {
   mic1[i] = analogRead(A1)-280;
   mic0[i] = analogRead(A0)-280;
   mic2[i] = analogRead(A2)-280;
  }

  /*Serial.println("mic0");
  for (int i=0; i<SAMPLES; i++) {
    Serial.println(mic0[i]);
  }
  Serial.println("mic1");
  for (int i=0; i<SAMPLES; i++) {
    Serial.println(mic1[i]);
  }
  Serial.println("mic2");
  for (int i=0; i<SAMPLES; i++) {
    Serial.println(mic2[i]);
  }*/

  float dir = calculateDirection(mic0,mic1);
  float avg1 = calculateAvgAndStd(dir,0);

  dir = calculateDirection(mic0,mic2);
  float avg2 = calculateAvgAndStd(dir,1);

  dir = calculateDirection(mic2,mic1);
  float avg3 = calculateAvgAndStd(dir,2);

  //if (avg1 > -50) {
    double inputs[] = {1.0};
    feed_forward(inputs);
    for (int i=0; i<OUTPUT_NEURONS; i++) {
      Serial.print("output ");
      Serial.print(output_layer[i].value);
      Serial.print(" ");
    }
    delay(500);
    Serial.println(" ");
  //}

  //time2=micros();
  //Serial.println(time2-time1);

  //Serial.print("memory2 ");
  //Serial.println(1024*8-mf.freeMemory());
}

float calculateAvgAndStd(float dir, int control) {

  float ret = -99;

  if (control == 0) {
    std_values0[counter_values0] = dir;
    counter_values0++;
  }
  else if (control == 1) {
    std_values1[counter_values1] = dir;
    counter_values1++;
  }
  else if (control == 2) {
    std_values2[counter_values2] = dir;
    counter_values2++;
  }

  if (counter_values0 > 9 || counter_values1 > 9 ||
      counter_values2 > 9) {
    float sum0 = 0;
    float sum1 = 0;
    float sum2 = 0;
    for (int i=0; i<10; i++) {
      if (control == 0) sum0 = sum0 + std_values0[i];
      else if (control == 1) sum1 = sum1 + std_values1[i];
      else if (control == 2) sum2 = sum2 + std_values2[i];
    }
    float avg_std0 = sum0 / 10;
    float avg_std1 = sum1 / 10;
    float avg_std2 = sum2 / 10;
    if (control == 0) {
      Serial.print("0 and 1: ");
      ret = avg_std0;
      Serial.print(avg_std0);
      Serial.print("    ");
    }
    else if (control == 1) {
      Serial.print("0 and 2: ");
      Serial.print(avg_std1);
      ret = avg_std1;
      Serial.print("    ");
    }
    else if (control == 2) {
      Serial.print("1 and 2: ");
      Serial.print(avg_std2);
      ret = avg_std2;
      Serial.print("    ");
    }

    double var0 = 0;
    double var1 = 0;
    double var2 = 0;
    for (int i=0; i<10; i++) {
      if (control == 0) {
        double temp = std_values0[i]-avg_std0;
        var0 = var0 + temp*temp;
      }
      else if (control == 1) {
        double temp = std_values1[i]-avg_std1;
        var1 = var1 + temp*temp;
      }
      else if (control == 2) {
        double temp = std_values2[i]-avg_std2;
        var2 = var2 + temp*temp;
      }
    }
    float std_dev;
    if (control==0) std_dev = sqrt(var0 / 10);
    else if (control==1) std_dev = sqrt(var1 / 10);
    else if (control==2) std_dev = sqrt(var2 / 10);

    Serial.print(std_dev);
    Serial.print("    ");

    if (control == 0) {
      counter_values0 = 0;
    }
    if (control == 1) counter_values1 = 0;
    if (control == 2 ) {
      counter_values2 = 0;
      Serial.println();
      delay(1000);
    }
  }
  return ret;
}

float calculateDirection(float mic0[], float mic1[]) {

  convolution(mic0, bC_float, aC_float, mic0_C);
  convolution(mic0, bI_float, aI_float, mic0_I);
  convolution(mic1, bC_float, aC_float, mic1_C);
  convolution(mic1, bI_float, aC_float, mic1_I);

  addSignals(mic0_I, mic1_C);
  addSignals(mic0_C, mic1_I);

  absoluteSignal(mic0_I);
  absoluteSignal(mic0_C);

  float avg1 = average(mic0_I);
  float avg2 = average(mic0_C);

  return avg1 / avg2;
}

void moveRobot(float dir) {
  //Using 1.5khz, 86...90 center, lower left, bigger right
  if (dir < 0.79 ) pulse-=150;
  else if (dir > 0.94) pulse+=150;
  else if ((dir>=0.79) && (dir < 0.86)) pulse-=70;
  else if ((dir <= 0.94) && (dir > 0.90)) pulse +=70;
  if (pulse < 900) pulse = 900;
  if (pulse > 2100) pulse = 2100;

  //Serial.println(pulse);
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(pulse);
  digitalWrite(servoPin, LOW);
  delay(refreshTime);
}

void convolution(float in_signal[], float b[], float a[], float out_signal[])
{
  int bn = 4;
  int an = 5;
  int nT = SAMPLES;
  float pad_in[SAMPLES+8];
  float local_out[SAMPLES+8];

  for (int i=0; i<(SAMPLES+8); i++) {
    if (i<4) pad_in[i] = 0;
    else if (i>(SAMPLES+8)-5) pad_in[i] = 0;
    else pad_in[i] = in_signal[i-4];
    local_out[i]=0;
  }

  for (int i=4; i<(SAMPLES+8)-5; i++) {
    for (int j=0; j<bn; j++) {
      local_out[i] = local_out[i] + b[j] *pad_in[i-j];
    }
  }
  for (int i=4; i<(SAMPLES+8)-5; i++) {
    for (int j=1; j<an; j++) {
      local_out[i] = local_out[i] - a[j] * local_out[i-j];
    }
  }
  for (int i=4; i<(SAMPLES+8)-5; i++) {
    out_signal[i-4] = local_out[i];
  }
}

//s1 in/out, s2 in. saving memory
void addSignals(float s1[], float s2[])
{
  for (int i=0; i<SAMPLES; i++)
  {
    s1[i] = s1[i] + s2[i];
  }
}

void absoluteSignal(float in[])
{
  for (int i=0; i<SAMPLES; i++)
  {
    if (in[i] < 0) { in[i] = -1*in[i]; }
  }
}

long average(float in[])
{
  float sum = 0;
  for (int i=0; i<SAMPLES; i++)
  {
    sum = sum + in[i];
  }
  return sum / (SAMPLES);
}

double sigmoid(double t) {
    return 1 / (1+exp(-t));
}

void feed_forward(double inputs[INPUT_NEURONS]) {

    int i, j;

    for (i=0; i<INPUT_NEURONS; i++) {
        input_layer[i].value = inputs[i];
    }

    for (i=0; i<HIDDEN_NEURONS; i++) {
        double sum = 0.0;
        for (j=0; j<INPUT_NEURONS; j++) {
            sum = sum + hidden_layer[i].weights[j] * input_layer[j].value;
        }
        sum = sum + hidden_layer[i].weights[INPUT_NEURONS];
        hidden_layer[i].value = sigmoid(sum);
    }

    for (i=0; i<OUTPUT_NEURONS; i++) {
        double sum = 0.0;
        for (j=0; j<HIDDEN_NEURONS; j++) {
            sum = sum + output_layer[i].weights[j] * hidden_layer[j].value;
        }
Serial.println(output_layer[i].weights[0]);
Serial.println(output_layer[i].weights[1]);
        sum = sum + output_layer[i].weights[HIDDEN_NEURONS];
        output_layer[i].value = sum;
Serial.println(output_layer[i].value);
    }

}

void set_weights(double hidden_weights[INPUT_NEURONS*HIDDEN_NEURONS],
                double output_weights[HIDDEN_NEURONS*OUTPUT_NEURONS]) {

    int i,j;

    for (i=0; i<HIDDEN_NEURONS; i++) {
        for (j=0; j<INPUT_NEURONS+1; j++) {
            hidden_layer[i].weights[j] = hidden_weights[i*(INPUT_NEURONS+1)+j];
        }
    }

    for (i=0; i<OUTPUT_NEURONS; i++) {
        for (j=0; j<HIDDEN_NEURONS+1; j++) {
            output_layer[i].weights[j] = output_weights[i*(HIDDEN_NEURONS+1)+j];
        }
    }

}