#include <Distance.h>

#define RUN_MODE_STOP    0
#define RUN_MODE_RUN     1

#define MOTOR_MODE_STOP      0
#define MOTOR_MODE_FORWARD   1
#define MOTOR_MODE_BACKWARD  2
#define MOTOR_MODE_RIGHT     3   
#define MOTOR_MODE_LEFT      4
#define MOTOR_MODE_BREAK     5

#define CLEAN_MODE_RANDAM    0
#define CLEAN_MODE_CORNER    1
#define CLEAN_MODE_BOTH      3


#define OUT_MOTOR1_A      4
#define OUT_MOTOR1_B      5
#define OUT_MOTOR1_PWM    3
#define OUT_MOTOR2_A      12
#define OUT_MOTOR2_B      13
#define OUT_MOTOR2_PWM    11
#define OUT_MOTOR_BRUSH_A      8
#define OUT_MOTOR_BRUSH_PWM    9
#define OUT_LED           10

#define IN_TOUCH_SENSOR   7
#define IN_BUTTON_MODE    6
#define IN_BUTTON_START   2
#define WINDUP_GUARD_GAIN 100.0
#define PID_UPDATE_INTERVAL 200 // milliseconds

#define PGAIN_ADR 0
#define IGAIN_ADR 4
#define DGAIN_ADR 8

int delayTime;

int presetMode;
int oldTouchSensorVal = 0;
int oldModeButtonVal = 0;
int oldStartButtonVal = 0;

int currentCleanMode = 0;
int currentMilliTime = 0;
int currentMotorMode;
int currentRunMode;
//time_t prevtime;
int startSec;

//---PID---
float iState = 0;
float lastDistance = 0;
float pgain;
float igain;
float dgain;
float pTerm, iTerm, dTerm; 
int pgainAddress, igainAddress, dgainAddress;
unsigned long lastPIDTime;  // most recent PID update time in ms 
int targetDistance;

Distance myDistance = Distance(4);

void setup(){
  pinMode(OUT_MOTOR1_A,OUTPUT); //信号用ピン
  pinMode(OUT_MOTOR1_B,OUTPUT); //信号用ピン
  pinMode(OUT_MOTOR2_A,OUTPUT); //信号用ピン
  pinMode(OUT_MOTOR2_B,OUTPUT); //信号用ピン 
  pinMode(OUT_MOTOR_BRUSH_A,OUTPUT); //信号用ピン  
  pinMode(OUT_LED, OUTPUT);
  pinMode(IN_TOUCH_SENSOR,INPUT); //入力用ピン   
  pinMode(IN_BUTTON_MODE,INPUT); //入力用ピン 
  pinMode(IN_BUTTON_START,INPUT); //入力用ピン   
  
  Serial.begin(9600); // シリアルポートを9600bpsで開く
  
  setP(30.0); // make sure to keep the decimal point on these values
  setI(0.0);  // make sure to keep the decimal point on these values
  setD(0.0);  // make sure to keep the decimal point on these values
  lastPIDTime = millis();
  targetDistance = 130;//100mm target

  delayTime = 1000;  

  currentRunMode = RUN_MODE_STOP;
}

