//========= Pac-Attac Antweight Meltybrain ==============================
//
//  Warner King
//  May 2021
//

// -------------------------------- Libraries ---------------------------
#include "FlySkyIBus.h"
#include <MPU6050.h>
#include "Wire.h"
#include <EEPROM.h>
// -------------------------------- I/O Pins ----------------------------
#define TACHOMETER_IN_PIN 2           //in from op-amp comparator
#define TACHOMETER_OUT_PIN 13         // out to tachometer LED. Currently just set high 

#define LEFT_MOTOR_1_PIN 11
#define LEFT_MOTOR_2_PIN 10

#define RIGHT_MOTOR_1_PIN 9
#define RIGHT_MOTOR_2_PIN 3

#define VIBE_MOTOR_PIN 12

#define LED_1_PIN 8
#define LED_2_PIN 7
#define LED_3_PIN 5
#define LED_4_PIN 6
#define LED_5_PIN 4

#define MPU6050_SDA_PIN A4
#define MPU6050_SCL_PIN A5

#define RX_IBUS_IN_PIN 0


// -------------------------------- R/C Input ---------------------------
#define RIGHT_HORIZ 0                   //iBUS channels from the receiver. These can be changed in the transmitter as well
#define RIGHT_VERT 1
#define LEFT_VERT 2
#define LEFT_HORIZ 3
#define AUXA 4
#define AUXB 5
#define AUXC 6
#define AUXD 7
#define VARA 8
#define VARB 9
#define FAILSAFE_UNIQUE 12              //My receiver outputs random data pn this channel constantly if there is a valid connection to the transmitter. Used for failsafe

//#define WRITEEEPROM_SWITCH AUXA
#define ANIMATION_SWITCH AUXA
#define VIBE_SWITCH AUXB
#define POSITIONING_MODE_SWITCH AUXC
#define REVERSE_SWITCH AUXD

#define COARSE_ADJUST_DIAL VARA
#define FINE_ADJUST_DIAL VARB

#define SPIN_THROTTLE LEFT_VERT

#define JOYSTICK_CENTER 1500
#define JOYSTICK_OVER_MAX 2100
#define JOYSTICK_UNDER_MIN 900

#define DEADZONE 4
// -------------------------------- Constants ---------------------------
#define LEFT_MOTOR 0
#define RIGHT_MOTOR 1
#define VIBE_MOTOR 2

#define STOPPED_STATE 0
#define TANK_DRIVE_STATE 1
#define SPIN_TACH_STATE 2
#define SPIN_ACCEL_STATE 3
#define SPIN_MANUAL_STATE 4

#define SPIN_THROTTLE_MIN_THRESHOLD 1200
#define FORWARD_DIRECTION_MIN_THRESHOLD 1600
#define TACH_SPIN_MODE_BELOW 1200
#define ACCEL_SPIN_MODE_BELOW 1700
#define MANUAL_SPIN_MODE_BELOW 2100

#define FRONT_ANGLE_OF_BOT 90.0
#define RADIUS_TO_ACCEL 4.0               //millimeters between bot center and center of accel chip
#define FAILSAFE_ATTEMPTS 100             //number of times to loop before declaring loss of signal
// -------------------------------- Global Variables --------------------
MPU6050 mpu;

unsigned int cyclesBeforeUpdating = 0;
unsigned int wheelRPM;
unsigned int wheelRPS;
unsigned int botRPM;
float botRPS;
volatile int wheelRPMCount;
unsigned long timeOldWheelRPM = 0;
unsigned long timeOldBotRPM = 0;
unsigned int currentBotAngle = 0;
float currentBotAngleF = 0;
unsigned int spinSpeed = 0;
unsigned int systemState = STOPPED_STATE;

unsigned int accelCycles = 0;
long accelAveraging = 0.0;

int previousFailsafeValue = 0;
int failsafeCounter = 0; 


float pacmanMouth = 0;
bool pacmanMouthOpening = false;

int eeAddress = 0;
int dataloggingCounter = 0;
// -------------------------------- Setup -------------------------------

void rpm_fun() {
  //Update count
  wheelRPMCount++;
}

