/////////////////////////////////////////
// HXbot REflex firmware                /
// EoF 2016 EoF@itphx.ru                /
/////////////////////////////////////////

#include <PowerFunctions.h>
#include <Ultrasonic.h>
#include <Wire.h>
#include <Servo.h>

// DEBUG
#define DEBUG 1

// DEFINE
#define SLAVE_ADDRESS 0x04
#define XOR_SEQ 0xFF
#define EXT_COM 0xAA

//#define LED_PIN 13
#define IRE_PIN 2
#define IRR_PIN 3
#define SV_PIN 13 // vertical
#define SV_CENTER 150 // 0 - up (min. 60), 180 - down (max. 170)
#define SH_PIN 12 //horisontal
#define SH_CENTER 90 // 0 - right, 180 - left
#define SERVO_DELAY 10 // time interval between servo moves
#define ULTRASONIC_PIN 6
#define DRIVE_CHANNEL 2
#define TIMEOUT 5000
#define HEAD_V_FLAG 0x31
#define HEAD_H_FLAG 0x30
// Front and rear servos
#define SF_PIN 10 // front servo
#define SF_CENTER 96
#define SR_PIN 11 // rear servo
#define SR_CENTER 93
#define FR_STEP 2
// IR distance sensors
#define IRD_FRONT_PIN 0
#define IRD_F_MAX 3.0
#define IRA_FRONT_PIN 1
#define IRA_F_MIN 1.0
#define IRD_REAR_PIN 2
#define IRD_R_MAX 3.0
#define IRA_REAR_PIN 3
#define IRA_R_MIN 1.0
#define IR_REPEAT_COUNT 3

// COMMANDS
// Util
#define COM_PING 0x01
#define COM_GET_TEMP 0x02
#define COM_GET_DISTANCE 0x03

// Stop
#define STOP 0x0F
// Move
#define MOVE_FLAG 0x04
#define MOVE_FLOAT 0x10
#define MOVE_FWD1 0x11
#define MOVE_FWD2 0x12
#define MOVE_FWD3 0x13
#define MOVE_FWD4 0x14
#define MOVE_FWD5 0x15
#define MOVE_FWD6 0x16
#define MOVE_FWD7 0x17
#define MOVE_BREAK 0x18
#define MOVE_REV7 0x19
#define MOVE_REV6 0x1A
#define MOVE_REV5 0x1B
#define MOVE_REV4 0x1C
#define MOVE_REV3 0x1D
#define MOVE_REV2 0x1E
#define MOVE_REV1 0x1F
// Steering
#define STEER_FLAG 0x05
#define STEER_CENTER 0x20
#define STEER_LEFT1 0x21
#define STEER_LEFT2 0x22
#define STEER_LEFT3 0x23
#define STEER_LEFT4 0x24
#define STEER_LEFT5 0x25
#define STEER_LEFT6 0x26
#define STEER_LEFT7 0x27
#define STEER_FIX_CENTER 0x28
#define STEER_RIGHT7 0x29
#define STEER_RIGHT6 0x2A
#define STEER_RIGHT5 0x2B
#define STEER_RIGHT4 0x2C
#define STEER_RIGHT3 0x2D
#define STEER_RIGHT2 0x2E
#define STEER_RIGHT1 0x2F

// Response
#define OK_RSP  0x00
#define NO_RSP  0xFF
#define ERR_RSP 0x01
#define BLK_RSP 0x02
#define CSE_RSP 0x03
#define IOE_RSP 0x04
#define TMO_RSP 0x05

// VAR
const float typVbg = 1.13; // 1.0 -- 1.2

float ird_f, ira_f, ird_r, ira_r;
boolean ird_ignore = false, ira_ignore = false;

byte cmd = 0;
byte flg = 0;
byte ext = 0;

byte autoresponse = 0;
long usDistance = 0;
unsigned long lastCommandTime = 0;
boolean isMove = false;
boolean frBlock = false;

