#include <SoftwareSerial.h>

/*
 * Define constants
 */
#define READ_DELAY 100     //milliseconds delay to ensure next byte is ready to be read from serial input buffer

#define MAX_PULSES_ALLOW 10 //max pulses to allow per requirements
#define MIN_PULSES_ALLOW 1 //minimum pulses to allow, per requirements
#define MAX_FREQ_ALLOW 36 //max frequency allowed, per requirements
#define MIN_FREQ_ALLOW 1 //min frequency allowed, per requirements
#define MAX_PWR_LVL_ALLOW 3 //maximum capacitor charge level, per requirements
#define MIN_PWR_LVL_ALLOW 1 //minimum capacitor charge level, per requirements

#define HIGH_TIME_MICROS 500 //high time in microseconds for square wave pulse per requirements
#define TERMINATOR '\n' //the terminator to be used (if any) for any data received from GUI
#define MAX_COMMAND_SIZE 10 //the maximum length in bytes of a command string that may be sent from the GUI at one time
#define TIMEOUT 30000 //timeout for capacitor charging in milliseconds (30 seconds). If we haven't charged by now we have a problem...

#define STARTUP_FREQUENCY 1 // the default frequency that is set when the device is reset
#define STARTUP_PULSE_COUNT 1 // the default pulse count that is set when the device is reset
#define STARTUP_POWER_LEVEL 1 // the default power level that is set when the device is reset

#define E_CAP_CHARGE_TIMEOUT 101

/*
 * Digital Output pin for switching main capacitor charging relay on/off.
 */
#define RELAY_PIN 13 

/*
 * Digital Output pin to drive IGBT gate via op-amp. This is the main pulse pin.
 */
#define PULSE_PIN 12

/*
 * Analog Input pin for sensing raw ADC value. This ADC value indicates capacitor charge level.
 */
#define CAP_SENSE_LVL_PIN 5 //analog in pin from which to read capacitor charge level

/*
 * Bluetooth IO pins
 */
#define BLUETOOTH_SERIAL_RX_PIN 8
#define BLUETOOTH_SERIAL_TX_PIN 7

/*
 * These are the raw ADC values that are experimentally determined to be read
 * on the CAP_LVL_SENSE_PIN pin from the capacitor charge level circuit, for each discrete power level.
 */
#define CAP_SENSE_LVL_1_THRESH 178 //Expected analog raw value given 80 V on capacitor
#define CAP_SENSE_LVL_2_THRESH 358 //Expected analog raw value given 160 V on capacitor
#define CAP_SENSE_LVL_3_THRESH 538 //Expected analog raw value given 240 V on capacitor

/*
 * Define valid command codes that can be recieved from UI
 */
#define CHANGE_FREQUENCY 'f' //change frequency of IGBT pulses
#define CHANGE_PWR_LVL 'l'  //change max capacitor charge level
#define CHANGE_PULSE_COUNT 'n' //change number of IGBT pulses
#define CHARGE_CAPS 'c'  //begin charging capacitors
#define PULSE 'p'  //fire pulse(s)
#define STATUS 's' //report status/state
#define RESET 'r'  //force microcontroller to known state


//Create Bluetooth serial object using 
SoftwareSerial BT_serial(BLUETOOTH_SERIAL_RX_PIN, BLUETOOTH_SERIAL_TX_PIN); //RX | TX

byte power_lvl = 1;
byte frequency = 1;
byte pulse_count = 1;
char command = '0';

void setup()
{
  pinMode(RELAY_PIN,OUTPUT);
  pinMode(PULSE_PIN,OUTPUT);
  pinMode(4,OUTPUT);
  digitalWrite(4,LOW);
  //Serial.begin(9600);     //Set Baud rate to 9600 //DEBUG 
  BT_serial.begin(9600);  //Set Baud rate to 9600
}

/*
 * Loop and handle serial commands from the GUI as they arrive
 */
