/*
 * The MIT License
 *
 * Copyright (c) 2008-2009 MIT 6.270 Robotics Competition
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 */


// Include headers from OS
#include <joyos.h>
#include <lib/pid.h>
#include <util/delay.h>
#include <kern/global.h>
#include <happylib.h>
#include <stdlib.h>
// Include threading
// #include <pthread.h>

//gyro
#define GYRO_PORT 11
#define LSB_US_PER_DEG 1400000.0

//servo for scissor arms
#define ArmServo 0

//sharp distance sensors
#define FrontDistance 20
#define RearDistance 21
#define LeftDistance 22
#define RightDistance 23
#define SHARP_M 22840
#define SHARP_C 26

//front bump sensors/switches
#define LeftFrontSwitch 5
#define RightFrontSwitch 6
#define LeftBackSwitch 0
#define RightBackSwitch 1

//motors
#define RightMotor 0
#define LeftMotor 1
#define FlagboxMotor 3

//shaft encoders
#define RightEncoder 27
#define LeftEncoder 26

//ForwardPID (go straight) information
#define TargetVelocity 100
#define MaxVelocity 250
uint8_t kp = 7;
uint8_t ki = 7;
uint8_t kd = 0;
uint16_t PIDDelay = 50; //time step for PID controller
//when to use ForwardPID
uint8_t ShouldUseForwardPID = 0; //1 if yes, 0 if no 

//team number (for joyos - DO NOT DELETE)
uint8_t team_number[2] = {13,0};

//global panic/position/steps variable
uint8_t chapter = 0;
uint8_t enabled_chapter = 1;
uint8_t my_panic = 0;
uint8_t enabled_panic = 1;

uint8_t has_seen_flagbox = 0;
uint32_t time_after_flagbox = 500;
uint32_t last_time_flagbox_seen = 0;

float EncoderDifferential(); // Defined to be Right - Left

void ForwardApplyMV(float);
void LeftApplyMV(float);
void RightApplyMV(float);

float BoundedVelocity(float);
void SetRightVelocity(float);
void SetLeftVelocity(float);
void ResetEncoders();
void CoastMotors();
void CoastMotor(uint8_t);

void ResetEncoders()
{
	encoder_reset(RightEncoder);
	encoder_reset(LeftEncoder);
}

void ResetArms(void) 
{
  for(uint16_t i = 120; i <= 350; i = (i + 10)){
    servo_set_pos(ArmServo, i);
    pause(20);
  }
}

void ActivateFlagbox(void)
{
  motor_set_vel(FlagboxMotor, 30);
  pause(5000);
  motor_set_vel(FlagboxMotor, 0);
}


float EncoderDifferential()
{
	return (float)encoder_read(RightEncoder)-(float)encoder_read(LeftEncoder);
}

float BoundedVelocity(float ProposedVelocity)
{
	if (ProposedVelocity < 0)
	{
		return 0;
	}
	else if (ProposedVelocity > MaxVelocity)
	{
		return MaxVelocity;
	}
	else
	{
		return ProposedVelocity;
	}
}

void SetRightVelocity(float vel)
{
	motor_set_vel(RightMotor, (int16_t)vel);
}

void SetLeftVelocity(float vel)
{
	motor_set_vel(LeftMotor, (int16_t)vel);
}

void ForwardApplyMV(float MV)
{
	SetRightVelocity(BoundedVelocity(TargetVelocity+MV/2));
	SetLeftVelocity(BoundedVelocity(TargetVelocity-MV/2));	
}

void BackwardApplyMV(float MV)
{
  SetRightVelocity(-(BoundedVelocity(TargetVelocity+MV/2)));
  SetLeftVelocity(-(BoundedVelocity(TargetVelocity-MV/2)));
}

void MotorCoast(uint8_t Motor)
{
	motor_set_vel(Motor, 0);
}

void CoastMotors()
{
	MotorCoast(RightMotor);
	MotorCoast(LeftMotor);
	pause(20);
}

void StopDriving(void)
{
  motor_brake (RightMotor);
  motor_brake (LeftMotor);
  pause(20);
}

