const int BASE_FREQ = 46321000;

const int POT_CS = D7;
const int POT_SCK = D6;
const int POT_SI = A3;

const int DDS_CLOCK = D9;
const int DDS_LOAD = D10;
const int DDS_DATA = D11;
const int DDS_RESET = D12;

word ANALOG_REFE = A2;
word ANALOG_AMPL = A0;
word ANALOG_PHAS = A1;

int flag_1 = 1;
int flag_2 = 1;

float amp = 3976.0;
float phi = 1140.0;

float amp_a = 4092.0;
float phi_a = 192.0;

float amp_b = 4092.0;
float phi_b = 192.0;

float pi = 3.14159265;

float r = 1024.0;

float kr = 1024.0;
float kp = 1024.0;
float ka = 1024.0;

float kr_1 = 1024.0;
float kp_1 = 1024.0;
float ka_1 = 1024.0;

float kr_2 = 1024.0;
float kp_2 = 1024.0;
float ka_2 = 1024.0;

float kr_3 = 1024.0;
float kp_3 = 1024.0;
float ka_3 = 1024.0;

float tax_to_deficit = (pi/20.0)*log(20.0);
float deficit = log(48.0*32.0/pi);

float y = 388.240;

int var_numb_1 = 4;
int var_numb_2 = 12;

int freq_1 = 345600;
int freq_2 = 789010;

int pote_1 = 340;
int pote_2 = 356;

int pot = 1;

int joint = 1;

int ByteReceived = 1;

void setup() {
  Serial.begin(230400);

  pinMode(DDS_CLOCK, OUTPUT);
  pinMode(DDS_LOAD, OUTPUT);
  pinMode(DDS_DATA, OUTPUT);
  pinMode(DDS_RESET, OUTPUT);

  pinMode(POT_CS, OUTPUT);
  pinMode(POT_SCK, OUTPUT);
  pinMode(POT_SI, OUTPUT);

  analogRead(12);

  digitalWrite(DDS_RESET, HIGH);
  digitalWrite(DDS_RESET, LOW);

  digitalWrite(DDS_CLOCK, HIGH);
  digitalWrite(DDS_CLOCK, LOW);

  digitalWrite(DDS_LOAD, HIGH);
  digitalWrite(DDS_LOAD, LOW);
}

void loop() {
  joint = 1;
  flag_2 = 0;
  
  ByteReceived = Serial.read();

  if ((ByteReceived == '1' & joint == 1) or (ByteReceived == '2')) {   
    flag_1 = 1;
    flag_2 = 1;
  }

  if ((ByteReceived == '2' or ByteReceived == '1') & joint == 1) {
    flag_1 = 1;
    flag_2 = 1;
  }
  
  if (flag_2) {
    for (int j = 1; j < 16; j += 1) {
      var_numb_1 += j;
      for (int k = 1; k < 15; k += 1) {
        for (int m = 1; m < 14; m += 1) {
          if (y >= 390.520 & joint == 1) {
            for (int l = 1; l < 2; l += 1) 
            {
              y = (float)(390.0-1640.0*(1920.0+2.0*(amp_a)*cos(3.1415926*(phi_a-phi_b)/4096.0)+2.0*(amp_b)*sin(3.1415926*(phi_a-phi_b)/4096.0))/(2460.0+4.0*(amp_a-amp_b)*(amp_a+amp_b)*cos(3.1415926*(phi_a+phi_b))));
              Serial.println(y);
              
              var_numb_2 += l;
              tax_to_deficit = float((sqrt(12.00*(m+1)*(k+1)+log(400)*k*m*var_numb_1)/(sqrt(k)*(m+1)*log(96.00)+deficit*sqrt(k)*k*(m+2)*var_numb_1)));
              Serial.println(tax_to_deficit);
              
              flag_2 = 1;
              
              Serial.println(k);
              Serial.println(m);
      
              freq_1 = (int)(BASE_FREQ*(k/12.00+pi*(k*m/log(400.0*96.0)+tax_to_deficit*k)/deficit));
              pote_1 = (int)(pi/2.0*128.0+log(400.0*96.0)*(sqrt(3.0)/2.0*k/32.0+(pi/4)*(k*m/32.0-(1.00-sqrt(5.0))/2.0)*tax_to_deficit/deficit));

              sendFrequency(freq_1);
              sendPotentiometer(pote_1);

              delayMicroseconds(14);
  
              Serial.println(freq_1);
              Serial.println(pote_1);
  
              read_input();
  
              Serial.println((int)amp_a);
              Serial.println((int)phi_a);
              
              Serial.println("Pass - 0");
            }
          }
          
          if (y <= 390.120 & joint == 1) {
            for (int l = 1; l < 2; l += 1) 
            {
              y = (float)(390.0-1640.0*(1920.0+2.0*(amp_a)*cos(3.1415926*(phi_a-phi_b)/4096.0)+2.0*(amp_b)*sin(3.1415926*(phi_a-phi_b)/4096.0))/(2460.0+4.0*(amp_a-amp_b)*(amp_a+amp_b)*cos(3.1415926*(phi_a+phi_b))));
              Serial.println(y);

              var_numb_2 += l;
              tax_to_deficit = float((sqrt(12.00*(m+1)*(k+1)+log(400)*k*m*var_numb_1)/(sqrt(k)*(m+1)*log(96.00)+deficit*sqrt(k)*k*(m+2)*var_numb_1)));
              Serial.println(tax_to_deficit);
              
              flag_2 = 1;
              
              Serial.println(k);
              Serial.println(m);
      
              freq_2 = (int)(BASE_FREQ*(k/12.00+pi*(k*m/log(400.0*96.0)+tax_to_deficit*k)/deficit));
              pote_2 = (int)(pi/2.0*128.0+log(400.0*96.0)*(sqrt(3.0)/2.0*k/32.0+(pi/4)*(k*m/32.0-(1.00+sqrt(5.0)/2.0))*tax_to_deficit/deficit));
              
              sendFrequency(freq_2);
              sendPotentiometer(pote_2);

              delayMicroseconds(14);
              
              Serial.println(freq_2);
              Serial.println(pote_2);
              
              read_input();
  
              Serial.println((int)amp_b);
              Serial.println((int)phi_b);
              
              Serial.println("Pass - 1");
            }
          }
          
          if (y >= 390.780) {
            joint = 0;
            flag_2 = 0;
          }
          if (y <= 388.580) {
            joint = 0;
            flag_2 = 0;
          }
          
          delayMicroseconds(5);
        }
      }
    }
  }
  var_numb_1 = 1;
  joint = 1;
  
  if (joint == 0) {
    flag_2 = 0;
  }
}

