// BattTest sketch for Arduino (c) 2009 Joachim Thiemann

const int Abatt = 0;            // Analog sensor at battery +
const int Acurrent = 1;         // Analog sensor after resistor to measure current
const int Dload = 8;            // digital pin driving transistor
const int Dled = 13;            // LED

const int lowbatt = 3200;       // this is the voltage (in mV) at which we stop.
const int switchTime = 120;     // on-off period time, in loop time counts
const int loopTime = 500;       // loop time, .5s

const float voltMult = 4.9;    // convert analog reading to mV: 5000/1024 + fudge factor

int incomingByte;
unsigned long int timeOffset;   // records process start time; all timestamps relative to this
int timerCount;                 // countdown timer to swich load on-off

enum state { 
  IDLE=0, START, RUN_FAST, RUN_UNLOADED, RUN_LOADED, END } 
state;

void setup()
{
  // make sure transistor is OFF
  digitalWrite( Dload, LOW );
  // set the transistor pin to output
  pinMode( Dload, OUTPUT );
  // set state to IDLE
  state = IDLE;
  // LED off
  ledstate( 0 );
  // initialise serial port
  Serial.begin(9600);
}

void loop()
{
  // read serial port, change state if needed
  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    if (incomingByte=='S') state = START;
    if (incomingByte=='F') {
      Serial.println("Starting fast discharge");
      timeOffset = millis();
      state = RUN_FAST;
    }
    if (incomingByte=='E') state = END;
  }

  // perform state machine code
  if (state!=IDLE) {     // short-circuit idle state

    switch(state) {
    case START:
      Serial.println("Starting");
      timeOffset = millis();
      timerCount = switchTime;
      state = RUN_UNLOADED;
      break;

    case RUN_FAST:
      digitalWrite( Dload, HIGH );
      ledstate(2);
      break;

    case RUN_UNLOADED:
      digitalWrite( Dload, LOW );
      ledstate(1);
      timerCount--;
      if (timerCount<0) {
        timerCount = switchTime;
        state = RUN_LOADED;
      }
      break;

    case RUN_LOADED:
      digitalWrite( Dload, HIGH );
      ledstate(2);
      timerCount--;
      if (timerCount<0) {
        timerCount = switchTime;
        state = RUN_UNLOADED;
      }
      break;

    case END:
      Serial.println("Ending");
      digitalWrite( Dload, LOW );
      ledstate(0);
      state = IDLE;
      break;

    deafult:
      Serial.println("Error");
      state = IDLE;
    }

    // unless in IDLE, always print status line, check for low batt, then wait a bit.
    printStatus();
    if ((state!=IDLE)&&(analogRead(Abatt)<((float)lowbatt/voltMult))) {
      Serial.println("Battery low");
      state=END;
    }
    delay(loopTime);
  }
  //Serial.print('.');delay(100);
}

void printStatus()
{
  // print status line
  unsigned long int timestamp;
  int V1, V2;
  V1 = (float)analogRead(Abatt)*voltMult;
  V2 = (float)analogRead(Acurrent)*voltMult;

  timestamp = millis()-timeOffset;
  Serial.print( timestamp );
  Serial.print( ' ' );

  Serial.print( (int)state );
  Serial.print( ' ' );

  Serial.print( V1 );
  Serial.print( ' ' );
  Serial.print( V2 );
  Serial.println( ' ' );
}

void ledstate( int state )
{
  // if state = 0, off, state = 1, toggle, state = 2, on.
  static int prevState;
  if (state==0) {
    digitalWrite( Dled, LOW );
    prevState = 0;
  }
  if (state==1) {
    if (prevState==0) {
      digitalWrite( Dled, HIGH );
      prevState = 1;
    } 
    else {
      digitalWrite( Dled, LOW );
      prevState = 0;
    }
  }
  if (state==2) {
    digitalWrite( Dled, HIGH );
    prevState = 1;
  }
}