void setup() {
  Wire.begin();
  Serial.begin(115200);
  IBus.begin(Serial);
  pinMode(TACHOMETER_IN_PIN, INPUT);
  pinMode(RX_IBUS_IN_PIN, INPUT);
  pinMode(TACHOMETER_OUT_PIN, OUTPUT);
  pinMode(LEFT_MOTOR_1_PIN, OUTPUT);
  pinMode(LEFT_MOTOR_2_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_1_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_2_PIN, OUTPUT);
  pinMode(VIBE_MOTOR_PIN, OUTPUT);
  pinMode(LED_1_PIN, OUTPUT);
  pinMode(LED_2_PIN, OUTPUT);
  pinMode(LED_3_PIN, OUTPUT);
  pinMode(LED_4_PIN, OUTPUT);
  pinMode(LED_5_PIN, OUTPUT);

  digitalWrite(TACHOMETER_OUT_PIN, HIGH);

  attachInterrupt(0, rpm_fun, FALLING);
  wheelRPMCount = 0;
  wheelRPM = 0;
  wheelRPS = 0;
  botRPM = 0;
  botRPS = 0;
  timeOldWheelRPM = 0;
  delay(500);
  Serial.println(" MPU6050 sensor start");

  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_16G)) {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  Serial.println(" MPU6050 sensor end");
  delay(100);
  mpu.setRange(MPU6050_RANGE_16G);

  Serial.print(" * Accelerometer:         ");
  switch(mpu.getRange())
  {
    case MPU6050_RANGE_16G:            Serial.println("+/- 16 g"); break;
    case MPU6050_RANGE_8G:             Serial.println("+/- 8 g"); break;
    case MPU6050_RANGE_4G:             Serial.println("+/- 4 g"); break;
    case MPU6050_RANGE_2G:             Serial.println("+/- 2 g"); break;
  }  
}

struct SpeedDataObject {
  float rpsT;
  float acc;
  float acc1;
  float acc2;
};

void loop() {
  IBus.loop();
  Vector rawAccel = mpu.readRawAccel();
  bool validRCInput = isRCInputValid();

//printOutRCOutput();
  
  switch(systemState) {
    case STOPPED_STATE:
      Serial.println("Stopped state!");
      if (validRCInput) {
        systemState = TANK_DRIVE_STATE;
      }
      break;
    case TANK_DRIVE_STATE:
      if (!validRCInput) {
        systemState = STOPPED_STATE;
      } else if (IBus.readChannel(SPIN_THROTTLE) >= SPIN_THROTTLE_MIN_THRESHOLD) {
        systemState = getSpinStateFromSwitch();
      }
      break;
    case SPIN_TACH_STATE:
    case SPIN_ACCEL_STATE:
    case SPIN_MANUAL_STATE:
      if (!validRCInput) {
        systemState = STOPPED_STATE;
      } else if (IBus.readChannel(SPIN_THROTTLE) < SPIN_THROTTLE_MIN_THRESHOLD) {
        systemState = TANK_DRIVE_STATE;
      } else {
        systemState = getSpinStateFromSwitch();
      }
      break;
    default:
      break;
  }

  switch(systemState) {
    case STOPPED_STATE:
      setMotorOutput(VIBE_MOTOR, 0, true);
      setMotorOutput(RIGHT_MOTOR, 0, true);
      setMotorOutput(LEFT_MOTOR, 0, true);
      break;
    case TANK_DRIVE_STATE:
      tankDrive();
      if (IBus.readChannel(VIBE_SWITCH) > JOYSTICK_CENTER) {
        setMotorOutput(VIBE_MOTOR, 254, true);
      } else {
        setMotorOutput(VIBE_MOTOR, 0, true);
      }
      break;
    case SPIN_TACH_STATE:
      setMotorOutput(VIBE_MOTOR, 0, true);
      cyclesBeforeUpdating++;
      if (cyclesBeforeUpdating > 100) { 
        updateWheelRPM();
        cyclesBeforeUpdating = 0;
      }
      updateBotRPM();
      updateBotAngle();
      updateAngleFromRC();
      spinDrive();
      break;
    case SPIN_ACCEL_STATE:
      setMotorOutput(VIBE_MOTOR, 0, true);
      updateSpeedFromAccel(rawAccel);
      updateBotAngle();
      updateAngleFromRC();
      spinDrive();
      break;

    case SPIN_MANUAL_STATE:
      botRPS = map(IBus.readChannel(COARSE_ADJUST_DIAL), 999, 2001, 5, 50);
      updateBotAngle();
      updateAngleFromRC();
      spinDrive();
      setMotorOutput(VIBE_MOTOR, 0, true);
      break;
    default:
      break;
  } 
  
  switch(systemState) {
    case STOPPED_STATE:
      setLEDs(LOW, LOW, LOW, LOW, HIGH);
      break;
    case TANK_DRIVE_STATE:
      setLEDs(HIGH, HIGH, LOW, LOW, LOW);
      break;
    case SPIN_TACH_STATE:
    case SPIN_ACCEL_STATE:
    case SPIN_MANUAL_STATE:
      if (IBus.readChannel(ANIMATION_SWITCH) > JOYSTICK_CENTER) {

          if (pacmanMouthOpening) {
            pacmanMouth = pacmanMouth + 0.1;
          } else {
            pacmanMouth = pacmanMouth - 0.1;
          }

          if (pacmanMouth > 30.0) {
            pacmanMouthOpening = false;
          } else if (pacmanMouth < 1.0) {
            pacmanMouthOpening = true;
          }

          if (currentBotAngleF < (pacmanMouth + FRONT_ANGLE_OF_BOT) && currentBotAngleF > (FRONT_ANGLE_OF_BOT - pacmanMouth)) {
            setAllLEDs(LOW);
          } else {
            setAllLEDs(HIGH);
          }
      } else {
        if (currentBotAngleF < (10.0 + FRONT_ANGLE_OF_BOT) && currentBotAngleF > (FRONT_ANGLE_OF_BOT - 10.0)){
          setAllLEDs(HIGH);
        } else {
          setAllLEDs(LOW);
        }
      }
      break;
    default:
      break;
  } 
}