PowerFunctions pfDrive(IRE_PIN, DRIVE_CHANNEL);
Ultrasonic usSensor(ULTRASONIC_PIN);
Servo servV, servH, servF, servR;
byte destV, destH;
unsigned long lastServoMove = 0;

// SETUP 
void setup() {
  
  if (DEBUG) {
    Serial.println("GO!");
    Serial.begin(9600);
  }
  
  //pinMode(LED_PIN, OUTPUT);
  
  // Initialize i2c as slave
  Wire.begin(SLAVE_ADDRESS);
 
  // Define callbacks for i2c communication
  Wire.onReceive(receiveData);
  Wire.onRequest(answer);
  
  analogReference(DEFAULT);
  
  destH = SH_CENTER;
  destV = SV_CENTER;
  
  if (DEBUG) {
    Serial.println("Setup complete");
  }
}

// MAIN LOOP
void loop() {
  // Timeout protection
  if ((lastCommandTime + TIMEOUT < millis()) && isMove) {
    pfDrive.combo_pwm(PWM_BRK, PWM_BRK);
    servV.detach();
    servH.detach();
    isMove = false;
    autoresponse = TMO_RSP;
  }
  
  moveServo();
  
  //usDistance = usSensor.MeasureInCentimeters();
  if (DEBUG) {
    ird_f = analogRead(IRD_FRONT_PIN) * readvcc() / 1024;
    ira_f = analogRead(IRA_FRONT_PIN) * readvcc() / 1024;
    ird_r = analogRead(IRD_REAR_PIN) * readvcc() / 1024;
    ira_r = analogRead(IRA_REAR_PIN) * readvcc() / 1024;

    Serial.print("VCC: ");
    Serial.println(readvcc());
    Serial.print("IRD_F: ");
    Serial.println(ird_f);
    Serial.print("IRA_F: ");
    Serial.println(ira_f);
    Serial.print("IRD_R: ");
    Serial.println(ird_r);
    Serial.print("IRA_R: ");
    Serial.println(ira_r);
    Serial.println("------");
    delay(1000);
  }
  //delay(100);
}

void moveResponse(byte rsp = OK_RSP) {
  lastCommandTime = millis();
  isMove = true;
  autoresponse = rsp;
}

// Callback for received data
void receiveData(int byteCount) {
  
  while(Wire.available()) {
    // Get command
    cmd = Wire.read();
    if (cmd == EXT_COM && byteCount == 3) {
      flg = 0x00;
      ext = 0x00;
      
      if (Wire.available()) ext = Wire.read();
      if (Wire.available()) flg = Wire.read();
    }
    else {
      // Cleanup I2C bus
      while(Wire.available()) {
        ext = Wire.read();
      }
    }
      
    switch (cmd) {
      case COM_PING:
        autoresponse = OK_RSP;
        break;
      case STOP:
        pfDrive.combo_pwm(PWM_BRK, PWM_BRK);
        moveResponse();
        break;
      case COM_GET_TEMP:
        autoresponse = getInternalTemp();
        break;
      case COM_GET_DISTANCE:
        usDistance = usSensor.MeasureInCentimeters();
        if (usDistance >= 255) {
          autoresponse = NO_RSP;
        } else {
          autoresponse = usDistance;
        }
        break;
      case EXT_COM:
        switch (flg) {
          case HEAD_V_FLAG: case HEAD_H_FLAG:
            headControl(flg, ext);
            break;
          case MOVE_FLAG: case STEER_FLAG:
            moveControl(flg, ext);
            break;
        }
        break;
      default:
        autoresponse = ERR_RSP;
        break;
    }
  }
}

void headControl(byte flg, byte val) {  
  if (! servV.attached()) servV.attach(SV_PIN);
  if (! servH.attached()) servH.attach(SH_PIN);
    
  switch (flg) {
    case HEAD_V_FLAG:
      destV = val;
      break;
    case HEAD_H_FLAG:
      destH = val;
      break;    
  }
  moveResponse();
}

