#define L1 10
#define L2 11
#define L3 12
#define LED 13

#define BTN1 9
#define BTN2 A2
#define TBAT A3

//#define MP1 2
#define ME1 3
//#define MP2 4
#define ME2 5

#define VIN A1
#define VBAT A0
#define VCC 5.08

#define BTOUT 300

#define MINPOWER 128
#define MIDPOWER 224
#define MAXPOWER 255
#define OFFPOWER 0


byte run_power = OFFPOWER;
int btn2 = 1023;
bool b1_pressed = false, b2_pressed = false, bm_pressed = false;
double b1_time, b2_time, bm_time, status_time;

const float r1_1 = 100000;
const float r1_2 = 10000;
const float r2_1 = 100000;
const float r2_2 = 10000;

float kin, kbat, vin, vbat;

void setup() {
  Serial.begin(9600);
    
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(L3, OUTPUT);

  //pinMode(MP1, OUTPUT);
  //digitalWrite(MP1, LOW);
  pinMode(ME1, OUTPUT);
  
  //pinMode(MP2, OUTPUT);
  //digitalWrite(MP2, HIGH);
  pinMode(ME2, OUTPUT);
  
  pinMode(BTN1, INPUT);
  pinMode(BTN2, INPUT);
  pinMode(VIN, INPUT);
  pinMode(VBAT, INPUT);

  kin = r1_2 / (r1_1 + r1_2);
  kbat = r2_2 / (r2_1 + r2_2);

  Serial.println("INIT DONE");
}

void loop() {
  if (digitalRead(BTN1) == LOW) {
    b1_pressed = true;
  } else {
    b1_pressed = false;
  }

  btn2 = analogRead(BTN2);
  if (btn2 > 600) {
    b2_pressed = false;
    bm_pressed = false;
  }
  
  if (btn2 > 200 && btn2 < 300) {
    b2_pressed = true;
  } else {
    b2_pressed = false;
  }
  
  if (btn2 > 400 && btn2 < 500) {
    bm_pressed = true;
  } else {
      bm_pressed = false;
  }

  // LEDs
  vin = (analogRead(VIN) * VCC) / 1024.0 / kin;
  vbat = (analogRead(VBAT) * VCC) / 1024.0 / kbat;

  if (vbat > 19) {
    digitalWrite(L1, HIGH);
    digitalWrite(L2, LOW);
  }
  
  if (vbat > 20) {
    digitalWrite(L3, HIGH);
    digitalWrite(L2, LOW);
  }

  if (vbat < 19 && vbat >= 18) {
    digitalWrite(L1, LOW);
    digitalWrite(L3, LOW);
    digitalWrite(L2, HIGH);
  }

  if (vbat < 18) {
    digitalWrite(L1, LOW);
    digitalWrite(L3, LOW);
    digitalWrite(L2, LOW);
  }
    
  // B1
  if (b1_pressed && run_power == OFFPOWER && (millis() - b1_time > BTOUT)) {
    //Serial.println("b1_pressed");
    //digitalWrite(MP1, HIGH);
    analogWrite(ME1, MAXPOWER);
    run_power = MAXPOWER;
    b1_time = millis();

//    Serial.print("Test");
//    for(byte i = 255; i > 128; i--) {
//      analogWrite(ME1, i);
//      Serial.print(i);
//      delay(100);
//    }
  }

  if (b1_pressed && run_power != OFFPOWER && (millis() - b1_time > BTOUT)) {
    //Serial.println("b1_pressed");
    //digitalWrite(MP1, LOW);
    //digitalWrite(MP2, HIGH);
    digitalWrite(ME1, OFFPOWER);
    digitalWrite(ME2, OFFPOWER);
    run_power = OFFPOWER;
    b1_time = millis();
  }

  // B2 
  if (b2_pressed && run_power == OFFPOWER && (millis() - b2_time > BTOUT)) {
    //Serial.println("b2_pressed");
    //digitalWrite(MP1, HIGH);
    //digitalWrite(MP2, LOW);
    analogWrite(ME1, MAXPOWER);
    delay(100);
    analogWrite(ME1, MINPOWER);
    analogWrite(ME2, MAXPOWER);
    run_power = MINPOWER;
    b2_time = millis();
  }

  if (b2_pressed && run_power != OFFPOWER && (millis() - b2_time > BTOUT)) {
    //Serial.println("b2_pressed");
    //digitalWrite(MP1, LOW);
    //digitalWrite(MP2, HIGH);
    digitalWrite(ME1, OFFPOWER);
    digitalWrite(ME2, OFFPOWER);
    run_power = OFFPOWER;
    b2_time = millis();
  }

  // BM
  if (bm_pressed && run_power != OFFPOWER && (millis() - bm_time > BTOUT)) {    
    if (run_power == MINPOWER) {
      analogWrite(ME1, MIDPOWER);
      run_power = MIDPOWER;
    } else {
      if (run_power == MIDPOWER) {
        analogWrite(ME1, MAXPOWER);
        run_power = MAXPOWER;
      } else {
        if (run_power == MAXPOWER) {
          analogWrite(ME1, MINPOWER);
          run_power = MINPOWER;
        }
      }
    }    
    bm_time = millis();
  }
  

  if (millis() - status_time > 1000) {
//    Serial.print("VIN: ");
//    Serial.println(analogRead(VIN));
//    Serial.print("VBAT: ");
//    Serial.println(analogRead(VBAT));
    Serial.print("POWER: ");
    Serial.println(run_power);
    Serial.print("BTN1: ");
    Serial.println(digitalRead(BTN1));
    Serial.print("BTN2: ");
    Serial.println(analogRead(BTN2));
    Serial.print("B1: ");
    Serial.println(b1_pressed);
    Serial.print("B2: ");
    Serial.println(b2_pressed);
    Serial.print("BM: ");
    Serial.println(bm_pressed);

    Serial.print("VIN: ");
    Serial.println(vin);
    Serial.print("VBAT: ");
    Serial.println(vbat);
    Serial.print("TBAT: ");
    Serial.println(analogRead(TBAT));

    Serial.println();
    status_time = millis();
  }
}