void updateSpeedFromAccel(Vector accelInput){

  //float radiusToAccel = 5.16; //millimeters between bot center and center of accel chip

  float divisor = 0.8313; //(IBus.readChannel(COARSE_ADJUST_DIAL) / 1500.0);
  float divisor2 = (IBus.readChannel(FINE_ADJUST_DIAL) / 1500.0);
  float correctedRadius = RADIUS_TO_ACCEL * divisor2;

  float accelCorrected = accelInput.YAxis * divisor;
  
  accelCycles++;
  accelAveraging = accelAveraging + accelCorrected;

  if (accelCycles > 100) {
    float averagedAccel = (float) (abs(accelAveraging) / accelCycles) * 0.0005;

    float mid = (float) averagedAccel / (1.118 * correctedRadius);
    botRPS =  sqrt(mid) * 16.666;

    //Serial.print(botRPS);
    //Serial.print(" ");
    //Serial.print(averagedAccel);
    //Serial.println(" ");
    accelCycles = 0;
    accelAveraging = 0L;
  }
}

void updateBotAngle() {
//  int timer = millis() - timeOldBotRPM;
//  timeOldBotRPM = millis();
//
//  float amountToIncrementAngle = (float) ((botRPS * 36.0)/100.0) * timer;

  int timer = micros() - timeOldBotRPM;
  timeOldBotRPM = micros();

  float amountToIncrementAngleMid = (float) ((botRPS * 36.0)/100.0) * timer;
  float amountToIncrementAngle = (float) amountToIncrementAngleMid / 1000.0;
  currentBotAngleF = currentBotAngleF + amountToIncrementAngle;

  if (currentBotAngleF > 360.0) {
    currentBotAngleF = currentBotAngleF - 360.0;
  }
}

void updateBotRPM() {
  float divisor = (IBus.readChannel(COARSE_ADJUST_DIAL) / 1000.0) + 1.0;
  divisor = divisor + (IBus.readChannel(FINE_ADJUST_DIAL) / 2000.0);
 // botRPM = (float) wheelRPM / divisor;

  botRPS = (float) wheelRPS / divisor;
}

void updateWheelRPM() {
  detachInterrupt(0);
  //Note that this would be 60*1000/(millis() - timeold)*rpmcount if the interrupt
  //happened once per revolution instead of twice. Other multiples could be used
  //for multi-bladed propellers or fans
  //wheelRPM = (float) 60*1000/(millis() - timeOldWheelRPM)*wheelRPMCount;
  wheelRPS = (float) 1000/(millis() - timeOldWheelRPM)*wheelRPMCount;
  timeOldWheelRPM = millis();
  wheelRPMCount = 0;
  
  //Restart the interrupt processing
  attachInterrupt(0, rpm_fun, RISING);
}

void setAllLEDs(int state) {
  digitalWrite(LED_1_PIN, state);
  digitalWrite(LED_2_PIN, state);
  digitalWrite(LED_3_PIN, state);
  digitalWrite(LED_4_PIN, state);
  digitalWrite(LED_5_PIN, state);
}

void setLEDs(int state1, int state2, int state3, int state4, int state5) {
  digitalWrite(LED_1_PIN, state1);
  digitalWrite(LED_2_PIN, state2);
  digitalWrite(LED_3_PIN, state3);
  digitalWrite(LED_4_PIN, state4);
  digitalWrite(LED_5_PIN, state5);
}