void moveServo() {
  byte curV, curH;

  if (! isMove) return;
  
  if (lastServoMove + SERVO_DELAY < millis()) {    
    curV = servV.read();
    curH = servH.read();
    
    if (destV < curV) servV.write(curV - 1);
    if (destV > curV) servV.write(curV + 1);
    if (destH < curH) servH.write(curH - 1);
    if (destH > curH) servH.write(curH + 1);
    
    lastServoMove = millis();
  }
}
 
// Callback for sending data
void answer() {
  Wire.write(autoresponse);
}

boolean ird_f_ok() {
  if (! ird_ignore) {
  
    ird_f = 0.0;
    
    for (byte i = 0; i < IR_REPEAT_COUNT; i++) {
      ird_f = ird_f + analogRead(IRD_FRONT_PIN) * readvcc() / 1024;
    }
    ird_f = ird_f / IR_REPEAT_COUNT;
    
    if (ird_f <= IRD_F_MAX) {
      return true;
    } else {
      return false;
    }
    
  } else return true;  
}

boolean ird_r_ok() {
  if (! ird_ignore) {
  
    ird_r = 0.0;
    
    for (byte i = 0; i < IR_REPEAT_COUNT; i++) {
      ird_r = ird_r + analogRead(IRD_REAR_PIN) * readvcc() / 1024;
    }
    ird_r = ird_r / IR_REPEAT_COUNT;
    
    if (ird_r <= IRD_R_MAX) {
      return true;
    } else {
      return false;
    }
    
  } else return true;  
}

boolean ira_f_ok() {
  if (! ira_ignore) {
  
    ira_f = 0.0;
    
    for (byte i = 0; i < IR_REPEAT_COUNT; i++) {
      ira_f = ira_f + analogRead(IRA_FRONT_PIN) * readvcc() / 1024;
    }
    ira_f = ira_f / IR_REPEAT_COUNT;
    
    if (ira_f >= IRA_F_MIN) {
      return true;
    } else {
      return false;
    }
    
  } else return true;  
}

boolean ira_r_ok() {
  if (! ira_ignore) {
  
    ira_r = 0.0;
    
    for (byte i = 0; i < IR_REPEAT_COUNT; i++) {
      ira_r = ira_r + analogRead(IRA_REAR_PIN) * readvcc() / 1024;
    }
    ira_r = ira_r / IR_REPEAT_COUNT;
    
    if (ira_r >= IRA_R_MIN) {
      return true;
    } else {
      return false;
    }
    
  } else return true;  
}

void stopMove() {
  pfDrive.single_pwm(RED, PWM_BRK);
  pfDrive.single_pwm(RED, PWM_BRK);
  pfDrive.single_pwm(RED, PWM_BRK);
}

