//Torque Sensor Panasonic Motor Velo VERSION 1.2
unsigned long iTimeRead = 0;
unsigned long iTimeToStop = 0;
unsigned long iTimeToStop2 = 0;
unsigned long iTimeToRun = 0;
unsigned long iTimePedalIdle = 0;
unsigned long iTimeSpeedDown = 0;
unsigned long iTimePressButton = 0;
unsigned long iReads = 0;
unsigned long iReads2 = 0;
unsigned long iNow = 0;
unsigned long SpeedRead = 0;
unsigned long SpeedReadPrev = 0;
unsigned long iReadButton = 0;
float fReadTorque = 0;
float fTorquePrev = 0;
float fTorque_idle = 0;
float fTorque_idle_min = 0;
float fTorque_idle_max = 0;
float fTorque_dif = 0;
float fTorqueMin = 0; // for value torque when no pedaling
int iSpeed = 0;
int iSentSpeed = 0;
int i = 0;
int iSpeedStep = 0;
bool RunMot = 1;
bool Assist_ON = 0; // ON / OFF pedal assistant
bool Helper_ON = 0; // Run small speed to help push a bike with your hands
const int sizeArray = 2;
float TorqueDifArray[sizeArray] = {};
int ArrCount = 0;

void setup() {
  #define TIME_READ 100  // ms to integrate analogRead
  #define TIME_READY 500 // ms to read lowest torque before being ready to ride
  #define TIME_TO_STOP 2000 // ms to Emergency stop motor
  #define TIME_TO_RUN 600 // ms to start pedal assist
  #define TIME_PEDAL_IDLE 1500 // ms. Time to detect pedaling idle
  #define TIME_SPEED_DOWN 600 // ms. Time to down speed motor to 0
 // #define TORQUE_DIF 30 // Difference betwin fTorqueMin and Max Torque
  #define TORQUE_DIF_NORMAL 25 // Difference Torque to normal pedaling mode
  #define iSPEED_START 85 // start value for iSpeed
  #define iSPEED_END 255 // end value for iSpeed
  //#define POLUSES 24 // how many poluses have motor

  #define pin_torque_Pwm PA8
  #define pin_torque_read PA0
  #define pin_Mot PA3
  #define pin_Speed_read PB10
  #define pin_Button PA6
  #define pin_Buzzer PB0

  pinMode(pin_torque_Pwm, OUTPUT);
  pinMode(pin_torque_read,INPUT);
  pinMode(pin_Mot,OUTPUT);
  pinMode(pin_Speed_read,INPUT);
  pinMode(pin_Button,INPUT);
  pinMode(pin_Buzzer, OUTPUT);

  tone(pin_torque_Pwm,65535);

  iTimeRead = millis() + TIME_READ;
  iTimeToStop = millis() + TIME_TO_STOP;
  iTimeToStop2 = millis() + TIME_TO_STOP;
  iTimeToRun = millis() + TIME_TO_RUN;
  iTimePedalIdle = millis() + TIME_PEDAL_IDLE;
  iTimeSpeedDown = millis() + TIME_SPEED_DOWN;
  iSpeed = iSPEED_START;
}