void loop()
{
   if (BT_serial.available() > 0) 
   {
     command = BT_serial.read();
     if (is_valid_command(command)) 
     {
       switch(command)
       {
         case PULSE:
         //Serial.println("In pulse case:"); //DEBUG
         //Serial.println(command);
         pulse();
         //Serial.println("Bytes available:");
         //Serial.println(BT_serial.available());
         break;
      
         case CHARGE_CAPS:
         //Serial.println("In charge caps case."); //DEBUG
         //Serial.println(command);
         charge_caps();
         //Serial.println("Available:");
         //Serial.println(BT_serial.available());
         break;
         
         case CHANGE_PWR_LVL:
         //Serial.println("In set power level case."); //DEBUG
         //Serial.println(command);
         set_power_level();
         //Serial.println("Power level set to:");
         //Serial.println(power_lvl);
         //Serial.println("Available:");
         //Serial.println(BT_serial.available());
         break;
         
         case CHANGE_FREQUENCY:
           //Serial.println("In frequency case. Command is:"); //DEBUG
           //Serial.println(command);
           set_frequency();
           //Serial.println("Frequency set to:");
           //Serial.println(frequency);
           //Serial.println("Bytes available:");
           //Serial.println(BT_serial.available());
           break;
         
         case CHANGE_PULSE_COUNT:
           //Serial.println("In count case. Command is:"); //DEBUG
           //Serial.println(command);
           set_pulse_count();
           //Serial.println("pulse_count set to:");
           //Serial.println(pulse_count);
           //Serial.println("Bytes available:");
           //Serial.println(BT_serial.available());
           break;
      
         case RESET:
           reset();
           break;
         
         default:
           break;
         
        } //end switch
      } //end is valid command
    } //end serial available
} //end loop

/*
* Sets the local power level to the value recieved from a GUI change event.
* Checks that the level recieved is one of the defined allowed power levels.
* 
* Arguments: pwr_lvl
* Returns: 1 if the power level is successfully set to one of the defined allowed power levels, 0 otherwise.
*/
int set_power_level(){
  //Serial.println("In set_power_level rxed_pwr_lvl is:"); //DEBUG
  int rxed_pwr_lvl = get_command_val();
  //Serial.println(rxed_pwr_lvl);
  if((rxed_pwr_lvl >= MIN_PWR_LVL_ALLOW) && (rxed_pwr_lvl <= MAX_PWR_LVL_ALLOW)) {
    power_lvl = rxed_pwr_lvl;
    BT_serial.write(power_lvl);
    return 1;
  }
  else {
    BT_serial.write(-1);
    return 0;
  }
}

/*
 * Sets the global pulse_count variable to the value returned from get_command_val().
 * Checks that the number of pulses returned is within the range specified by requirements, [1, 10].
 * 
 * Arguments: none
 * 
 * Returns: 1 if the number of pulses is between MIN_PULSES_ALLOW and MAX_PULSES_ALLOW, inclusive,
 *          0 otherwise.
 *
 */
int set_pulse_count(){
  int rxed_pulses = get_command_val();
  if((rxed_pulses >= MIN_PULSES_ALLOW) && (rxed_pulses <= MAX_PULSES_ALLOW)) {
    pulse_count = rxed_pulses;
    BT_serial.write(pulse_count);
    return 1;
  }
  else {
    BT_serial.write(-1);
    return 0;
  }
}


/*
 * This function extracts a subtring representing a number value from the command string received from the GUI.
 * This substring of the entire command string must still be available in the serial input buffer
 * when this function is called.
 * 
 * This function will read the next 3 bytes available in the serial input buffer.
 * The first 2 bytes are interpreted as the integer value of the command. The 3rd byte read is assumed
 * to be the TERMINATOR charcter. 
 * 
 * The substring extracted should contain the numerical value of the command.
 * 
 * Arguments: none
 *
 * Returns: The substring of the command string representing the command numerical value
 */
int get_command_val(){
  int i = 0;
  char command_value[MAX_COMMAND_SIZE] = {0};
  //Serial.println("BT_serial.avaiable:"); //DEBUG
  //Serial.println(BT_serial.available());
  //Serial.println("sizeof:");
  //Serial.println(sizeof(command_value));
  while(BT_serial.available() && (i < (sizeof(command_value)-1))){
     command_value[i] = BT_serial.read();
     if(command_value[i] == TERMINATOR) {
       i++;
       command_value[i] = '\0';
       break;
     }
     i++;
  }
  //Serial.println("In get_command_val raw command val is:"); //DEBUG
  //Serial.println(command_value);
  //Serial.println("In get_command_val command val is:");
  //Serial.println(atoi(command_value));
  return atoi(command_value);
}


/*
 * Sets the pulse frequency to the value received from the GUI.
 * Checks that the value is in allowed range.
 * 
 * Reports "ok" to GUI if value is in range and set, "error" otherwise.
 */
int set_frequency(){
  byte rxed_freq = get_command_val();
  if((rxed_freq >= MIN_FREQ_ALLOW) && (rxed_freq <= MAX_FREQ_ALLOW)){
    frequency = rxed_freq;
    BT_serial.write(frequency);
    return 1;
  }
  else {
    BT_serial.write(-1);
    return 0;
  }
}

/*
 * Charge capacitors to the level specified by the pwr_level variable.
 * 
 * Arguments: pwr_level,
 * Returns an integer that equals the power level that was set in the range [0,3], or -1 on failure.
 */