void moveControl(byte flg, byte cmd) {
  if (flg == MOVE_FLAG) {
    if (cmd >= MOVE_FWD1 && cmd <= MOVE_FWD7) {
      if (! ira_f_ok()) {
        stopMove();
        moveResponse(BLK_RSP);
        return;
      }
      if (! ird_f_ok()) {
        stopMove();
        moveResponse(BLK_RSP);
        return;
      }
    }
    
    if (cmd >= MOVE_REV7 && cmd <= MOVE_REV1) {
      if (! ira_r_ok()) {
        stopMove();
        moveResponse(BLK_RSP);
        return;
      }
      if (! ird_r_ok()) {
        stopMove();
        moveResponse(BLK_RSP);
        return;
      }
    }
  }
  
  switch (cmd) {
    // Moving
    case MOVE_FLOAT:
      pfDrive.single_pwm(RED, PWM_FLT);
      break;
    case MOVE_FWD1:
      pfDrive.single_pwm(RED, PWM_FWD1);
      break;
    case MOVE_FWD2:
      pfDrive.single_pwm(RED, PWM_FWD2);
      break;
    case MOVE_FWD3:
      pfDrive.single_pwm(RED, PWM_FWD3);
      break;
    case MOVE_FWD4:
      pfDrive.single_pwm(RED, PWM_FWD4);
      break;
    case MOVE_FWD5:
      pfDrive.single_pwm(RED, PWM_FWD5);
      break;
    case MOVE_FWD6:
      pfDrive.single_pwm(RED, PWM_FWD6);
      break;
    case MOVE_FWD7:
      pfDrive.single_pwm(RED, PWM_FWD7);
      break;
    case MOVE_BREAK:
      pfDrive.single_pwm(RED, PWM_BRK);
      break;
    case MOVE_REV1:
      pfDrive.single_pwm(RED, PWM_REV1);
      break;
    case MOVE_REV2:
      pfDrive.single_pwm(RED, PWM_REV2);
      break;
    case MOVE_REV3:
      pfDrive.single_pwm(RED, PWM_REV3);
      break;
    case MOVE_REV4:
      pfDrive.single_pwm(RED, PWM_REV4);
      break;
    case MOVE_REV5:
      pfDrive.single_pwm(RED, PWM_REV5);
      break;
    case MOVE_REV6:
      pfDrive.single_pwm(RED, PWM_REV6);
      break;
    case MOVE_REV7:
      pfDrive.single_pwm(RED, PWM_REV7);
      break;

    // Steering  
    case STEER_CENTER:
      pfDrive.single_pwm(BLUE, PWM_FLT);
      frControl(cmd);
      break;
    case STEER_LEFT1:
      pfDrive.single_pwm(BLUE, PWM_FWD1);
      frControl(cmd);
      break;
    case STEER_LEFT2:
      pfDrive.single_pwm(BLUE, PWM_FWD2);
      frControl(cmd);
      break;
    case STEER_LEFT3:
      pfDrive.single_pwm(BLUE, PWM_FWD3);
      frControl(cmd);
      break;
    case STEER_LEFT4:
      pfDrive.single_pwm(BLUE, PWM_FWD4);
      frControl(cmd);
      break;        
    case STEER_LEFT5:
      pfDrive.single_pwm(BLUE, PWM_FWD5);
      frControl(cmd);
      break; 
    case STEER_LEFT6:
      pfDrive.single_pwm(BLUE, PWM_FWD6);
      frControl(cmd);
      break; 
    case STEER_LEFT7:
      pfDrive.single_pwm(BLUE, PWM_FWD7);
      frControl(cmd);
      break;
    case STEER_FIX_CENTER:
      pfDrive.single_pwm(BLUE, PWM_BRK);
      frControl(cmd);
      break;
    case STEER_RIGHT1:
      pfDrive.single_pwm(BLUE, PWM_REV1);
      frControl(cmd);
      break;
    case STEER_RIGHT2:
      pfDrive.single_pwm(BLUE, PWM_REV2);
      frControl(cmd);
      break;
    case STEER_RIGHT3:
      pfDrive.single_pwm(BLUE, PWM_REV3);
      frControl(cmd);
      break;
    case STEER_RIGHT4:
      pfDrive.single_pwm(BLUE, PWM_REV4);
      frControl(cmd);
      break;
    case STEER_RIGHT5:
      pfDrive.single_pwm(BLUE, PWM_REV5);
      frControl(cmd);
      break;
    case STEER_RIGHT6:
      pfDrive.single_pwm(BLUE, PWM_REV6);
      frControl(cmd);
      break;
    case STEER_RIGHT7:
      pfDrive.single_pwm(BLUE, PWM_REV7);
      frControl(cmd);
      break;
    default:  
      moveResponse(NO_RSP);
      return;
      break;
  }
  // Send response
  moveResponse();
}

