//#include <stdio.h>				// for printf
//#include <avr/pgmspace.h>		// for PSTR()
#include <math.h>
#include <stdlib.h>
#include "avrx.h"
#include "hardware.h"
#include "generalio.h"
#include "serialio.h"
#include "definitions.h"			// to get motor command defines.
#include "MotorTask.h"

//#define pid_velocity_dump 1
//#define pid_plot2 2


AVRX_GCC_TASKDEF(MotorTask, 40, MOTOR_PRIORITY);

/*
    Globals/Statics etc.
*/

MotorInfo Left, Right;

volatile signed char RightEncoder=0, LeftEncoder=0;		// Wheel encoder counts

volatile float   X=0,
		Y=0,
		Theta=0;
volatile int		iX=0,
		iY=0,
		iTheta=0;

volatile unsigned char OdometryResetPending;
		
// Last encoder status.
static char encoders_old;

static TimerControlBlock MotorTimer;
Mutex MotorLoop;
extern char velocity_print;


/* -------------------------------------------------------------
    This is the actual task that takes does the PID at regular
    intervals.
   ------------------------------------------------------------- */
NAKEDFUNC(MotorTask)
{
  	signed char LeftEnc, RightEnc;
    float DeltaTheta,
    		dist;
  
    // Init PID variables
    MotorInit();

    while(1)
    {
        AvrXStartTimer(&MotorTimer, PID_LOOP_DELAY);
        AvrXSetSemaphore(&MotorLoop);   		// Synchronize other motion tasks
        
        BeginCritical();
        LeftEnc = LeftEncoder;
        LeftEncoder = 0;
        RightEnc = RightEncoder;
        RightEncoder = 0;
        EndCritical();

		// Now update totals.
        AddToLong(&Left.Encoder, LeftEnc);
        AddToLong(&Right.Encoder, RightEnc);

#if pid_plot
	PutCRLF();
	PutDecSignedWord(Right.VelocitySetpoint>>8);
	PutChar(',');
	PutDecSignedByte(RightEnc);
#endif
#if pid_plot2
	if (plot_count < MAX_SAMPLES)
	{
		plot_array[plot_count].velocity = RightEnc;
	}
#endif


#if pid_velocity_dump
if (velocity_print)        
{
        PutCRLF();
        PutDecSignedByte(LeftEnc);
        PutChar('=');
        PutDecSignedByte(RightEnc);
}
#endif
 
 		//
 		// Update odometry readings      
        //
        
        // Was an odometry reset requested?
        if (OdometryResetPending == TRUE)
        {
        	OdometryResetPending = FALSE;	// Do only once!
        	
        	X = 0;
        	Y = 0;
        	Theta = 0;
        }
        
        DeltaTheta = (LeftEnc-RightEnc) * (DEG_PER_TICK/2.0);
		dist = (LeftEnc+RightEnc) * (DIST_PER_TICK/2.0);

		Theta += DeltaTheta;
        X += dist * cos(Theta);
        Y -= dist * sin(Theta);
        Theta += DeltaTheta;

        if (Theta >= 2.0*M_PI)
            Theta -= 2.0*M_PI;       // Keep theta between 0-360
        else if (Theta < 0.0)
            Theta += 2.0*M_PI;

        // Keep integer versions around for user interface
        BeginCritical();		// Just in case..
        iTheta = Theta * (float)(360 / (2*M_PI));		// Convert to degrees
		iX = X;
        iY = Y;
		EndCritical();
		
        DoMotion(&Left);
        DoPid(&Left);
        DoMotion(&Right);
        DoPid(&Right);
        
#if pid_plot
		PutChar(',');
		PutDecByte(Right.pwm_out);
		PutChar(13);
#endif

#if pid_plot2
		if (plot_count < MAX_SAMPLES)
		{
			plot_array[plot_count].power = Right.pwm_out;
			plot_count ++;
		}
#endif
        
        // Apply calculated pwm outputs simultaneously!
       	PWM_R=Right.pwm_out;
       	PWM_L=Left.pwm_out;
       	       	
#if pid_debug5
		PutCRLF();
#endif

        AvrXWaitTimer(&MotorTimer);
    }
}

void MotorInit(void)
{
    // Initialize Motion Control data
    
    //Kp = K_P;	//eeprom_rb((unsigned)&Kp);
    //Kd = K_D;	//eeprom_rb((unsigned)&Kd);
    //Ki = K_I;	//eeprom_rb((unsigned)&Ki);
    //Ko = K_O;	//eeprom_rb((unsigned)&Ko);
    Left.Acceleration = Right.Acceleration = K_ACCEL; 	//eeprom_rw((unsigned)&Acceleration);
    Left.Velocity = Right.Velocity = 0;
    Left.VelocitySetpoint = Right.VelocitySetpoint = 0;
    Left.PrevErr = Right.PrevErr = 0;
    Left.Ierror = Right.Ierror = 0;
    Left.Encoder = Right.Encoder = 0;
    
    OdometryResetPending = TRUE;		// Reset on startup.
    
    // Init encoder status
	encoders_old = ENCODER_PORT & ENCODER_MASK;
}


/* ---------------------------------------------------------
 void DoMotion(MotorInfo *p)

 Called at the loop rate to add "velocity" to the setpoint thus
 effecting a motion.

 Velocity is ramped up and down by "acceleration"

 Velocity and Acceleration are 8.8 numbers.  Velocity
 is added to the setpoint (an 24.8 number)
 ----------------------------------------------------------- */