void updateAngleFromRC() {
  int steering = (IBus.readChannel(RIGHT_HORIZ) - JOYSTICK_CENTER);

  float amountToChange = (float) steering / 500.0;

  if (IBus.readChannel(REVERSE_SWITCH) > JOYSTICK_CENTER){      //reverse outputs for driving backwards or upside down
    amountToChange = -amountToChange;
  }

  currentBotAngleF = currentBotAngleF - amountToChange;
  
  if (currentBotAngleF > 360.0) {
    currentBotAngleF = currentBotAngleF - 360.0;
  } else if (currentBotAngleF < 0.0) {
    currentBotAngleF = currentBotAngleF + 360.0;
  }
}

void spinDrive() {
  int throttle = IBus.readChannel(SPIN_THROTTLE);
  bool driveDirection = 1;
  
  if (IBus.readChannel(REVERSE_SWITCH) > JOYSTICK_CENTER){      //reverse outputs for driving backwards or upside down
    driveDirection = 0 ;
  }
  
  int motorPWM = map(throttle, 990, 2010, 0, 254);
 
  float currentBotAngleSpinner = currentBotAngleF + 180.0; //270
  if (currentBotAngleSpinner > 360.0) {
    currentBotAngleSpinner = currentBotAngleSpinner - 360.0;
  }
  if (IBus.readChannel(RIGHT_VERT) > FORWARD_DIRECTION_MIN_THRESHOLD) {
    // 
    motorPWM = motorPWM * 2;
    if (motorPWM > 254) {
      motorPWM = 254;
    }
    if (currentBotAngleF > (90.0 - 75.0) && currentBotAngleF < (90.0 + 75.0)) {
      setMotorOutput(RIGHT_MOTOR, 0, !driveDirection);
      setMotorOutput(LEFT_MOTOR, motorPWM, driveDirection);
    } else if (currentBotAngleF > (270.0 - 75.0) && currentBotAngleF < (270.0 + 75.0)){
      setMotorOutput(RIGHT_MOTOR, motorPWM, !driveDirection);
      setMotorOutput(LEFT_MOTOR, 0, driveDirection);
    } else {
      setMotorOutput(RIGHT_MOTOR, motorPWM, !driveDirection);
      setMotorOutput(LEFT_MOTOR, motorPWM, driveDirection);  
    }
  } else {
    setMotorOutput(RIGHT_MOTOR, motorPWM, !driveDirection);
    setMotorOutput(LEFT_MOTOR, motorPWM, driveDirection);  
  }
}

void tankDrive() {
  int y = (IBus.readChannel(RIGHT_VERT) - JOYSTICK_CENTER);
  int x = -(IBus.readChannel(RIGHT_HORIZ) - JOYSTICK_CENTER);
  
  float V =(500-abs(x)) * (y/500.0) + y;
  float W= (500-abs(y)) * (x/500.0) + x;
  
  int R = (float)(V+W) /2.0;
  int L = (float)(V-W)/2.0; 

  if (IBus.readChannel(REVERSE_SWITCH) > JOYSTICK_CENTER){      //reverse outputs for driving backwards or upside down
    int temp = L;
    L = -R;
    R = -temp;
  }
   
  int Rpwm = map(abs(R), 0, 501, 0, 254);
  int Lpwm = map(abs(L), 0, 501, 0, 254);

  if (Lpwm < DEADZONE){
    setMotorOutput(LEFT_MOTOR, 0, 0);
  }else if (L >= 0) {
    setMotorOutput(LEFT_MOTOR, Lpwm, 1);
  }else{
    setMotorOutput(LEFT_MOTOR, Lpwm, 0);
  }
  if (Rpwm < DEADZONE){
    setMotorOutput(RIGHT_MOTOR, 0, 0);
  }else if (R >= 0) {
    setMotorOutput(RIGHT_MOTOR, Rpwm, 1);
  }else{
    setMotorOutput(RIGHT_MOTOR, Rpwm, 0);
  }
}

bool isRCInputValid() {
  if (IBus.readChannel(FAILSAFE_UNIQUE) == previousFailsafeValue) {
    if (failsafeCounter <= FAILSAFE_ATTEMPTS) {
      failsafeCounter++;
    }
  } else {
    failsafeCounter = 0;
  }
  previousFailsafeValue = IBus.readChannel(FAILSAFE_UNIQUE);
  if (failsafeCounter >= FAILSAFE_ATTEMPTS) {
    return false;
  }
  
  if ((IBus.readChannel(RIGHT_VERT) > JOYSTICK_OVER_MAX) or (IBus.readChannel(RIGHT_VERT) < JOYSTICK_UNDER_MIN)){
    return false;  
  }
  if ((IBus.readChannel(RIGHT_HORIZ) > JOYSTICK_OVER_MAX) or (IBus.readChannel(RIGHT_HORIZ) < JOYSTICK_UNDER_MIN)){
    return false;  
  }
  return true;  
}

