#define PIN_OUT1 A1
#define PIN_OUT2 A2
#define PinServo1 6 // Пин 1 сервопривода
#define PinServo2 8 // Пин 2 сервопривода
#define PinButG 9   // Пин зеленой кнопки
#define PinButR 10  // Пин красной кнопки
#define PIN_LED 3   // Пин светодиода
// Кнопки подключать к земле и к управляющему

#include <Servo.h>
Servo servo1;
Servo servo2;
int mode = 0;
int green, red;
#include <BMI160Gen.h>
#include "TroykaCurrent.h"
float offsetLS1, offsetLS2;
float multiplier = 0.185;
unsigned long myTime;
ACS712 dataI1(PIN_OUT1);
ACS712 dataI2(PIN_OUT2);

#include <SPI.h>
#include <SD.h>

File logFile;

void setup() {
   servo1.attach(PinServo1);
  servo2.attach(PinServo2);
  pinMode(PinButG, INPUT_PULLUP);
  pinMode(PinButR, INPUT_PULLUP);
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH); // Turn on the LED
  Serial.begin(9600); // initialize Serial communication
  while (!Serial); // wait for the serial port to open
  // initialize device
  //Serial.println("test3");
  //BMI160.begin(BMI160GenClass::I2C_MODE);
  //uint8_t dev_id = BMI160.getDeviceID();
  //uint8_t dev_id = 0x68;
  // Set the accelerometer range to 250 degrees/second
  BMI160.setGyroRange(250);
  BMI160.setAccelerometerRange(2);
  offsetLS1 = -(OffsetCurrent(A1));
  offsetLS2 = -(OffsetCurrent(A2));

  Serial.print("Initializing SD card...");

  if (!SD.begin(4)) {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");

  logFile = SD.open("log.txt", FILE_WRITE);

  // if the file opened okay, write to it:
  if (logFile) {
    Serial.println("Writing to log.txt...");
    Serial.println("ax;ay;az;L current;R current;EMG;Time Time, s"); // CSV headers
    logFile.println("ax;ay;az;L current;R current;EMG;Time Time, s");
    // close the file:
    logFile.close();
    Serial.println("done.");
  } else {
    // if the file didn't open, print an error:
    Serial.println("error opening log.txt");
  }
}

void moto () {
  int pot_signal = analogRead(A3);
  if (mode == 0) {
    servo1.writeMicroseconds(1540);
    servo2.writeMicroseconds(1540);
  }
  if (mode == 1) {
    int servo_micros = pot_signal * (996.0 / 1023.0);
    servo1.writeMicroseconds(1540 - servo_micros);
    servo2.writeMicroseconds(1540 - servo_micros);
  }
  if (mode == 2) {
    int servo_micros = pot_signal * (860.0 / 1023.0);
    servo1.writeMicroseconds(1540 + servo_micros);
    servo2.writeMicroseconds(1540 + servo_micros);
  }
}

void loop() {
    moto();
  if (red == 0) {
    if (digitalRead(PinButR) == 0) {
      delay(10);
      if (digitalRead(PinButR) == 0) {
        red = 1;
      }
    }
  }
  if (green == 0) {
    if (digitalRead(PinButG) == 0) {
      delay(10);
      if (digitalRead(PinButG) == 0) {
        green = 1;
      }
    }
  }

  if (red == 1) {
    if (digitalRead(PinButR) == 1) {
      delay(10);
      if (digitalRead(PinButR) == 1) {
        red = 0;
        if (mode == 0) {
          mode = 1;
        }
        else {
          mode = 0;
        }
      }
    }
  }  if (green == 1) {
    if (digitalRead(PinButG) == 1) {
      delay(10);
      if (digitalRead(PinButG) == 1) {
        green = 0;
        if (mode == 1) {
          mode = 2;
        }
        else if (mode == 2) {
          mode = 1;
        }
      }
    }
  }
  int gxRaw, gyRaw, gzRaw, axRaw, ayRaw, azRaw; // raw gyro values
  float gx, gy, gz, ax, ay, az;
  // read raw gyro measurements from device
  BMI160.readGyro(gxRaw, gyRaw, gzRaw);
  BMI160.readAccelerometer(axRaw, ayRaw, azRaw);
  // convert the raw gyro data to degrees/second
  gx = convertRawGyro(gxRaw);
  gy = convertRawGyro(gyRaw);
  gz = convertRawGyro(gzRaw);
  ax = convertRawAcc(axRaw);
  ay = convertRawAcc(ayRaw);
  az = convertRawAcc(azRaw);
  int mus = muscle_read(A0);
  float LS1_current, LS2_current;
  LS1_current = ReadCurrent(A1, offsetLS1);
  LS2_current = ReadCurrent(A2, offsetLS2);
  myTime = millis();

  logFile = SD.open("log.txt", FILE_WRITE);
  // display ;-separated acceleration x/y/z values
  Serial.print(ax); logFile.print(ax);
  Serial.print("\t"); logFile.print("\t");
  Serial.print(ay); logFile.print(ay);
  Serial.print("\t"); logFile.print("\t");
  Serial.print(az); logFile.print(az);
  Serial.print("\t"); logFile.print("\t");
  // display ;-separated current values from leeft and right servomotor
  Serial.print(LS1_current); logFile.print(LS1_current);
  Serial.print("\t"); logFile.print("\t");
  Serial.print(LS2_current); logFile.print(LS2_current);
  Serial.print("\t"); logFile.print("\t");
  // display ;-separated current values from leeft and right servomotor Power
  int I1 = dataI1.readCurrentDC() * 5.0;
  int I2 = dataI2.readCurrentDC() * 5.0;
  Serial.print(I1); logFile.print(I1);
  Serial.print("\t"); logFile.print("\t");
  Serial.print(I2); logFile.print(I2);
  Serial.print("\t"); logFile.print("\t");
  // display ;-Muscle Sensor EMG
  Serial.print(mus); logFile.print(mus);
  Serial.print("\t"); logFile.print("\t");
  // display time (since the device powerup)
  Serial.println(myTime); logFile.println(myTime);
  delay(10);
  logFile.close();
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

float convertRawAcc(int aRaw) {
  float a = aRaw / 16384.0;
  return a;
}
double muscle_read(int current_Pin) { // Getting the reading
  int num = 50;
  float SensorRead;
  float Total = 0.0;
  for (int i = 0; i <= num; i++)
  {
    SensorRead = analogRead(current_Pin);
    Total = Total + SensorRead;
  }
  Total = (Total / num);
  return (Total);
}

double ReadCurrent(int current_Pin, double offset) { // Getting the reading
  float SensorRead, Current; float Total = 0.0;
  for (int i = 0; i <= 100; i++)
  {
    SensorRead = analogRead(current_Pin) * (5.0 / 1023.0); //We read the sensor output
    Current = (SensorRead - 2.5) / multiplier; //Calculate the current value
    Total = Total + Current;
  }
  Total = (Total / 100) + offset;
  return (Total);
}

double OffsetCurrent(int current_Pin) {
  float SensorRead, Current; float Total = 0.0;
  for (int i = 0; i <= 1000; i++)
  {
    SensorRead = analogRead(current_Pin) * (5.0 / 1023.0); //We read the sensor output
    Current = (SensorRead - 2.5) / multiplier; //Calculate the current value
    Total = Total + Current;
  }
  Total = (Total / 1000);
  return (Total);
}