void frControl(byte cmd) {
  if (! frBlock) {
    if (! servF.attached()) servF.attach(SF_PIN);
    if (! servR.attached()) servR.attach(SR_PIN);
  
    switch (cmd) {
      case STEER_CENTER:
        servF.write(SF_CENTER);
        servR.write(SR_CENTER);
        break;
      case STEER_LEFT1:
        servF.write(SF_CENTER - FR_STEP * 1);
        servR.write(SR_CENTER + FR_STEP * 1);
        break;
      case STEER_LEFT2:
        servF.write(SF_CENTER - FR_STEP * 2);
        servR.write(SR_CENTER + FR_STEP * 2);
        break;
      case STEER_LEFT3:
        servF.write(SF_CENTER - FR_STEP * 3);
        servR.write(SR_CENTER + FR_STEP * 3);
        break;
      case STEER_LEFT4:
        servF.write(SF_CENTER - FR_STEP * 4);
        servR.write(SR_CENTER + FR_STEP * 4);
        break;        
      case STEER_LEFT5:
        servF.write(SF_CENTER - FR_STEP * 5);
        servR.write(SR_CENTER + FR_STEP * 5);
        break; 
      case STEER_LEFT6:
        servF.write(SF_CENTER - FR_STEP * 6);
        servR.write(SR_CENTER + FR_STEP * 6);
        break; 
      case STEER_LEFT7:
        servF.write(SF_CENTER - FR_STEP * 7);
        servR.write(SR_CENTER + FR_STEP * 7);
        break;
      case STEER_FIX_CENTER:
        servF.write(SF_CENTER);
        servR.write(SR_CENTER);
        break;
      case STEER_RIGHT1:
        servF.write(SF_CENTER + FR_STEP * 1);
        servR.write(SR_CENTER - FR_STEP * 1);
        break;
      case STEER_RIGHT2:
        servF.write(SF_CENTER + FR_STEP * 2);
        servR.write(SR_CENTER - FR_STEP * 2);
        break;
      case STEER_RIGHT3:
        servF.write(SF_CENTER + FR_STEP * 3);
        servR.write(SR_CENTER - FR_STEP * 3);
        break;
      case STEER_RIGHT4:
        servF.write(SF_CENTER + FR_STEP * 4);
        servR.write(SR_CENTER - FR_STEP * 4);
        break;
      case STEER_RIGHT5:
        servF.write(SF_CENTER + FR_STEP * 5);
        servR.write(SR_CENTER - FR_STEP * 5);
        break;
      case STEER_RIGHT6:
        servF.write(SF_CENTER + FR_STEP * 6);
        servR.write(SR_CENTER - FR_STEP * 6);
        break;
      case STEER_RIGHT7:
        servF.write(SF_CENTER + FR_STEP * 7);
        servR.write(SR_CENTER - FR_STEP * 7);
        break;
      default:
        servF.write(SF_CENTER);
        servR.write(SR_CENTER);
        break;
    }
  }
}

// Get the internal temperature of the arduino
double getInternalTemp(void) {
  unsigned int wADC;
  double t;
  ADMUX = (_BV(REFS1) | _BV(REFS0) | _BV(MUX3));
  ADCSRA |= _BV(ADEN); // enable the ADC
  delay(20); // wait for voltages to become stable.
  ADCSRA |= _BV(ADSC); // Start the ADC
  while (bit_is_set(ADCSRA,ADSC));
  wADC = ADCW;
  t = (wADC - 324.31 ) / 1.22;
  return (t);
}

float readvcc() {
  float tmp = 0.0;

  // Read 1.1V reference against Avcc
  // set the reference to vcc and the measurement to the internal 1.1V reference
  #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
      ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
      ADMUX = _BV(MUX5) | _BV(MUX0);
  #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
      ADMUX = _BV(MUX3) | _BV(MUX2);
  #else
      // works on an Arduino 168 or 328
      ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  #endif

  delay(3); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Start conversion
  while (bit_is_set(ADCSRA,ADSC)); // measuring

  uint8_t low  = ADCL; // must read ADCL first - it then locks ADCH
  uint8_t high = ADCH; // unlocks both

  tmp = (high<<8) | low;
  tmp = (typVbg * 1023.0) / tmp;

  return tmp;
}