void loop(){
  
  //赤外線距離センサ
  myDistance.available();

  currentMilliTime = millis();
  blinkLed(currentMilliTime);//LEDの点灯
  
  //=================STOP================================
  if(currentRunMode == RUN_MODE_STOP){  
    //Motor
    setMotorMode(MOTOR_MODE_STOP);
    
    //Mode Button
    if(isModeButtonPressed() == true){//Button up
      if(currentCleanMode == CLEAN_MODE_RANDAM)currentCleanMode = CLEAN_MODE_CORNER;
      else if(currentCleanMode == CLEAN_MODE_CORNER)currentCleanMode = CLEAN_MODE_BOTH;
      else currentCleanMode = CLEAN_MODE_RANDAM;
    }
    //Start Button
    if(isStartButtonPressed() == true){
      currentRunMode = RUN_MODE_RUN;
    }
    
  }
  else{//=================RUN================================
    if(currentCleanMode == CLEAN_MODE_CORNER){//壁際掃除モード
      
      if(isTouchSensorPressed() == true){
        motorBackAndTurnLeft();
      }      
      //PID制御
      // This checks for rollover with millis()
      if (currentMilliTime < lastPIDTime) {
        lastPIDTime = 0;
      }
      if ((currentMilliTime - lastPIDTime) > PID_UPDATE_INTERVAL) {
        lastPIDTime +=  PID_UPDATE_INTERVAL;
        int dist = myDistance.numberOfDistance();
        int input = updatePID(targetDistance, dist);
        
        setMotorRotation(input);
      }   
    }
    else{//ランダム反転掃除モード
      if(isTouchSensorPressed() == true){
        motorBackAndTurn();
      }
      else{
        setMotorMode(MOTOR_MODE_FORWARD);
      }
      
      setBrushMotorMode(MOTOR_MODE_FORWARD);
      
    }  
    
    if(isStartButtonPressed() == true){
      currentRunMode = RUN_MODE_STOP;
      setMotorMode(MOTOR_MODE_STOP);
      setBrushMotorMode(MOTOR_MODE_STOP);     
    }     
  }
  
}

//LEDの点滅
//  ランダム掃除モード：遅い点滅
//  壁際掃除モード    ：速い点滅
void blinkLed(int currentMilliTime){
    if(currentCleanMode == CLEAN_MODE_RANDAM){
      if(abs(currentMilliTime % 1000) < 500) digitalWrite(OUT_LED,HIGH);
      else digitalWrite(OUT_LED,LOW);
    }
    else if(currentCleanMode == CLEAN_MODE_CORNER){
      if(abs(currentMilliTime % 500) < 250) digitalWrite(OUT_LED,HIGH);
      else digitalWrite(OUT_LED,LOW);
    }
    else{//CLEAN_MODE_BOTH   
      digitalWrite(OUT_LED,HIGH);
    }
}

//モード変更ボタンが押されたときの処理
boolean isModeButtonPressed(){
    //BUTTON
    boolean ret = false;
    int modeButtonVal = digitalRead(IN_BUTTON_MODE);
    if((modeButtonVal == LOW) && (oldModeButtonVal == HIGH)){//Button up
      ret = true;
      delay(50);//チャタリング対策
    }
    oldModeButtonVal = modeButtonVal;  
    return ret;
}

//スタートボタンが押されたときの処理
boolean isStartButtonPressed(){
    //BUTTON
    boolean ret = false;
    int startButtonVal = digitalRead(IN_BUTTON_START);
    if((startButtonVal == LOW) && (oldStartButtonVal == HIGH)){//Button up
      ret = true;
      delay(50);//チャタリング対策
    }
    oldStartButtonVal = startButtonVal;  
    return ret;
}

//前方の衝突検知用のタッチセンサが反応したときの処理
boolean isTouchSensorPressed(){
    //BUTTON
    boolean ret = false;
    int touchSensorVal = digitalRead(IN_TOUCH_SENSOR);
    if((touchSensorVal == HIGH) && (oldTouchSensorVal == LOW)){//Button up
      ret = true;
      delay(50);//チャタリング対策
    }
    oldTouchSensorVal = touchSensorVal;  
    return ret;
}

//掃除ブラシのモータ制御
void setBrushMotorMode(int mode)
{
  switch(mode){
    case MOTOR_MODE_STOP:
      digitalWrite(OUT_MOTOR_BRUSH_A,LOW);
      analogWrite(OUT_MOTOR_BRUSH_PWM,0);    
      break;
 
     case MOTOR_MODE_FORWARD:
      digitalWrite(OUT_MOTOR_BRUSH_A,HIGH);
      analogWrite(OUT_MOTOR_BRUSH_PWM,200);    
      break;
  }  
}