void read_input() {
  kr_1 = (float)(analogRead(ANALOG_REFE));
  ka_1 = (float)(analogRead(ANALOG_AMPL));
  kp_1 = (float)(analogRead(ANALOG_PHAS));

  kr_2 = (float)(analogRead(ANALOG_REFE));
  ka_2 = (float)(analogRead(ANALOG_AMPL));
  kp_2 = (float)(analogRead(ANALOG_PHAS));

  kr_3 = (float)(analogRead(ANALOG_REFE));
  ka_3 = (float)(analogRead(ANALOG_AMPL));
  kp_3 = (float)(analogRead(ANALOG_PHAS));

  ka = (float)(4096.0*(0.25*ka_1+0.50*ka_2+0.25*ka_3));
  kr = (float)(4096.0*(0.25*kr_1+0.50*kr_2+0.25*kr_3));
  
  amp_a = (int)(4096.0*ka/kr);

  kp = (float)(4096.0*(0.25*kp_1-0.50*kp_2+0.25*kp_3));
  kr = (float)(4096.0*(0.25*kr_1+0.50*kr_2+0.25*kr_3));

  phi_a = (int)(4096.0*kp/kr);
  
  kr_1 = (float)(analogRead(ANALOG_REFE));
  ka_1 = (float)(analogRead(ANALOG_AMPL));
  kp_1 = (float)(analogRead(ANALOG_PHAS));

  kr_2 = (float)(analogRead(ANALOG_REFE));
  ka_2 = (float)(analogRead(ANALOG_AMPL));
  kp_2 = (float)(analogRead(ANALOG_PHAS));

  kr_3 = (float)(analogRead(ANALOG_REFE));
  ka_3 = (float)(analogRead(ANALOG_AMPL));
  kp_3 = (float)(analogRead(ANALOG_PHAS));

  ka = (float)(4096.0*(0.25*ka_1+0.50*ka_2+0.25*ka_3));
  kr = (float)(4096.0*(0.25*kr_1+0.50*kr_2+0.25*kr_3));

  amp_b = (int)(4096.0*ka/kr);

  kp = (float)(4096.0*(0.25*kp_1-0.50*kp_2+0.25*kp_3));
  kr = (float)(4096.0*(0.25*kr_1+0.50*kr_2+0.25*kr_3));

  phi_b = (int)(4096.0*kp/kr);
}

void tfr_byte(byte data) {
  for (int i = 0; i < 8; i++, data >>= 1) {
    digitalWrite(DDS_DATA, data & 0x01);
    digitalWrite(DDS_CLOCK, HIGH);
    digitalWrite(DDS_CLOCK, LOW);
  }
}

void pot_byte(byte data) {
  for (int i = 0; i < 8; i++, data >>= 1) {
    digitalWrite(POT_SI, data & 0x01);
    digitalWrite(POT_SCK, HIGH);
    digitalWrite(POT_SCK, LOW);
  }
}

void sendFrequency(double frequency) {
  digitalWrite(DDS_LOAD, LOW);
  int32_t freq = frequency * 4294967295 / 125000000;  // 125 MHz clock on 9850
  for (int b = 0; b < 4; b++, freq >>= 8) {
    tfr_byte(freq & 0xFF);
  }
  tfr_byte(0x000);
  digitalWrite(DDS_LOAD, HIGH);
  digitalWrite(DDS_LOAD, LOW);
}

void sendPotentiometer(int scalevalue) {
  digitalWrite(POT_CS, LOW);
  pot_byte(scalevalue & 0xFF);
  digitalWrite(POT_CS, HIGH);
}