void DoMotion(MotorInfo *p)
{
    if (p->Velocity < p->VelocitySetpoint)
    {
        p->Velocity += p->Acceleration;
        if (p->Velocity > p->VelocitySetpoint)
            p->Velocity = p->VelocitySetpoint;
    }
    else 
    {
        p->Velocity -= p->Acceleration;
        if (p->Velocity < p->VelocitySetpoint)
            p->Velocity = p->VelocitySetpoint;
    }
    AddToPosition(p, p->Velocity);
}


/* -------------------------------------------------------------
    void AddToPosition(pMotorInfo, int)

    Add an 8.8 value to the current setpoint.
   ------------------------------------------------------------- */
void AddToPosition(pMotorInfo p, int o)
{
    unsigned char s;
    
    s = SREG;				// Save processor status
    BeginCritical();
    
    p->Setpoint += o;        // Set point is 24.8 number
    
    SREG = s;				// Restore processor status
}

/* -------------------------------------------------------------
    uint8_t DoPid(pMotorInfo)

    The PID calculations.
   ------------------------------------------------------------- */
void DoPid(MotorInfo *p)
{
    int Perror, output;

    Perror = (p->Setpoint>>8) - p->Encoder;     // Set point is an 24.8 number (+/- 1 mile..)

#if pid_debug2
	//Debug
	PutCRLF();
	PutChar('E');
	PutChar('=');
	PutHexWord(Perror);
	PutChar('(');
	PutDecSignedWord(Perror);
	PutChar(')');
#endif

    // Derivative error is the delta Perror over Td (scale output to permit use of fraction in K constants)
    output = (R_ParameterTable.Kp*Perror + R_ParameterTable.Kd*(Perror - p->PrevErr) + R_ParameterTable.Ki*p->Ierror)/K_O;
    p->PrevErr = Perror;

    // Accumulate Integral error *or* Limit output.
    // Stop accumulating when output saturates
    
    if (output >= MAX_PWM_OUTPUT)
        output = MAX_PWM_OUTPUT;
    else if (output <= -MAX_PWM_OUTPUT)
        output = -MAX_PWM_OUTPUT;
    else
        p->Ierror += Perror;

    // Convert output to 0xFF-0x7F-0x00 (Fwd, netural, Rev)
    // locked anti-phase drive.
    p->pwm_out = (unsigned char)(output - (MAX_PWM_OUTPUT+1));
}

// Function Name:   motors()
// 
// Description:     Apply motor commands (from arbitration task to motors).
//
// WARNING: the velocity setpoints below are directly proportional to the PID sampling rate! Keep them in sync!
void motors(BehaviourInfo *pInfo)
{
	if (pInfo->type == ABSOLUTE)
	{
		// Check for enhanced precision mode request and act accordingly.
		Left.VelocitySetpoint = (pInfo->precision == TRUE) ? pInfo->output_l : pInfo->output_l<<8;
		Right.VelocitySetpoint = (pInfo->precision == TRUE) ? pInfo->output_r : pInfo->output_r<<8;
	}
	else
	{
		AddToPosition(&Left, pInfo->output_l<<8);
		AddToPosition(&Right, pInfo->output_r<<8);	
	}
}


// Function Name:   SynchWithMotion()
// 
// Description:     Synchronize with the motor control loop
//
//
inline void SynchWithMotion(void)
{
    AvrXWaitSemaphore(&MotorLoop);
}

void AddToLong(volatile long *p, signed char o)  // Used by AddToPosition & AddToSetpoint macros
{
    unsigned char s;
    
    s = SREG;				// Save processor status
    BeginCritical();
    *p += o;
    
    SREG = s;				// Restore processor status
}

// Function Name:   EmergencyStop()
// 
// Description:     Break RIGHT NOW! (do not deccellerate!)
//
//
inline void EmergencyStop(void)
{
	Left.Velocity = Right.Velocity = 0;
	Left.VelocitySetpoint = Right.VelocitySetpoint = 0;
	//Left.PrevErr = Right.PrevErr = 0;
	//Left.Ierror = Right.Ierror = 0;
	//Left.Encoder = Right.Encoder = 0;
	//PWM_R = MAX_PWM_OUTPUT;
	//PWM_L = MAX_PWM_OUTPUT;
}

//
// Decode quadrature signal from Left/Right encoders
// (based on XOR algorithm from http://www.emesystems.com/BS2fsm.htm)
//
inline void QuadratureDecoder(void)
{
    unsigned char new = PINE & ENCODER_MASK;
    unsigned char x = new ^ encoders_old;
    x |= (x>>1 & 0x55) | (x<<1 & 0xAA);
    x &= ((new>>1 ^ encoders_old) & 0x55) | ((new<<1 ^ encoders_old) & 0xAA);
    encoders_old = new;

    // Decode
    if (x & ENCODER_L_A) LeftEncoder++;
    if (x & ENCODER_L_B) LeftEncoder--;
    if (x & ENCODER_R_A) RightEncoder++;
    if (x & ENCODER_R_B) RightEncoder--;
}

//
// Function Name: WaitForMotionToStop()
//
void WaitForMotionToStop(void)
{
    while (Left.Velocity || Right.Velocity)
    {
        SynchWithMotion();
	}
}

//
// Function Name: ResetOdometry()
//
void ResetOdometry(void)
{
	OdometryResetPending = TRUE;
}