//走行用モータの制御
void setMotorMode(int mode){
  if(mode == currentMotorMode)return;
  
  currentMotorMode = mode;
  switch(mode){
    case MOTOR_MODE_BREAK:
      digitalWrite(OUT_MOTOR1_A,HIGH);
      digitalWrite(OUT_MOTOR1_B,HIGH);
      digitalWrite(OUT_MOTOR2_A,HIGH);
      digitalWrite(OUT_MOTOR2_B,HIGH);      
      analogWrite(OUT_MOTOR1_PWM,0);    
      analogWrite(OUT_MOTOR2_PWM,0);   
      break;
      
    case MOTOR_MODE_STOP:
      digitalWrite(OUT_MOTOR1_A,LOW);
      digitalWrite(OUT_MOTOR1_B,LOW);
      digitalWrite(OUT_MOTOR2_A,LOW);
      digitalWrite(OUT_MOTOR2_B,LOW);      
      analogWrite(OUT_MOTOR1_PWM,0);    
      analogWrite(OUT_MOTOR2_PWM,0);   
      break;
 
    case MOTOR_MODE_BACKWARD:
      digitalWrite(OUT_MOTOR1_A,HIGH);
      digitalWrite(OUT_MOTOR1_B,LOW);
      digitalWrite(OUT_MOTOR2_A,HIGH);
      digitalWrite(OUT_MOTOR2_B,LOW);        
      analogWrite(OUT_MOTOR1_PWM,200);  
      analogWrite(OUT_MOTOR2_PWM,200);   
      break;
     
     case MOTOR_MODE_FORWARD:
      digitalWrite(OUT_MOTOR1_A,LOW);
      digitalWrite(OUT_MOTOR1_B,HIGH);
      digitalWrite(OUT_MOTOR2_A,LOW);
      digitalWrite(OUT_MOTOR2_B,HIGH); 
      analogWrite(OUT_MOTOR1_PWM,200);  
      analogWrite(OUT_MOTOR2_PWM,200); 
      break;

    case MOTOR_MODE_LEFT:
      digitalWrite(OUT_MOTOR1_A,HIGH);
      digitalWrite(OUT_MOTOR1_B,LOW);
      digitalWrite(OUT_MOTOR2_A,LOW);
      digitalWrite(OUT_MOTOR2_B,HIGH);         
      analogWrite(OUT_MOTOR1_PWM,200);  
      analogWrite(OUT_MOTOR2_PWM,200); 
      break;

    case MOTOR_MODE_RIGHT:
      digitalWrite(OUT_MOTOR1_A,LOW);
      digitalWrite(OUT_MOTOR1_B,HIGH);
      digitalWrite(OUT_MOTOR2_A,HIGH);
      digitalWrite(OUT_MOTOR2_B,LOW);  
      analogWrite(OUT_MOTOR1_PWM,200);  
      analogWrite(OUT_MOTOR2_PWM,200); 
      break;      
  
  }

}


//少し後退して反転する処理
void motorBackAndTurn(){
	
  int rand = random(1, 6) ;
  int rotationTime = rand * 500;//500~3000msec
  setMotorMode(MOTOR_MODE_BREAK);
  delay(500);
  setMotorMode(MOTOR_MODE_BACKWARD);
  delay(1000);
  setMotorMode(MOTOR_MODE_BREAK);
  delay(500);  
  setMotorMode(MOTOR_MODE_RIGHT);
  delay(rotationTime);  
  //setMotorMode(MOTOR_MODE_STOP);
  setMotorMode(MOTOR_MODE_FORWARD);
}

//少し後退して左に反転する処理
void motorBackAndTurnLeft(){
	
  setMotorMode(MOTOR_MODE_BREAK);
  delay(500);
  setMotorMode(MOTOR_MODE_BACKWARD);
  delay(500);
  setMotorMode(MOTOR_MODE_BREAK);
  delay(500);  
  setMotorMode(MOTOR_MODE_LEFT);
  delay(500);  
  setMotorMode(MOTOR_MODE_FORWARD);
}