int getSpinStateFromSwitch() {  
  if (IBus.readChannel(POSITIONING_MODE_SWITCH) < TACH_SPIN_MODE_BELOW) {
    return SPIN_TACH_STATE;
  } else if (IBus.readChannel(POSITIONING_MODE_SWITCH) < ACCEL_SPIN_MODE_BELOW) {
    return SPIN_ACCEL_STATE;
  } else if (IBus.readChannel(POSITIONING_MODE_SWITCH) < MANUAL_SPIN_MODE_BELOW) {
    return SPIN_MANUAL_STATE;
  }
  return STOPPED_STATE;
}

void setMotorOutput(int motorToChange, int outputSpeed, bool forward) {
  switch(motorToChange) {
    case LEFT_MOTOR:
      if (forward) {
        analogWrite(LEFT_MOTOR_1_PIN, outputSpeed);
        analogWrite(LEFT_MOTOR_2_PIN, 0);
      } else {
        analogWrite(LEFT_MOTOR_1_PIN, 0);
        analogWrite(LEFT_MOTOR_2_PIN, outputSpeed);
      }
      break;
    case RIGHT_MOTOR:
      if (forward) {
        analogWrite(RIGHT_MOTOR_1_PIN, outputSpeed);
        analogWrite(RIGHT_MOTOR_2_PIN, 0);
      } else {
        analogWrite(RIGHT_MOTOR_1_PIN, 0);
        analogWrite(RIGHT_MOTOR_2_PIN, outputSpeed);
      }
      break;
    case VIBE_MOTOR:
      analogWrite(VIBE_MOTOR_PIN, outputSpeed);
      break;
    default:
      return;
  }
}

void printOutRCOutput() {
  Serial.print(IBus.readChannel(RIGHT_HORIZ), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(RIGHT_VERT), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(LEFT_VERT), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(LEFT_HORIZ), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(AUXA), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(AUXB), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(AUXC), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(AUXD), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(VARA), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(VARB), DEC);
  Serial.print(" ");
  Serial.print(IBus.readChannel(FAILSAFE_UNIQUE), DEC);
  Serial.println(" ");
}

void printOutAccelOutput(Vector accelInput) {
  Serial.print(accelInput.XAxis);
  Serial.print(" ");
  Serial.print(accelInput.YAxis);
  Serial.print(" ");
  Serial.print(accelInput.ZAxis);
  Serial.println(" ");  
}

void printOutRPMOutput() {
  Serial.print(wheelRPS,DEC);
  Serial.print(" ");
  Serial.print(botRPS,DEC);
  Serial.print(" ");
  Serial.println(currentBotAngle,DEC);
}

//  Serial.print("chan 9 ");
//   Serial.print(IBus.readChannel(9));
//   Serial.print("chan 10 ");
//   Serial.print(IBus.readChannel(10));
//   Serial.print("chan 11 ");
//   Serial.print(IBus.readChannel(11));
//   Serial.print("chan 12 ");
//   Serial.print(IBus.readChannel(12));
//   Serial.print("chan 13 ");
//   Serial.print(IBus.readChannel(13));
//   Serial.print("chan 14 ");
//   Serial.println(IBus.readChannel(14));
//   Serial.print("chan 15 ");
//   Serial.println(IBus.readChannel(15));
  
//  if (IBus.readChannel(WRITEEEPROM_SWITCH) > JOYSTICK_CENTER) {
//    dataloggingCounter++;
//    if (dataloggingCounter > 200) {
//      dataloggingCounter = 0;
//      float accelToLog = rawAccel.YAxis * 0.0005;
//      SpeedDataObject dataOb = {
//        botRPS,
//        accelToLog,
//        rawAccel.XAxis * 0.0005,
//        rawAccel.ZAxis * 0.0005,
//      };
//      EEPROM.put(eeAddress, dataOb);
////      Serial.print("Wrote add ");
////      Serial.print(eeAddress);
////      Serial.print(" Acc ");
////      Serial.println(accelToLog);
//      eeAddress += sizeof(SpeedDataObject);
//      if (eeAddress > EEPROM.length()) {
//        eeAddress = 0;
//      }
//    }
//  }