int charge_caps()
{ 
  int raw_adc_val = 0;
  //Serial.println(raw_adc_val); //DEBUG
  switch(power_lvl){
    //Serial.println("In charge_caps: power_lvl is:"); //DEBUG
    //Serial.println(power_lvl);
    //Serial.println(analogRead(CAP_SENSE_LVL_PIN));
    case 1:
       if(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_1_THRESH)
       {
          digitalWrite(RELAY_PIN, HIGH);
          unsigned long start_time = millis();
          while(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_1_THRESH) {
            if(millis()-start_time > TIMEOUT){
              digitalWrite(RELAY_PIN, LOW);
              BT_serial.write(E_CAP_CHARGE_TIMEOUT);
              return -1;
            }
          }
          digitalWrite(RELAY_PIN, LOW);
       }
       break;
       
    case 2:
       if(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_2_THRESH)
       {
          digitalWrite(RELAY_PIN, HIGH);
          unsigned long start_time = millis();
          while(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_2_THRESH) {
            if(millis()-start_time > TIMEOUT){
              digitalWrite(RELAY_PIN, LOW);
              BT_serial.write(E_CAP_CHARGE_TIMEOUT);
              return -1;
            }
          }
          digitalWrite(RELAY_PIN, LOW);
       }
       break;
       
    case 3:
       if(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_3_THRESH)
       {
          digitalWrite(RELAY_PIN, HIGH);
          unsigned long start_time = millis();
          while(analogRead(CAP_SENSE_LVL_PIN) < CAP_SENSE_LVL_3_THRESH) {
            if(millis()-start_time > TIMEOUT){
              digitalWrite(RELAY_PIN, LOW);
              BT_serial.write(E_CAP_CHARGE_TIMEOUT);
              return -1;
            }
          }
          digitalWrite(RELAY_PIN, LOW);
       }
       break;
       
    default:
       break;
  }
  //Serial.println(raw_adc_val); //DEBUG
  BT_serial.write(power_lvl);
  return power_lvl;
}

/*
 * This function checks the global status of the device, then fires pulses to drive the IGBT
 * based on device configuration.
 * 
 * Arguments: none
 * Returns: 1 if the device is ready and pulses were fired successfully. Returns 0 otherwise.
 */
int pulse(){
  
  short freq_to_lowtime_map[36] = 
  {995,500,333,245,200,166,143,122,111,100,91,83,77,71,66,62,
  59,55,52,49,47,45,43,41,40,38,37,36,34,33,32,31,30,29,28,27};
  
  short lowtime = freq_to_lowtime_map[frequency-1];
  byte pulse_cntr = 0;
  while (pulse_cntr < pulse_count) {
    digitalWrite(PULSE_PIN, HIGH);
    delayMicroseconds(HIGH_TIME_MICROS);
    digitalWrite(PULSE_PIN, LOW);
    delay(lowtime);
    /*
    digitalWrite(4,HIGH); //DEBUG
    delay(500);
    digitalWrite(4,LOW);
    delay(lowtime);
    */
    pulse_cntr++;
  }
  return 1;
}

/*
 * Send status to Matlab
 *
 * Arguments: none
 * Returns: 
 */
int report_status(){
  if(BT_serial.isListening()){
    BT_serial.write(power_lvl);
    BT_serial.write(pulse_count);
    BT_serial.write(digitalRead(RELAY_PIN));
  }
  return 0;
}

/*
 * Closes main power relay and resets state to startup values.
 *
 * Arguments: none
 * Returns: 0 on success
 */
int reset(){
  digitalWrite(RELAY_PIN,LOW);
  digitalWrite(PULSE_PIN,LOW);
  frequency = STARTUP_FREQUENCY;
  pulse_count = STARTUP_PULSE_COUNT;
  power_lvl = STARTUP_POWER_LEVEL;
  BT_serial.write(1); //send 1 to GUI to signal successful reset
  return 1;
}

/*
 * Verifies that the given command is one of the defined valid commands.
 * 
 * Arguments: command
 * Returns: 1 if the command is valid, 0 if not valid.
 */
int is_valid_command(char command){
  //Serial.println("In is_valid_command:"); //DEBUG
  //Serial.println("Command is:");
  //Serial.println(command);
  
  char valid_cmds[] = {'l', 'n', 'c', 'p', 's', 'r', 'f'};
  //Serial.println("sizeof is:"); //DEBUG
  //Serial.println(sizeof(valid_cmds));
  int i = 0;
  for(i=0; i<sizeof(valid_cmds); i++){
    //Serial.println(valid_cmds[i]); //DEBUG
    if(command == valid_cmds[i]){
      return 1;
    }
  }
  return 0;
}