//壁際走行中のモータ出力制御
//入力は -255 〜 255
void setMotorRotation(int input){
  
  if(input > 1000)input = 1000;
  else if(input < (-1000))input = (-1000); 
 
  input = input / 4;//input is -255 to 255
  
  int pwmInput2 = (input / 4) + 128;//0to255
  int pwmInput1 = 255 - pwmInput2;//0to2555
  
    
      digitalWrite(OUT_MOTOR1_A,LOW);
      digitalWrite(OUT_MOTOR1_B,HIGH);
      digitalWrite(OUT_MOTOR2_A,LOW);
      digitalWrite(OUT_MOTOR2_B,HIGH);    
    analogWrite(OUT_MOTOR1_PWM,pwmInput1);  
    analogWrite(OUT_MOTOR2_PWM,pwmInput2); 


  Serial.print("$distance=");
  Serial.print(lastDistance);
  Serial.print("$motor_r=");
  Serial.print(pwmInput1);
  Serial.print("$motor_l=");
  Serial.println(pwmInput2);


}


//===================PID===========================

void setupPID(unsigned int padd, int iadd, int dadd) {
  // with this setup, you pass the addresses for the PID algorithm to use to 
  // for storing the gain settings.  This way wastes 6 bytes to store the addresses,
  // but its nice because you can keep all the EEPROM address allocaton in once place.

  pgainAddress = padd;
  igainAddress = iadd;
  dgainAddress = dadd;

//  pgain = readFloat(pgainAddress);
//  igain = readFloat(igainAddress);
//  dgain = readFloat(dgainAddress);
}

float getP() {
  // get the P gain 
  return pgain;
}
float getI() {
  // get the I gain
  return igain;
}
float getD() {
  // get the D gain
  return dgain;
}


void setP(float p) {
  // set the P gain and store it to eeprom
  pgain = p; 
  //writeFloat(p, pgainAddress);
}

void setI(float i) {
  // set the I gain and store it to eeprom
  igain = i; 
  //writeFloat(i, igainAddress);
}

void setD(float d) {
  // set the D gain and store it to eeprom
  dgain = d; 
 // writeFloat(d, dgainAddress);
}

float updatePID(float targetDistance, float curDistance)
{
  // these local variables can be factored out if memory is an issue, 
  // but they make it more readable
  double result;
  float error;
  float windupGaurd;

  // determine how badly we are doing
  error = targetDistance - curDistance;

  // the pTerm is the view from now, the pgain judges 
  // how much we care about error we are this instant.
  pTerm = pgain * error;

  // iState keeps changing over time; it's 
  // overall "performance" over time, or accumulated error
  iState += error;

  // to prevent the iTerm getting huge despite lots of 
  //  error, we use a "windup guard" 
  // (this happens when the machine is first turned on and
  // it cant help be cold despite its best efforts)

  // not necessary, but this makes windup guard values 
  // relative to the current iGain
  windupGaurd = WINDUP_GUARD_GAIN / igain;  

  if (iState > windupGaurd) 
    iState = windupGaurd;
  else if (iState < -windupGaurd) 
    iState = -windupGaurd;
  iTerm = igain * iState;

  // the dTerm, the difference between the temperature now
  //  and our last reading, indicated the "speed," 
  // how quickly the temp is changing. (aka. Differential)
  dTerm = (dgain* (curDistance - lastDistance));

  // now that we've use lastDistance, put the current temp in
  // our pocket until for the next round
  lastDistance = curDistance;


  float ret = pTerm + iTerm - dTerm;
  

  // the magic feedback bit
  return ret;
}

void printPIDDebugString() {
//  // A  helper function to keep track of the PID algorithm 
//  Serial.print("PID formula (P + I - D): ");
//
//  printFloat(pTerm, 2);
//  Serial.print(" + ");
//  printFloat(iTerm, 2);
//  Serial.print(" - ");
//  printFloat(dTerm, 2);
//  Serial.print(" POWER: ");
//  printFloat(getHeatCycles(), 0);
//  Serial.print(" ");

}