#include <joyos.h>
#include <lib/pid.h>
#include "driving.h"
#include "sensing.h"

int16_t line_move;

int8_t line_follow_counter;
int8_t line_state_time;
float ki;
float kp;
float kd;

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

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 SetRightVelocityforward(float vel)
{
	motor_set_vel(RightMotor, (int16_t)vel);
}

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

void SetRightVelocitybackward(float vel)
{
	motor_set_vel(RightMotor, (int16_t) -1*vel);
}

void SetLeftVelocitybackward(float vel)
{
	motor_set_vel(LeftMotor, (int16_t) -1*vel);
}
void ForwardApplyMV(float MV)
{
	SetRightVelocityforward(BoundedVelocity(TargetVelocityStraight+MV/2));
	SetLeftVelocityforward(BoundedVelocity(TargetVelocityStraight-MV/2));	
}

void BackwardApplyMV(float MV)
{
	SetRightVelocitybackward(BoundedVelocity(TargetVelocityStraight+MV/2));
	SetLeftVelocitybackward(BoundedVelocity(TargetVelocityStraight-MV/2));	
}

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

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

void drive_straight(int16_t Direction, uint32_t Timeout, int8_t Function, int16_t n){       // Drive Straight for DriveTime seconds.Returns 1 when finished.

	pause(500);
	ResetEncoders();
	int16_t kp = 2;
	int16_t ki = 3;
	float kd = 0;
	uint16_t PIDDelay = 20;
	int16_t newn = n;
	int16_t counter = 0;

	uint32_t TimeoutDriveStraight = get_time() + Timeout;            

	if(Direction == FORWARD){

		struct pid_controller ForwardPID;

		init_pid(	&ForwardPID,
				(float)kp,
				(float)ki,
				(float)kd,
				&EncoderDifferential,
				&ForwardApplyMV);
		ForwardPID.goal = 0.0;
		ForwardPID.enabled = 1;
		
		while (true)
		{
			if((Average_Encoder_Read() > newn) || (get_time() > TimeoutDriveStraight) /*|| (encoder_read(LeftEncoder) > 91*Distance)*/){
			printf("\nLeft: %2d Right: %2d",motor_get_current(1),motor_get_current(0));
				break;
			}
			update_pid(&ForwardPID);
			counter++;
			printf("\nUpdating PID");
			pause(PIDDelay);
			if(Function == ACROSS && counter >= 10 ){
				counter = 0;
				if(find_distance_measurement(Front_Distance_Sensor) < 23){
					raise_arms();
				}
			}
			if(Function == FLAGACROSS && counter >= 10){
				counter = 0;
				if(detect_line() && find_distance_measurement(Left_Distance_Sensor) < 50){
					break;
				}
			}
		}
	}
	else{
	
		struct pid_controller BackwardPID;

		init_pid(	&BackwardPID,
				(float)kp,
				(float)ki,
				(float)kd,
				&EncoderDifferential,
				&BackwardApplyMV);
		BackwardPID.goal = 0.0;
		BackwardPID.enabled = 1;
		
		while (true)
		{
			if( get_time() > TimeoutDriveStraight /*|| (encoder_read(LeftEncoder) > 91*Distance)*/){
				break;
			}
			update_pid(&BackwardPID);
			printf("\nUpdating PID Left: %2d Right: %2d", motor_get_current(1),motor_get_current(0));
			pause(PIDDelay);
		}
	}
	CoastMotors(); 
}