// -------------------------------------------
// Author: Andrew Combs
// Title: KIA Optima Speed Control
// Created using the open source Sparkfun CANBUS reader
// and AccelStepper libraries. 


// CAN Bus Sniffing Arduino MEGA 2560
#include <Timer.h>
#include <Canbus.h>
#include <defaults.h>
#include <global.h> // modified for Arduino MEGA
#include <mcp2515.h>
#include <mcp2515_defs.h>
#include <AccelStepper.h>
#include <Servo.h>

#define leftAccPin 27
#define rightAccPin 28


const double maxSpeed = 100.0; //steps per second
const double hi = 1660;
const double low = 1075;
const double midHi = 1385;
const double midLow = 1355;
const double maxBrakeStep = 235.0;
const double maxThrottleDegrees = 80; // default 180
const double minThrottleDegrees = 50; // 160

double brakeCalNum1;
double brakeCalNum2;
double brakeCalNum3;
double brakeCalNum4;
double steerLeftCalNum1;
double steerLeftCalNum2;
double steerRightCalNum1;
double steerRightCalNum2;
double throttleCalNum1;
double throttleCalNum2;
double throttleCalNum3;

bool neutral = false;

// initialize throttle and brake position
volatile double chThrottle = 80;// default 165
volatile double chBraking = 0.0;

int thrPos = 80;// default 165
int brPos = 0;

//const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution of your stepper
// initialize stepper library on pins 22,23,24,25
//Stepper brakeStepper(stepsPerRevolution, 22, 23, 24, 25);
AccelStepper brakeStepper(AccelStepper::FULL4WIRE, 25, 24, 23, 22);

// Create servo motor object
Servo accServo1; // twelve servo objects can be created
Servo accServo2;

// desired speed for vehicle
int desiredSpeed = 0;

//current vehicle speed
int currentSpeed = 0;


// varaible for timing
Timer t;
int speedEvent;

void setup() {
  // timer setup to incrase desired speed in steps for graphing desired vs current speed
  int speedEvent = t.every(2000, increaseDesiredSpeed);
  int stopEvent = t.after(20000, decreaseDesiredSpeed);



  //Calibrations
  calibrateBraking();
  calibrateThrottle();

  // Setup stepper control
  // set the speed to 60 rpm
  //brakeStepper.setSpeed(10);
  brakeStepper.setMaxSpeed(maxSpeed);
  brakeStepper.setAcceleration(500);
  brakeStepper.setCurrentPosition(0);

  // Setup servo
  //.attach(pin,min,max); min and max figures are in microseconds
  accServo2.attach(leftAccPin); // servo attached to pin 27
  accServo1.attach(rightAccPin); // servo attaches to pin 28


  // Setup CAN READER
  Serial.begin(9600);
  //Initialize CAN controller
  if (Canbus.init(CANSPEED_500))
    Serial.println("CAN Init ok");
  else
    Serial.println("Can't Init CAN");
  delay(1000);
}

