#include <SDPArduino.h>
#include <Wire.h>

/*
 * Master board sample code to be used in conjuction with the rotary encoder
 * slave board and sample code.
 * This sketch will keep track of the rotary encoder positions relative to
 * the origin. The origin is set to the position held when the master board
 * is powered.
 *
 * Rotary encoder positions are printed to serial every 200ms where the
 * first result is that of the encoder attached to the port at 11 o'clock
 * on the slave board (with the I2C ports at at 12 o'clock). The following
 * results are in counter-clockwise sequence.
 *
 * Author: Chris Seaton, SDP Group 7 2015
 */


#define ROTARY_SLAVE_ADDRESS 5
#define ROTARY_COUNT 6
#define PRINT_DELAY 50

#define MOTOR_1 1
#define MOTOR_2 0
#define MOTOR_3 3
#define MOTOR_4 4

// Initial motor position is 0.
int positions[ROTARY_COUNT] = {0};
boolean flags[ROTARY_COUNT]={0};

float velocities[ROTARY_COUNT]={0};
int times[ROTARY_COUNT]={0};

int previous[ROTARY_COUNT] = {0};
int current[ROTARY_COUNT] = {0};

int sensor_fwd_reading = 0;

float PID_output = 0;

int ERROR_calib = 3500;

double Kp = 0.5;
double Ki=0.5;
double Kd = 0.05;
double area=0;

double line_new_error = 0;
double line_last_error = 0;

// GLOBAL VALUES for motors
double motor2_pow = 0;
double vel_delta_new = 0;
double vel_delta_old = 0;

double vel_desired = 7;

void setup() {
  Serial.begin(9600);  // Serial at given baudrate
  Serial.print("Seial here");
  Wire.begin();  // Master of the I2C bus

  // motor_move(0, 100);

  SDPsetup();
  helloWorld();

}

void update_motor2_PID() {
  // Update motor speeds
  update_velocities_pieris();
  double vel_current = velocities[MOTOR_2];
  // calcualte new power
  vel_delta_new = vel_desired - vel_current;
  area+=vel_delta_new;
  double vel_adjustment = Kp * vel_delta_new + Kd * (vel_delta_new - vel_delta_old) + Ki * (area);
  vel_delta_old = vel_delta_new;
  motor2_pow = vel_adjustment;

  // DEBUG

  Serial.print("vel_current = ");
  Serial.println(vel_current);

  Serial.print("vel_desired - vel_current = ");
  Serial.println(vel_delta_new);

  Serial.print("vel_adjustment = ");
  Serial.println(vel_adjustment);

  Serial.println();

  // make sure you don't go over 100
  if (motor2_pow > 100) {
    motor2_pow = 100;
  }
  else if (motor2_pow < -100) {
    motor2_pow = -100;
  }
  // motors_speed_fwd = abs(motors_speed_fwd);
}

void motor_move(int id, double speed) {
  Serial.println("INSIDE motor_move");
  if (speed >= 0) {
    motorForward(id, speed);
    Serial.print("FWD: id ");
    Serial.println(id);
    Serial.print("FWD: speed ");
    Serial.println(speed);
  }
  else {
    motorBackward(id, abs(speed));
    Serial.print("BKW: id ");
    Serial.println(id);
    Serial.print("BKW: speed ");
    Serial.println(speed);
  }
}


/**
 * Get the angle positions in a small delay and calculates the --
 * --velocity of a wheel spinnig
 * Updates globals for velocity of all wheels
 */
void update_velocities_pieris(){
  int delay_read=40;
  for(int i=0;i<ROTARY_COUNT;i++){
   velocities[i]=0.0f;
  }
    Wire.requestFrom(ROTARY_SLAVE_ADDRESS, ROTARY_COUNT);
    for (int i = 0; i < ROTARY_COUNT; i++) {
      positions[i] += (float) Wire.read();  // Must cast to signed 8-bit type
      previous[i]=positions[i];
    }
    delay(delay_read);
    Wire.requestFrom(ROTARY_SLAVE_ADDRESS, ROTARY_COUNT);
    for (int i = 0; i < ROTARY_COUNT; i++) {
      positions[i] += (float) Wire.read();  // Must cast to signed 8-bit type
      current[i]=positions[i];
    }
    for (int i = 0; i < ROTARY_COUNT; i++) {
      velocities[i]=((current[i]-previous[i])*(PI/180))/((float)delay_read/1000.0f);
    }
}

void loop() {


  // sensor_fwd_reading += random(-500, 500);

  update_motor2_PID();
  // double  vel_to_pow = map(motor2_pow,-9.16, 9.16, -100, 100);
  motor_move(0, motor2_pow);


  Serial.print("motor2_pow = ");
  Serial.println(motor2_pow);

  // Serial.print("vel_to_pow = ");
  // Serial.println(vel_to_pow);

  delay(10);
  //update_velocities_pieris();

  //printMotorVelocities();

  Serial.println();

}



void updateMotorPositions() {
  // Request motor position deltas from rotary slave board
  Wire.requestFrom(ROTARY_SLAVE_ADDRESS, ROTARY_COUNT);

  // Update the recorded motor positions
  for (int i = 0; i < ROTARY_COUNT; i++) {
    positions[i] = (int8_t) Wire.read();  // Must cast to signed 8-bit type
  }
}


void printMotorPositions() {
  Serial.print("Motor positions: ");
  for (int i = 0; i < ROTARY_COUNT; i++) {
    Serial.print(positions[i]);
    Serial.print(' ');
  }
  Serial.println();
  delay(50);  // Delay to avoid flooding serial out
}

void printMotorVelocities() {
  Serial.print("Motor velocities: ");
  for (int i = 0; i < ROTARY_COUNT; i++) {
    Serial.print(velocities[i]);
    Serial.print(' ');
  }
  Serial.println();
  delay(PRINT_DELAY);  // Delay to avoid flooding serial out
}