#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_ADS1015.h>

//I2C device found at address 0x3C  ! -> SSD1306
//I2C device found at address 0x48  ! -> ADS1115
//I2C device found at address 0x60  ! -> MCP4725

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define MCP4725 96 // Adresse 96 oder 97, wie Brücke
#define OLED_RESET     10 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_ADS1115 ads1115(0x48);

//IO pins
byte const io_enter = 2;
byte const io_right = 3;
byte const io_discharge = 4;
byte const io_akku_on = 5;
byte const io_left = 6;
byte const io_U_akku = A7;
byte const io_U_own = A0;
byte const io_U_temp = A1;

//control parameters
int dev_dawert = 3500;
int dawert = 3500;
int const step_up = 1;
int const step_down = 1;
int const search_step_up = 5;
int const search_step_down = 5;
int16_t time;
float T;
float T_start;
byte const T_count = 20;

//limit values
int U_akku_max = 0;
int U_akku_dis_min;
int U_akku_dis_max;
int U_akku_dis_tmp;
int U_akku_dis_tmp_step;
byte const dawert_min = 119;
byte const dawert_max = 131;

//sensor valueas
byte const U_rep = 50;
float dU[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float dU_xy = 0;
float dU_xx = 0;
float dU_X;
float dU_Y;
float dU_mid = 0;
float dU_mid_max = 0;

int U_akku;
int U_out;
int I_akku;
int32_t U_sum;
int16_t U;
int16_t coulomb_u;
float coulomb_v;
int16_t coulomb;
float U_temp;
float U_own;
float R_T;
float dT[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
const byte dT_count = 20;
byte dT_n;
float dT_mid;
const byte dT_t = 30; // t = dT_t(s) * 1024
int dT_sum = 0;
byte dT_position = 0;
int16_t dT_t_tmp = 0;
float dT_mid_max = 0;
float dT_xy = 0;
float dT_xx = 0;
float dT_X;
float dT_Y;

//temp sensor parameters
float const B = 3435.0;
float const R_R = 10000.0;
float const R_ST = 1000.0;

//menue
bool menu = true;
int sub_menu = 0;
byte position = 0;
byte menu_step;
byte old_position  = 0;
byte new_position  = 0;
bool press;
bool pressed;

bool select_type = true;
byte type;
String types[] = {"NiMh", "NiCd", "Alkali"};
byte const types_count = 3;

bool select_I_charge = true;
int I_charge;
int charges[] = {10, 20, 30, 40, 50, 80, 100, 150, 200, 250, 300, 350, 400, 450, 500};
byte const charges_count = 15;

bool select_discharge = true;
bool discharge = false;
bool direction = true;
bool max_discharge = true;
String discharges[] = {"true", "false"};
byte const discharge_count = 2;

//global bools
bool full = false;
bool search_mode = true;

void setup() {
  analogReference(INTERNAL4V096);
  pinMode(io_enter, INPUT);
  pinMode(io_right, INPUT);
  pinMode(io_discharge, ANALOG);
  pinMode(io_left, INPUT);
  pinMode(io_akku_on, OUTPUT);
  pinMode(io_U_akku, INPUT);

  digitalWrite(io_akku_on, LOW);
  analogWrite(io_discharge, 0);

  Wire.begin();
  writemcp4725(dev_dawert);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
  ads1115.begin();
}

void loop() {
  if (menu == true) {
    sub_menu = 0;
    position = (types_count * 5);
    while (select_type) {
      position = position + arrow();
      write_display_menu(sub_menu, position);
      select_type = enter();
    }
    type = position % types_count;
    sub_menu += 1;
    position = (charges_count * 5) + 6;
    while (select_I_charge) {
      write_display_menu(sub_menu, position);
      position = position + arrow();
      select_I_charge = enter();
    }
    I_charge = charges[position % charges_count];
    sub_menu += 1;
    position = (discharge_count * 5);
    while (select_discharge && type != 2) {
      write_display_menu(sub_menu, position);
      position = position + arrow();
      select_discharge = enter();
    }
    discharge = (position + 1) % discharge_count;
    menu = false;

    U_akku = map(U_measure(0,  2, 5), 0, 32768, 0, 2048);

    if (U_akku < 100) {
      menu = true;
      display.clearDisplay();
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(2);
      display.setCursor(0, 5);
      display.print("no battery");
      //print
      display.display();
      delay(2000);
    }
  }

  display.clearDisplay();
  display.display();

  if (discharge == true && type != 2) {
    dawert = 150;
    fnc_discharge();
  }

  for (int i = 0; i < dT_count; i++) {
    T = temp();
    U_akku = map(U_measure(0, 2, 20), 0, 32768, 0, 2048);
    dT[i] = T;
    dU[i] = U_akku;
  }

  switch (type) {
    case 0: //NiMh
      dawert = 4096;
      while (true) {
        charge_nimh();
      }
      break;
    case 1: //NiCd
      dawert = 4096;
      while (true) {
        charge_nicd();
      }
      break;
    case 2: //Alkali
      dawert = 4096;
      while (true) {
        charge_alkali();
      }
      break;
  }
}

bool enter() {
  press = false;
  pressed = true;
  if (digitalRead(io_enter) == HIGH) {
    delay(10);
    if (digitalRead(io_enter) == HIGH) {
      press = true;
    }
  }
  while (press) {
    if (digitalRead(io_enter) == LOW) {
      pressed = false;
      press = false;
    }
  }
  return pressed;
}

byte arrow() {
  press = false;
  menu_step = 0;
  if (digitalRead(io_right) == HIGH) {
    delay(10);
    if (digitalRead(io_right) == HIGH) {
      press = true;
    }
  }
  if (digitalRead(io_left) == HIGH) {
    delay(10);
    if (digitalRead(io_left) == HIGH) {
      press = true;
    }
  }
  while (press) {
    if (digitalRead(io_left) == LOW) {
      press = false;
      menu_step = 1;
    }
    if (digitalRead(io_right) == LOW) {
      press = false;
      menu_step = -1;
    }
  }
  delay(250);
  return menu_step;
}

void write_display_menu(int sub_menu, int position) {
  switch (sub_menu) {
    case 0: //select_type
      display.clearDisplay();
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("select batterie type");
      display.setTextSize(2);
      display.setCursor(0, 16);
      display.print(">");
      display.setCursor(20, 16);
      display.print(types[position % types_count]);
      //print
      display.display();
      break;
    case 1: //select_I_charge
      display.clearDisplay();
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("select I charge");
      display.setTextSize(2);
      display.setCursor(0, 16);
      display.print(">");
      display.setCursor(20, 16);
      display.print(charges[position % charges_count]);
      display.setCursor(10, 35);
      display.print("mA");
      //print
      display.display();
      break;
    case 2: //select_discharge
      display.clearDisplay();
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("discharge before load");
      display.setTextSize(2);
      display.setCursor(0, 16);
      display.print(">");
      display.setCursor(20, 16);
      display.print(discharges[position % discharge_count]);
      //print
      display.display();
      break;
  }
}

int16_t U_measure(byte channel, int gain, int rep) {
  switch (gain) {
    case 0:
      ads1115.setGain(GAIN_TWOTHIRDS);
      break;
    case 1:
      ads1115.setGain(GAIN_ONE);
      break;
    case 2:
      ads1115.setGain(GAIN_TWO);
      break;
    case 4:
      ads1115.setGain(GAIN_FOUR);
      break;
    case 8:
      ads1115.setGain(GAIN_EIGHT);
      break;
    case 16:
      ads1115.setGain(GAIN_SIXTEEN);
      break;
    case 101:
      //
      break;
    default:
      ads1115.setGain(GAIN_TWOTHIRDS);
      break;
  }
  U_sum = 0;
  for (int i = 0; i < rep; i++) {
    switch (channel) {
      case 0:
        U = ads1115.readADC_Differential_0_1();;
        break;
      case 1:
        U = ads1115.readADC_Differential_2_3();
        break;
      case 2:
        U = analogRead(io_U_akku);
        break;
    }
    U_sum += U;
  }
  U = U_sum / rep;
  return U;
}

void search_I(int I_charge, bool direction) {
  analogWrite(io_discharge, 0);
  digitalWrite(io_akku_on, LOW);
  I_akku = map(U_measure(1, 16, 10), 0, 32768, 0, 2560) * -1;
  writemcp4725(dawert);
  direction = true;
  while (direction) {
    U_out = U_measure(2,  101, 50);
    U_akku = map(U_measure(0,  2, 10), 0, 32768, 0, 2048);

    if (U_out > (U_akku + 10)) {
      direction = false;
      digitalWrite(io_akku_on, HIGH);
    } else {
      dawert -= search_step_down;
      writemcp4725(dawert);
      digitalWrite(io_akku_on, LOW);
    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //line 1 M
    display.setCursor(10, 0);
    display.print("<search voltage>");
    //line 2 L
    display.setCursor(0, 12);
    display.print("U_a: ");
    display.print(U_akku);
    //line 2 L
    display.setCursor(75, 12);
    display.print("I_a: ");
    display.print(I_akku);
    //line 3 L
    display.setCursor(0, 22);
    display.print("U_o: ");
    display.print(U_out);
    //print
    display.display();
  }

  while (I_akku != I_charge && !direction) {
    U_akku = map(U_measure(0,  2, 10), 0, 32768, 0, 2048);
    I_akku = map(U_measure(1, 16, 10), 0, 32768, 0, 2560) * -1;
    if (I_akku < I_charge) {
      dawert -= search_step_down;
      writemcp4725(dawert);
    } else {
      dawert += search_step_up;
      writemcp4725(dawert);
    }
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //line 1 M
    display.setCursor(10, 0);
    display.print("<search current>");
    //line 2 L
    display.setCursor(0, 12);
    display.print("U_a: ");
    display.print(U_akku);
    //line 2 R
    display.setCursor(75, 12);
    display.print("I_a: ");
    display.print(I_akku);
    //print
    display.display();
  }
}

void write_display_data() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  temp_dT();
  if (coulomb < I_charge && discharge) {
    dT_mid_max = 0;
    dU_mid_max = 0;
  }

  //line 1 L
  display.setCursor(0, 0);
  display.print("dUmU: ");
  if (dU_mid >= 0) {
    display.print(" ");
  }
  display.print(dU_mid, 0);
  display.setCursor(70, 0);
  if (dU_mid_max == 0) {
    display.print("--");
  } else {
    display.print(dU_mid_max, 0);
  }
  display.setCursor(100, 0);
  display.print(U_akku);

  //line 2 L
  display.setCursor(0, 12);
  display.print("I-Uh:  ");
  display.print(I_akku);
  display.setCursor(70, 12);
  if (U_akku_max - U_akku == 0) {
    display.print("--");
  } else {
    display.print(U_akku_max - U_akku);
  }
  display.setCursor(100, 12);
  display.print(fnc_coulomb());

  //line 3 L
  display.setCursor(0, 22);
  display.print("dTmT: ");
  if (dT_mid >= 0) {
    display.print(" ");
  }
  display.print(dT_mid, 1);
  display.setCursor(70, 22);
  if (dT_mid_max == 0) {
    display.print("--");
  } else {
    display.print(dT_mid_max, 1);
  }
  display.setCursor(100, 22);
  display.print(T, 1);

  display.display();
}

void writemcp4725(int wert) {
  // Analog Wert 0 bis 4095
  if (wert < 0) {
    wert = 0;
  }
  if (wert > 4095) {
    wert = 4095;
  }
  Wire.beginTransmission(MCP4725);
  Wire.write(64);               // Kommando Update DAC
  Wire.write(wert >> 4);        // die 8 höherwertigen Bits
  Wire.write((wert & 15) << 4); // die 4 niederwertigen Bits
  Wire.endTransmission();
}

float temp() {
  analogReference(INTERNAL1V024);
  T = 0;
  for (int i = 0; i < T_count; i++) {
    U_own = 14000;// analogRead(io_U_own) * 5; //voltage divider 5:1 => 5V == 1V (alternativ ~14000)
    U_temp = analogRead(io_U_temp);
    R_T = ((U_own / U_temp) * R_ST) - R_ST;
    T += pow(((log(R_T / R_R) / B) + (1 / 293.15)), -1);
  }
  analogReference(INTERNAL4V096);
  T = T / T_count;
  T = T - 273.15;
  T = T * 1.05;
  return T;
}

void temp_dT() {
  dT_sum += (millis() - dT_t_tmp);
  dT_t_tmp = millis();
  if (dT_sum > (dT_t * 1024)) {
    dT_sum = 0;
    dT[dT_position] = temp();
    dT_position += 1;
    if (dT_position == dT_count) {
      dT_position = 0;
    }

    //dT

    dT_xx = 0;
    dT_xy = 0;
    dT_n = 1;

    //avarage x
    dT_X = dT_count / 2;

    //avarage y
    dT_Y = 0;
    for (int i = 0; i < dT_count; i++) {
      dT_Y += dT[i];
    }
    dT_Y = dT_Y / dT_count;

    for (int i = dT_position; i < dT_count; i++) {
      dT_xy += (dT_n - dT_X) * (dT[i] - dT_Y);
      dT_xx += pow((dT_n - dT_X), 2);
      dT_n++;
    }

    for (int i = 0; i < dT_position; i++) {
      dT_xy += (dT_n - dT_X) * (dT[i] - dT_Y);
      dT_xx += pow((dT_n - dT_X), 2);
      dT_n++;
    }

    dT_mid = dT_xy / dT_xx;
    dT_mid = dT_mid * (3600 / dT_t);

    if (dT_mid_max < dT_mid) {
      dT_mid_max = dT_mid;
    }

    //dU

    dU[dT_position] = U_akku;

    dU_xx = 0;
    dU_xy = 0;
    dT_n = 1;

    //avarage x
    dU_X = dT_count / 2;

    //avarage y
    dU_Y = 0;
    for (int i = 0; i < dT_count; i++) {
      dU_Y += dU[i];
    }
    dU_Y = dU_Y / dT_count;

    for (int i = dT_position; i < dT_count; i++) {
      dU_xy += (dT_n - dU_X) * (dU[i] - dU_Y);
      dU_xx += pow((dT_n - dU_X), 2);
      dT_n++;
    }

    for (int i = 0; i < dT_position; i++) {
      dU_xy += (dT_n - dU_X) * (dU[i] - dU_Y);
      dU_xx += pow((dT_n - dU_X), 2);
      dT_n++;
    }

    dU_mid = dU_xy / dU_xx;
    dU_mid = dU_mid * (3600 / dT_t);

    if (dU_mid_max < dU_mid) {
      dU_mid_max = dU_mid;
    }

  }
}

void fnc_discharge () {
  switch (type) {
    case 0: //NiMh
      U_akku_dis_min = 1000;
      U_akku_dis_max = 1100;
      break;
    case 1: //NiCd
      U_akku_dis_min = 950;
      U_akku_dis_max = 1050;
      break;
    case 2: //Alkali
      break;
  }
  writemcp4725(200);
  dawert = dawert_min;
  U_akku_dis_tmp = U_akku_dis_max - ((U_akku_dis_max - U_akku_dis_min) / (dawert_max - dawert_min) * (dawert - dawert_min));
  U_akku_dis_tmp_step = ((U_akku_dis_max - U_akku_dis_min) / (dawert_max - dawert_min));
  analogWrite(io_discharge, dawert);
  discharge = true;
  max_discharge = true;

  while (max_discharge) {
    U_akku = map(U_measure(0, 2, 20), 0, 32768, 0, 2048);
    if (dawert == dawert_max || U_akku <= U_akku_dis_tmp) {
      max_discharge = false;
    } else {
      dawert += step_up;
      if (dawert > dawert_max) {
        dawert = dawert_max;
      }
      analogWrite(io_discharge, dawert);
      delay(1000);
    }
    U_akku_dis_tmp = U_akku_dis_max - ((U_akku_dis_max - U_akku_dis_min) / (dawert_max - dawert_min) * (dawert - dawert_min));

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //line 1 M
    display.setCursor(30, 0);
    display.print("<discharge>");
    //line 2 L
    display.setCursor(0, 12);
    display.print("U_a: ");
    display.print(U_akku);
    //line 2 R
    display.setCursor(70, 12);
    display.print("d_w: ");
    display.print(dawert - dawert_min);
    //line 3 L
    display.setCursor(0, 22);
    display.print("T_a: ");
    display.print(temp(), 1);
    //line 3 R
    display.setCursor(70, 22);
    display.print("U_m: ");
    display.print(U_akku_dis_tmp);
    //print
    display.display();
  }

  while (discharge) {
    U_akku = map(U_measure(0, 2, 20), 0, 32768, 0, 2048);
    if (dawert <= dawert_min) {
      discharge = false;
    }
    if (U_akku <= U_akku_dis_tmp) {
      U_akku_dis_tmp += U_akku_dis_tmp_step;
      dawert -= step_down;
      if (dawert < 0) {
        dawert = 0;
      }
      analogWrite(io_discharge, dawert);
      delay(1000);
    }
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    //line 1 M
    display.setCursor(30, 0);
    display.print("<discharge>");
    //line 2 L
    display.setCursor(0, 12);
    display.print("U_a: ");
    display.print(U_akku);
    //line 2 R
    display.setCursor(70, 12);
    display.print("d_w: ");
    display.print(dawert - dawert_min);
    //line 3 L
    display.setCursor(0, 22);
    display.print("T_a: ");
    display.print(temp(), 1);
    //line 3 R
    display.setCursor(70, 22);
    display.print("U_m: ");
    display.print(U_akku_dis_tmp);
    //print
    display.display();
  }
  analogWrite(io_discharge, 0);
  discharge = true;
}
int fnc_coulomb() {
  coulomb_u = (millis() - time);
  coulomb_v += (float)coulomb_u * I_akku / 1024.0;
  coulomb = coulomb_v / 3600.0;
  time = millis();
  return coulomb;
}

void charge_nimh() {
  if (search_mode) {
    search_I(I_charge, true);
    delay(100);
    U_akku = map(U_measure(0,  2, U_rep), 0, 32768, 0, 2048);
    U_akku_max = U_akku;
    time = millis();
    search_mode = false;
    T_start = temp();
  }

  U_akku = map(U_measure(0, 2, U_rep), 0, 32768, 0, 2048);
  I_akku = -1 * map(U_measure(1, 16, U_rep), 0, 32768, 0, 2560);

  if (U_akku_max < U_akku || (coulomb < I_charge && discharge)) {
    U_akku_max = U_akku;
  }

  if (U_akku_max - U_akku > 5 || temp() > 35) {// || dT_mid < dT_mid_max - 2) {
    I_charge = 10;
    search_I(I_charge, false);
    delay(100);
    U_akku = map(U_measure(0,  2, U_rep), 0, 32768, 0, 2048);
    U_akku_max = U_akku;
    time = millis();
  }

  if (I_akku < I_charge) {
    dawert -= step_down;
    writemcp4725(dawert);
  } else {
    if (I_akku > I_charge) {
      dawert += step_up;
      writemcp4725(dawert);
    }
  }
  write_display_data();
}

void charge_nicd() {
  if (search_mode) {
    search_I(I_charge, true);
    delay(100);
    U_akku = map(U_measure(0,  2, U_rep), 0, 32768, 0, 2048);
    U_akku_max = U_akku;
    time = millis();
    search_mode = false;
    T_start = temp();
  }

  U_akku = map(U_measure(0, 2, U_rep), 0, 32768, 0, 2048);
  I_akku = -1 * map(U_measure(1, 16, U_rep), 0, 32768, 0, 2560);

  if (U_akku_max < U_akku || (coulomb < I_charge)) {// && discharge)) {
    U_akku_max = U_akku;
  }

  if (U_akku_max - U_akku > 15 || temp() > 35) {
    I_charge = 10;
    search_I(I_charge, false);
    delay(100);
    U_akku = map(U_measure(0,  2, U_rep), 0, 32768, 0, 2048);
    U_akku_max = U_akku;
    time = millis();
  }

  if (I_akku < I_charge) {
    dawert -= step_down;
    writemcp4725(dawert);
  } else {
    if (I_akku > I_charge) {
      dawert += step_up;
      writemcp4725(dawert);
    }
  }
  write_display_data();
}

void charge_alkali() {
  if (search_mode) {
    search_I(I_charge, true);
    delay(100);
    U_akku = map(U_measure(0,  2, U_rep), 0, 32768, 0, 2048);
    U_akku_max = U_akku;
    time = millis();
    search_mode = false;
  }

  U_akku = map(U_measure(0, 2, U_rep), 0, 32768, 0, 2048);
  I_akku = -1 * map(U_measure(1, 16, U_rep), 0, 32768, 0, 2560);

  if (U_akku_max < U_akku) {
    U_akku_max = U_akku;
  }

  if (I_akku < I_charge) {
    dawert -= step_down;
    writemcp4725(dawert);
  } else {
    if (I_akku > I_charge) {
      dawert += step_up;
      writemcp4725(dawert);
    }
  }

  if (U_akku > 1690) {
    while (true) {
      U_akku = map(U_measure(0, 2, U_rep), 0, 32768, 0, 2048);
      I_akku = -1 * map(U_measure(1, 16, U_rep), 0, 32768, 0, 2560);
      if (U_akku < 1690) {
        dawert -= step_down;
        writemcp4725(dawert);
      } else {
        if (U_akku > 1690) {
          dawert += step_up;
          writemcp4725(dawert);
        }
      }
      write_display_data();
    }
  }
  write_display_data();
}