//////////////////////////////////////////////////////////////////////////////////////
//  INITALIZATIONS
//////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////
//  PIN ASSIGNMENTS
//////////////////////////////////
//LEFT SIDE, TOP TO BOTTOM
//    RESET
//    IO0 = 0;    //TX PIN
//    IO1 = 1;    //RX PIN
  int IO2 = 2;
  int IO3 = 3;    //PWM PIN
  int IO4 = 4;
//    VCC 
//    GND 
//    XTAL1       //CRYSTAL PIN
//    XTAL2       //CRYSTAL PIN
  int IO5 = 5;    //PWM PIN
  int IO6 = 6;    //PWM PIN
  int IO7 = 7;
  int IO8 = 8;
//RIGHT SIDE, BOTTOM TO TOP
  int IO9 = 9;    //PWM PIN
  int IO10 = 10;  //PWM PIN
  int IO11 = 11;  //PWM PIN
  int IO12 = 12;
  int IO13 = 13;
//    AVCC 
//    AREF 
//    AGND 
  int AD0 = A0;
  int AD1 = A1;
  int AD2 = A2;
  int AD3 = A3;
  int AD4 = A4;
  int AD5 = A5; 

//////////////////////////////////
//  INITIALIZE PINS FOR VOLTAGE READING
//////////////////////////////////
int BATTERY = AD0;    //RECEIVING FROM (+) BATTERY TERMINAL

//////////////////////////////////
//  INITIALIZE PINS FOR TEMPERATURE SENSOR
//////////////////////////////////
int TEMP = AD5;         //RECEIVING FROM TEMP SENSOR

//////////////////////////////////
//  INITIALIZE PINS FOR XBEE (NOT NECESSARY IF USING SERIAL)
//////////////////////////////////
//int TX = IO1;         //DATA TO XBEE PIN 2 (DIN)
//int RX = IO0;         //DATA FROM XBEE PIN 3 (DOUT)

//////////////////////////////////
//  OTHER INITIALIZATIONS
//////////////////////////////////
// If comment exist, you can edit value
int i=0, j=0, k=0, t=0;
int startup_delay = 5000;   //(ms) delay time until program runs
float p1 = 4.0367;          //From "ZTP115MSensitivityEquation.m"
float p2 = -40.878;           //To convert volt to temp, use
float p3 = 135.65;            //these 5 coefficients
float p4 = -107.88;
float p5 = -6.6811;
float v1 = 0.0020452;       //From "BatteryVoltageEquation.m"
float v2 = 0.26644;           //To convert user voltage (12V~)
float v3 = 0.31018;           //to 4V~ range for microcontroller
float voltage_full = 12.6;  //(V) voltage setting for full battery
float voltage_empty = 11.7;   //(V) voltage setting for empty battery
float volt_full=0;
float volt_empty=0;
float volt_interval=0;
float temp_read=0;
float volt_read=0;
int temp=0;
int volt=0;
float temp_C=0;
float temp_F=0;


//////////////////////////////////////////////////////////////////////////////////////
//  SETUP
//////////////////////////////////////////////////////////////////////////////////////
void setup() {

  Serial.begin(9600);         //set BAUD rate for XBee
  pinMode(BATTERY, INPUT);
  pinMode(TEMP, INPUT);
  //pinMode(TX, OUTPUT);
  //pinMode(RX, INPUT);

  delay(startup_delay);
}

//////////////////////////////////////////////////////////////////////////////////////
//  MAIN CODE 
//////////////////////////////////////////////////////////////////////////////////////
void loop() {

    read_temp();
    read_volt();
    
    xbee_write(temp, volt);
}

//////////////////////////////////////////////////////////////////////////////////////
//  IMPORTANT FUNCTIONS
//////////////////////////////////////////////////////////////////////////////////////

//  READS DATA FROM VOLTAGE NODES
//  NO INPUTS
void read_volt(){

  volt_read = (float) analogRead(BATTERY)*5/1023;
  
  volt_full = (float) voltage_full * 47 / 147;
  volt_empty = (float) voltage_empty * 47 / 147;
  volt_interval = (volt_full - volt_empty)/8;
  
  if (volt_read < volt_empty)
    volt = 0;
  else if (volt_read < volt_empty+volt_interval)
    volt = 1;
  else if (volt_read < volt_empty+volt_interval*2)
    volt = 2;
  else if (volt_read < volt_empty+volt_interval*3)
    volt = 3;
  else if (volt_read < volt_empty+volt_interval*4)
    volt = 4;
  else if (volt_read < volt_empty+volt_interval*5)
    volt = 5;
  else if (volt_read < volt_empty+volt_interval*6)
    volt = 6;
  else if (volt_read < volt_empty+volt_interval*7)
    volt = 7;
  else if (volt_read < volt_empty+volt_interval*8)
    volt = 8;
  else
    volt = 9;

  /*
  Serial.print(volt_read);
  Serial.print("\t");
  Serial.print(volt_full);
  Serial.print("\t");
  Serial.print(volt_empty);
  Serial.print("\t");
  Serial.print(volt_interval);
  Serial.print("\t");
  Serial.println(volt);
  */
}

//  READS DATA FROM TEMPERATURE SENSOR
//  NO INPUTS
void read_temp(){

  temp_read = (float) analogRead(TEMP)*5/1023;
  temp_C = (float) p1*pow(temp_read,4) + p2*pow(temp_read,3) +
              p3*pow(temp_read,2) + p4*temp_read + p5;
  
  if (temp_C < 40)
    temp_C += (40-temp_C)*3/20;
  else if (temp_C > 40)
    temp_C += (100-temp_C)*2/60 - 2;

  temp_F = temp_C*9/5 + 32;
  
  if (temp_F > 210)
    temp = 211;
  else if (temp_F < 30)
    temp = 211;
  else
    temp = (int) round(temp_F);

  /*
  Serial.print(temp_read);
  Serial.print("\t");
  Serial.print(temp_C);
  Serial.print("\t");
  Serial.print(temp_F);
  Serial.print("\t");
  Serial.println(temp);
  */
}

//  WRITES XBTEMP AND XBVOLT TO XBEE
//  NO INPUTS
void xbee_write(byte xbtemp, byte xbvolt){

  Serial.write(xbtemp);
  Serial.write(xbvolt);

  delay(10);
}