/* Turn on a dime with specified speed, postive is clockwise. */
void
SetTurnVelocity(int8_t speed)
{
  motor_set_vel(RightMotor, speed);
  motor_set_vel(LeftMotor, (0-speed));
}

// left is positive, right is negative
void
Turn(int16_t degrees) {
  
  int8_t speed = 100;
  enabled_panic = 0;
  if(degrees < 0) {
    /* Right turn, invert speed. */
    speed = -speed;
  }
  int8_t offset = 5;
  int8_t gyro_thinks = (int)gyro_get_degrees();
  
  // Run for millisecond_time
  uint32_t StopTime = get_time()+ (abs(degrees) * 6);
  
  while((abs(gyro_thinks) < (abs(degrees) - abs(offset))) && (get_time() < StopTime)){
    if(abs(abs(gyro_thinks) - (abs(degrees) - abs(offset))) < 15.0) {
      speed = 25;
      if(degrees < 0) {
        speed = -speed;
      }
    }
    SetTurnVelocity(speed);
    gyro_thinks = (int8_t)gyro_get_degrees();
    pause(5);
  }
  
  enabled_panic = 1;
  StopDriving();
}

void
KickMyBalls(void) 
{
  for(uint16_t i = 350; i >= 120; i = (i - 10)){
    servo_set_pos(ArmServo, i);
    pause(5);
  }
  delay_busy_ms(1000);
  ResetArms();
}

// will use this in Drives such that this is for until distance less than reading
uint8_t
DistanceComparison(uint8_t distance1, uint8_t port1, uint8_t distance2, uint8_t port2) {
	if (distance2 == 0) {
	    return (!(distance1 < irdist_read(port1)));
    }
	else if (distance1 == 0) {
	    return (!(distance2 > irdist_read(port1)));
    }
    else {
		if (!((distance1 < irdist_read(port1)) && (distance2 > irdist_read(port1)))){
			return 1;
		}
		else {
			return 0;
	    }
	}
}

// will use this in Drives such that this is for until both pressed
uint8_t
BothBackDigitalSwitches(uint8_t port1, uint8_t port2, uint8_t blah1, uint8_t blah2) {
	return ((!(digital_read(port1))) | (!(digital_read(port2))));
}

// will use this in Drives such that this is for until both pressed
uint8_t
IfPassedFlagbox(uint8_t port, uint8_t blah, uint8_t blah1, uint8_t blah2) {
	if ((has_seen_flagbox == 0) && (irdist_read(port) > 30)) {
		return 1;
	}
	else if ((has_seen_flagbox == 0) && (irdist_read(port) < 30)) {
		has_seen_flagbox = 1;
		return 1;
	}
	else if ((has_seen_flagbox == 1) && (irdist_read(port) < 30)) {
		return 1;
	}
	else if ((has_seen_flagbox == 1) && (irdist_read(port) > 30)) {
		has_seen_flagbox = 2;
		last_time_flagbox_seen = get_time();
		return 1;
	}
	else if ((has_seen_flagbox == 2) && ((get_time() - last_time_flagbox_seen) < time_after_flagbox)){
		return 1;
	}
	else if ((has_seen_flagbox == 2) && ((get_time() - last_time_flagbox_seen) >= time_after_flagbox)){
		return 0;
	}
	else {
		return 1;
	}
}

uint8_t //return 1 if (sensorA-bound) <= sensorB <= (sensorA+bound)
DistanceWithinBounds(uint8_t bound, uint8_t port1, uint8_t port2, uint8_t blah) {
  if (((irdist_read(port1) - bound) <= irdist_read(port2)) && (irdist_read(port2)<= (irdist_read(port1) + bound))){
    return 0;
  }
  else{
    return 1;
  }
}

// will use this in Drives such that this is for until both pressed
uint8_t
BothFrontDigitalSwitches(uint8_t port1, uint8_t port2, uint8_t blah1, uint8_t blah2) {
	return (digital_read(port1) | digital_read(port2));
}