void loop() {
  //Get the desired speed from serial input if serial is available
  printSpeed();
  t.update();

  // create can message object
  tCAN message;

  // read the message
  if (mcp2515_check_message())
  {
    if (mcp2515_get_message(&message))
    {
      // can ID 440 is the message containing the vehicle speed
      if (message.id == 0x440)
      {
        // byte 2 of the message is the byte containing the vehicle speed
        // multiplby by 0.62 to convert from kph to mph
        currentSpeed = ceil(message.data[2] * 0.62);
      }
    }
  }
  // if we want to increase speed and the brake is not on
  if (currentSpeed <= desiredSpeed)
  {
    // move brake back to home position to turn of brake
    chBraking = 0;
    brakeControl(brakingSigConv(chBraking));
    brPos = 0;

    // ramp accelerator to increase speed
    while (currentSpeed <= desiredSpeed)
    {
      
      if (Serial.available())
      {
        //desiredSpeed = Serial.parseInt();
      }
      if (mcp2515_check_message())
      {
        if (mcp2515_get_message(&message))
        {
          if (message.id == 0x440)
          {
            currentSpeed = ceil(message.data[2] * 0.62);
          }
        }
      }

      accServo1.write(thrPos);
      accServo2.write(180 - thrPos);

      if (desiredSpeed < 5)
      {
        thrPos = 72;
      }
      else if (desiredSpeed < 8 )
      {
        thrPos = 67;
      }
      else if (desiredSpeed < 12)
      {
        thrPos = 62;
      }
      else
      {
        if(thrPos > 50)
        thrPos--;
        else 
        thrPos = 50;
      }
      printSpeed();
      t.update();
    }
  }

  // If we want to reduce speed an the brake is not on
  else if (currentSpeed > desiredSpeed)
  {
    accServo1.write(maxThrottleDegrees);
    accServo2.write(180 - maxThrottleDegrees);
    // ramp brake to decrease speed
    while (currentSpeed > desiredSpeed)
    {
      if (mcp2515_check_message())
      {
        if (mcp2515_get_message(&message))
        {
          if (message.id == 0x440)
          {
            currentSpeed = ceil(message.data[2] * 0.62);
          }
        }
      }
      brakeControl(chBraking);
      chBraking = brakingSigConv(brPos);
      brPos++;
      printSpeed();
      t.update();
    }
  }
}


void calibrateBraking(void)
{
  brakeCalNum1 = divideWithZeroCheck(maxBrakeStep, (hi - low));
  brakeCalNum2 = low * brakeCalNum1;
  brakeCalNum3 = ceil(maxBrakeStep / 2);
  brakeCalNum4 = divideWithZeroCheck(maxBrakeStep, brakeCalNum3);
}

//Range: 0 to 235 steps
double brakingSigConv(double s2)
{
  s2 = (brakeCalNum1 * s2) - brakeCalNum2;
  if (s2 <= brakeCalNum3)
  {
    s2 = (-brakeCalNum4 * s2) + maxBrakeStep; //turns 118 steps to 235
    neutral = false;
  }
  else
  {
    neutral = true; //will program this in later.
    s2 = 0;
  }
  if (s2 > maxBrakeStep)
  {
    s2 = maxBrakeStep;
  }
  return (s2);
}

void brakeControl(double sig1)
{
  brakeStepper.moveTo(sig1);
  if (brakeStepper.currentPosition() != sig1)
  {
    brakeStepper.setSpeed(maxSpeed);
    brakeStepper.runSpeedToPosition();
  }
}

void calibrateThrottle(void)
{
  throttleCalNum1 = divideWithZeroCheck((maxThrottleDegrees - minThrottleDegrees), (hi - low));
  throttleCalNum2 = low * throttleCalNum1;
  throttleCalNum3 = minThrottleDegrees - throttleCalNum2;
}

double throttleSigConv(double s3) //Range: 170 to 180 degrees but can use floats!
{
  s3 = (throttleCalNum1 * s3) + throttleCalNum3;
  s3 = round(s3 * 2) / 2.0d;
  return (s3);
}

void throttleControl(double sig2)
{
  Serial.println(sig2);
  accServo1.write(sig2);
  accServo2.write(180 - sig2);
}

double divideWithZeroCheck(double a, double b)
{
  double c;
  if (b == 0.0d)
  {
    Serial2.println("ERROR! ZERO IS BEING DIVIDED");
    c = 0.0d;
  }
  else
  {
    c = a / b;
  }
  return c;
}

void printSpeed(void)
{
  //desiredSpeed = Serial.parseInt();
  //                    Serial.print("Desired speed: ");
  //                    Serial.print(desiredSpeed, DEC);
  //                    Serial.print("Current speed: ");
  //                    Serial.print(currentSpeed, DEC);
  //                    Serial.println(" ");
  Serial.print(desiredSpeed);
  Serial.print(", ");
  Serial.println(currentSpeed);
  delay(10);
}

void increaseDesiredSpeed(void)
{
  desiredSpeed += 1;
}

void decreaseDesiredSpeed(void)
{
  t.stop(speedEvent);
  desiredSpeed = 0;
}