void loop() {
  iNow = millis();

  SpeedRead = pulseIn(pin_Speed_read, HIGH, 40000L);
  if (SpeedRead > 0)
    SpeedRead = 1000000 / SpeedRead;

  iTimeRead = millis() + TIME_READ;

  for (i = 0; iTimeRead > millis(); i++) {
    iReadButton += analogRead(pin_Button);
    fReadTorque += analogRead(pin_torque_read);
  }
  fReadTorque /= i;
  fReadTorque = 100 / fReadTorque;

  int iButton = iReadButton / i;
  iReadButton = 0;

  if (iButton > 500){ // Button operations
    if (iTimePressButton == 0)
      iTimePressButton = iNow;
    if ((iNow - iTimePressButton) > 1000){
      analogWrite(pin_Mot, 80);
      Helper_ON = 1;
    }
  } else {
    if (iTimePressButton > 0 && iNow - iTimePressButton < 500){
      if (Assist_ON) {
        analogWrite(pin_Buzzer, 150);
        delay(100);
        analogWrite(pin_Buzzer, 0);
        delay(100);
        analogWrite(pin_Buzzer, 150);
        delay(100);
        analogWrite(pin_Buzzer, 0);
        Assist_ON = 0;
      }
      else {
        analogWrite(pin_Buzzer, 150);
        delay(100);
        analogWrite(pin_Buzzer, 0);
        Assist_ON = 1;
      }
    }
    if (iNow - iTimePressButton > 1000) {
      analogWrite(pin_Mot, 0);
      Helper_ON = 0;
    }
    iTimePressButton = 0;
  }
 
  if (fTorqueMin == 0)
    fTorqueMin = fReadTorque;
  // if (fTorqueMin == 0 && TIME_READY < iNow)
  //   fTorqueMin = fReadTorque;
  // if (fReadTorque < fTorqueMin)
  //   fTorqueMin = fReadTorque;

  if (iTimePedalIdle > iNow) { // Detect pedal idle
    fTorque_idle = abs(fReadTorque - fTorquePrev);
    if (fTorque_idle < fTorque_idle_min || fTorque_idle_min == 0)
      fTorque_idle_min = fTorque_idle;
    if (fTorque_idle > fTorque_idle_max || fTorque_idle_max == 0)
      fTorque_idle_max = fTorque_idle;
    if (fTorque_idle_max - fTorque_idle_min > 10 || SpeedRead > 0){
      fTorque_idle_min = 0;
      fTorque_idle_max = 0;
      iTimePedalIdle = millis() + TIME_PEDAL_IDLE;
    }
  }
  else {
    if (fTorque_idle_max - fTorque_idle_min <= 10){
      fTorqueMin = fTorquePrev;
    }
    fTorque_idle_min = 0;
    fTorque_idle_max = 0;
    iTimePedalIdle = millis() + TIME_PEDAL_IDLE;
  }

  iSpeedStep = 12;
  if ((iSpeed-iSPEED_START) <= 20 && SpeedRead < 120) // smooth start from 0 speed
    iSpeedStep = iSpeedStep * (iSpeed - iSPEED_START) / 20 + 1;

  fTorque_dif = fReadTorque-fTorqueMin;
  TorqueDifArray[ArrCount] = fTorque_dif;
  ArrCount++;
  if (ArrCount == sizeArray)
    ArrCount = 0;
  fTorque_dif = 0;
  for(i = 0; i < sizeArray; i++)
    fTorque_dif += TorqueDifArray[i];
  fTorque_dif /= sizeArray;

  if (SpeedRead > 0 && iSpeed > iSPEED_START)
    fTorque_dif += 30; //Torque correction

  if (fTorque_dif > TORQUE_DIF_NORMAL) {
    iSpeedStep = iSpeedStep-round(iSpeedStep/((fTorque_dif - TORQUE_DIF_NORMAL)/8 + 1))+1;
    if (SpeedRead <= SpeedReadPrev) //waiting stabilization motor speed
      iSpeed += iSpeedStep; //increase speed
    if (iSpeed > iSPEED_END)
      iSpeed = iSPEED_END;
    iSentSpeed = iSpeed;
  }
  else {
    iSpeedStep = iSpeedStep-round(iSpeedStep/((TORQUE_DIF_NORMAL - fTorque_dif)/8 + 1))+1;
    if (fTorque_dif < TORQUE_DIF_NORMAL - 5)
      iSpeed -= iSpeedStep; //decriase speed
    iSentSpeed = iSpeed;
    // iSentSpeed = 0;
  }
  if (fTorque_dif <= 10) {
    if (iTimeSpeedDown < iNow)
      iSentSpeed = 0; //motor speed down to 0 after pause if low torque
  }
  else {
    iTimeSpeedDown = millis() + TIME_SPEED_DOWN;
  }
 
  if (iSpeed <= iSPEED_START){
    iSpeed = iSPEED_START;
    iSentSpeed = 0;
  }
 
  RunMot = 1;

  if (SpeedRead == 0 && iSentSpeed > 0){ // emergency stop motor
    if (iNow > iTimeToStop){
      RunMot = 0;
      iTimeToRun = millis() + TIME_TO_RUN;
    }
  }
  else {
    iTimeToStop = millis() + TIME_TO_STOP;
  }

  if (iSentSpeed > 0 && iTimeToRun < iNow)
    iTimeToStop2 = millis() + TIME_TO_STOP;

  if (iSentSpeed == 0 && iTimeToStop2 < iNow)
    iTimeToRun = millis() + TIME_TO_RUN;

  if (iTimeToRun > iNow) { // pause before start motor after idle
    RunMot = 0;
    // iSpeed = iSPEED_START+1;
    iTimeToStop = millis() + TIME_TO_STOP;
  }

  if (RunMot && Assist_ON){
    analogWrite(pin_Mot, iSentSpeed);
  }
  else if (!Helper_ON){
    analogWrite(pin_Mot, 0);
  }

  Serial.print("fTorqueMin= ");Serial.print(fTorqueMin);
  Serial.print("  fReadTorque= ");Serial.print(fReadTorque);
  Serial.print("  fTorque_dif= ");Serial.print(fTorque_dif);
  Serial.print("  iSpeedStep= ");Serial.print(iSpeedStep);
  Serial.print("  iSentSpeed= ");Serial.print(iSentSpeed);
  Serial.print("  SpeedRead= ");Serial.print(SpeedRead);
  Serial.print("  RunMot= ");Serial.println(RunMot);

  SpeedReadPrev = SpeedRead;
  fTorquePrev = fReadTorque;
  fReadTorque = 0;
  iReads = 0;
}