// for specified time, do it--NOTE: SECOND INPUT DOES NOTHING BUT IS NEEDED FOR THE FUNCTION POINTER
uint8_t
GoForTime(uint8_t deci_second, uint8_t blah, uint8_t blah2, uint8_t blah3) {
  uint32_t StopTime = get_time()+ deci_second*100;
  return (get_time() < StopTime);
}

void
DriveForward(uint8_t (*enabled_test_func)(uint8_t, uint8_t, uint8_t, uint8_t), uint16_t millisecond_time, uint8_t parameter1, uint8_t parameter2, uint8_t parameter3, uint8_t parameter4)
{
	// Run for millisecond_time
	uint32_t StopTime = get_time()+ millisecond_time;
  uint8_t IsEnabled = 1;
  
	ResetEncoders();
	
	struct pid_controller ForwardPID;
  
	init_pid(	&ForwardPID,
            (float)kp,
            (float)ki,
            (float)kd,
            &EncoderDifferential,
            &ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = IsEnabled;
  
	
	while (get_time() < StopTime)
	{
    IsEnabled = (uint8_t)(*enabled_test_func)(parameter1, parameter2, parameter3, parameter4);
		update_pid(&ForwardPID);
	  
		pause(PIDDelay);
	}
  
	CoastMotors();
}

void
DriveBackward(uint8_t (*enabled_test_func)(uint8_t, uint8_t, uint8_t, uint8_t), uint16_t millisecond_time, uint8_t parameter1, uint8_t parameter2, uint8_t parameter3, uint8_t parameter4)
{
  // Run for millisecond_time
  uint32_t StopTime = get_time() + millisecond_time;
  uint8_t IsEnabled = 1;
  
  ResetEncoders();
  
  struct pid_controller BackwardPID;
  
  init_pid( &BackwardPID,
            (float)kp,
            (float)ki,
            (float)kd,
            &EncoderDifferential,
            &BackwardApplyMV);
  BackwardPID.goal = 0.0;
  BackwardPID.enabled = IsEnabled;
  
  
  while (get_time() < StopTime)
  {
    IsEnabled = (uint8_t)(*enabled_test_func)(parameter1, parameter2, parameter3, parameter4);
    update_pid(&BackwardPID);
    
    pause(PIDDelay);
  }
  
  CoastMotors();
}

void
OrientSelf(void)
{
  if (irdist_read(FrontDistance) < 30 && irdist_read(LeftDistance) < 30){
    gyro_set_degrees(0);
    Turn(-90);
    pause(50);
  }
  else if (irdist_read(FrontDistance) < 30 && irdist_read(RightDistance) < 30){
    gyro_set_degrees(0);
    Turn(180);
    pause(50);
  }
  else if (irdist_read(RightDistance) < 40 && irdist_read(RearDistance) < 30){
    //left turn once
    gyro_set_degrees(0);
    Turn(90);
    pause(50);
  }
  else {
    pause(50);
  }
}

// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
  printf("\nHello! My Name  Is Oliver.");
   
  //Arm Resetting
  ResetArms();
  pause (500);
  
  printf("\nI'm a total     cutie.");
  //Calibrating distance sensors
  irdist_set_calibration (SHARP_M, SHARP_C);
  pause(150);
  
  //Calibrating gyros
  printf_P (PSTR("\nPlace robot,    press go."));
	go_click ();
	printf_P (PSTR("\nStabilizing..."));
	pause (500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_US_PER_DEG, 5000L);
  return 0;
}


int
umain(void)
{
  while(1) {
    while (my_panic) {
	  printf("\nPANIC! Chapter = %d", chapter);
      DriveBackward(&GoForTime, 510, 5, 1, 3, 3);
      enabled_chapter = 1;
      my_panic = 0;
    }
    // note: the drive commands only take in 3-5 variables--the other 1-3 were reserved for possibly more complicated "should be enabled" functions/have the extras so that the same function pointer can be used. we made the function pointer for the drive functions take in 4 variables all the time in case we ever needed to actually use four.
    while ((((digital_read(LeftFrontSwitch))&(digital_read(RightFrontSwitch))) | !(enabled_panic)) & !(my_panic)) {
      if (chapter == 0) {
        printf("\nChapter = %d", chapter);
        OrientSelf();
        chapter++;
      }
      else if (chapter == 1) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 1000, 40, RearDistance, 50, FrontDistance);
        gyro_set_degrees(0);
        chapter++;
    }
      else if (chapter == 2) {
        printf("\nChapter = %d", chapter);
        Turn(-90);
        chapter++;
      }
      else if (chapter == 3) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&BothBackDigitalSwitches, 1200, LeftBackSwitch, RightBackSwitch, 3, 3);
        chapter++;
      }
      else if (chapter == 4) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 5250, 38, FrontDistance, 52, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 5) {
        printf("\nChapter = %d", chapter);
        Turn(-45);
        chapter++;
      }
      else if (chapter == 6) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 1500, 0, FrontDistance, 22, RearDistance);
        chapter++;
      }
      else if (chapter == 7) {
      printf("\nChapter = %d", chapter);
      KickMyBalls();
      gyro_set_degrees(0);
      chapter++;
      }
      else if (chapter == 8) {
        printf("\nChapter = %d", chapter);
        Turn(-135);
        chapter++;
      }
      else if (chapter == 9) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&BothBackDigitalSwitches, 1200, LeftBackSwitch, RightBackSwitch, 3, 3);
        chapter++;
      }
      else if (chapter == 10) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 1500, 40, RearDistance, 50, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 11) {
        printf("\nChapter = %d", chapter);
        Turn(-90);
        chapter++;
      }
      else if (chapter == 12) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&BothBackDigitalSwitches, 1200, LeftBackSwitch, RightBackSwitch, 3, 3);
        chapter++;
      }    
      else if (chapter == 13) {
        printf("\nChapter = %d", chapter);
         DriveForward(&DistanceComparison, 9500, 38, FrontDistance, 52, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 14) {
        printf("\nChapter = %d", chapter);
        Turn(90);
        chapter++;
      }
      else if (chapter == 15) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 5250, 38, FrontDistance, 52, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 16) {
        printf("\nChapter = %d", chapter);
        Turn(90);
        chapter++;
      }
      else if (chapter == 17) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 9500, 38, FrontDistance, 52, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 18) {
        printf("\nChapter = %d", chapter);
        Turn(90);
        chapter++;
      }
      else if (chapter == 19) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 5250, 38, FrontDistance, 52, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 20) {
        printf("\nChapter = %d", chapter);
        Turn(-45);
        chapter++;
      }
      else if (chapter == 21) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 1500, 0, FrontDistance, 22, RearDistance);
        chapter++;
      }
      else if (chapter == 22) {
        printf("\nChapter = %d", chapter);
        KickMyBalls();
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 23) {
        printf("\nChapter = %d", chapter);
        Turn(-135);
        chapter++;
      }
      else if (chapter == 24) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&BothBackDigitalSwitches, 1200, LeftBackSwitch, RightBackSwitch, 3, 3);
        chapter++;
      }
      else if (chapter == 25) {
        printf("\nChapter = %d", chapter);
        DriveForward(&DistanceComparison, 1500, 40, RearDistance, 50, RearDistance);
        gyro_set_degrees(0);
        chapter++;
      }
      else if (chapter == 26) {
        printf("\nChapter = %d", chapter);
        Turn(-90);
        chapter++;
      }
      else if (chapter == 27) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&BothBackDigitalSwitches, 1200, LeftBackSwitch, RightBackSwitch, 3, 3);
        chapter++;
      }
      else if (chapter == 28) {
        printf("\nChapter = %d", chapter);
        DriveBackward(&IfPassedFlagbox, 9500, LeftDistance, 3, 3, 3);
        chapter++;
      }
      else if (chapter == 29) {
        printf("\nChapter = %d", chapter);
        Turn(-90);
        chapter++;
      }
      else if (chapter == 30) {
        DriveBackward(&DistanceWithinBounds, 4200, 25, FrontDistance, RearDistance, 1); 
        chapter++;
      }
      else if (chapter == 31) {
        ActivateFlagbox();
      }
    }
    my_panic = 1;
    enabled_chapter = 0;
  }
  
	return 0;
}