#include <stdio.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "libq.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include "types.h"

#ifdef DEBUG

void __attribute__((__interrupt__, auto_psv)) _U1RXInterrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _U2RXInterrupt(void)
{
    char c;


    __asm__ volatile ("push CORCON");
    // c = getU2();
    c = U1RXREG;
    
    if(CalibrationMode == 2)  //In Calibrate mode, so store received characters in buffer until line field is recieved
    {
      U1_inputBuf[U1_BufCtr++] = c;
      if(U1_BufCtr == U1_InputBufSize)
         U1_BufCtr = 0;
      putU1(c);
      if((c == 0x08) && (U1_BufCtr == 1))
        U1_BufCtr -= 1; 
      
      if((c == 0x08) && (U1_BufCtr > 1))
      {  
          putU1(' '); //Backspace destructively
          putU1(c);
          U1_BufCtr -= 2;      
      }
      if(c == '\r')
        {
        U1_StringReceived = U1_BufCtr;  //Indicate string recieved and length
        putU1('\n');
        }     
    }
    
    else  //  Handle single charcater commands)
    {
    switch (c) {
       
        case 'C': //Set display to raw counts
            CalibrationMode = 1;  //Indicate to WindingControl ISR that Calibration mode has been selected.
            break;

    //     case '5': //Set display to raw counts
    //        BATTERY_SWITCH = 1;
    //        break;
    //
    //    case '6': //Set display to engineering units
    //        BATTERY_SWITCH = 0;
    //        break;
            
            
            
        
        case 'x': //Toggle External Load
            if (StatusBitFields.BattSwitchRequest == 0)
                StatusBitFields.BattSwitchRequest = 1;
            else
                StatusBitFields.BattSwitchRequest = 0;

            break;

        case 'c': //Set display to raw counts
            raw = 1;
            break;

        case 'e': //Set display to engineering units
            raw = 0;
            break;
            
            
//        case '-':
//            if(P1DC4 > 0)
//              P1DC4 = P1DC4 - 1;
//        break;
//        
//        case '=':
//        if(P1DC4 < 2*P1TPER-1)
//               P1DC4 = P1DC4 + 1;
//        break;
                
            
#ifndef TUNE_WINDING_FLUX
           
                      
        case '-': //Decrease Commanded Current in Torque Mode
            if (StatusBitFields.Mode != PREINDEXPULSE_MODE) //Only allow user to set current if Index Pulse ahs been seen.
            {

                if (UserCommandedCurrent.Value > UserCommandedCurrent.LowerLimit + 50)
                    UserCommandedCurrent.Value = UserCommandedCurrent.Value - 50;

                // if (TargetFluxCurrent > -32768 + 100)
                //     TargetFluxCurrent = TargetFluxCurrent - 100;    


                TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
                StatusBitFields.Mode = TORQUE_MODE;
            }

            break;

        case '=': //Increase Commanded Current in Torque Mode

            if (StatusBitFields.Mode != PREINDEXPULSE_MODE) //Only allow user to set current if Index Pulse ahs been seen.
            {
                if (UserCommandedCurrent.Value < UserCommandedCurrent.UpperLimit - 50)
                    UserCommandedCurrent.Value = UserCommandedCurrent.Value + 50;

                //          if (TargetFluxCurrent < 32767 - 100)
                //     TargetFluxCurrent = TargetFluxCurrent + 100;

                TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
                StatusBitFields.Mode = TORQUE_MODE;
            }
            break;
        
             
#else
        case '-': //Decrease Commanded Current in Torque Mode
            if (TargetFluxCurrent > -32768 + 100)
                TargetFluxCurrent = TargetFluxCurrent - 100;
            TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
            StatusBitFields.Mode = TORQUE_MODE;
            break;

        case '=': //Increase Commanded Current in Torque Mode
            if (TargetFluxCurrent < 32767 - 100)
                TargetFluxCurrent = TargetFluxCurrent + 100;
            TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
            StatusBitFields.Mode = TORQUE_MODE;
            break;

#endif

        case 'r':
           // AUXIO_PIN2 = 1;

#ifdef TUNE_BATT_DRAW_CURRENT

              if (MaxBattDrawCurrent.Value == ConvertEngUnitsToFract(-DEFAULT_BATTDRAWCURRENTLIMIT, Cal.IBatt_cnt, Cal.IBatt_eng)) {
                    tmpMaxBattDrawCurrent = ConvertEngUnitsToFract(-(DEFAULT_BATTDRAWCURRENTLIMIT-1), Cal.IBatt_cnt, Cal.IBatt_eng);
                } else {
                    tmpMaxBattDrawCurrent = ConvertEngUnitsToFract(-DEFAULT_BATTDRAWCURRENTLIMIT, Cal.IBatt_cnt, Cal.IBatt_eng);
                }
                asm volatile ("disi #0x3FFF");
                MaxBattDrawCurrent.Value = tmpMaxBattDrawCurrent;
                asm volatile ("disi #0x0000");    
            
#endif
            
#ifdef TUNE_WINDING_QUADRATURE
            if (StatusBitFields.Mode == TORQUE_MODE) {

                TargetFluxCurrent = -TargetFluxCurrent;

                TargetQuadratureCurrent = -TargetQuadratureCurrent;
                CommandedCurrent = TargetQuadratureCurrent;

                TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
            }
#endif

#ifdef TUNE_WINDING_FLUX
            if (StatusBitFields.Mode == TORQUE_MODE) {
                TargetFluxCurrent = -TargetFluxCurrent;
                TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
            }
#endif          





#ifdef TUNE_SHUNT_REGULATOR  
            int tmpTargetVoltage;
            if (StatusBitFields.Mode == GENERATOR_MODE) {
                if (TargetVoltage.Value == ConvertEngUnitsToFract(DEFAULT_TARGETVOLTAGE, Cal.VBus_cnt, Cal.VBus_eng)) {
                    tmpTargetVoltage = ConvertEngUnitsToFract(175, Cal.VBus_cnt, Cal.VBus_eng);
                } else {
                    tmpTargetVoltage = ConvertEngUnitsToFract(DEFAULT_TARGETVOLTAGE, Cal.VBus_cnt, Cal.VBus_eng);
                }
                asm volatile ("disi #0x3FFF");
                TargetVoltage.Value = tmpTargetVoltage;
                asm volatile ("disi #0x0000");


            }
#endif 

#ifdef TUNE_VQVD_DISTANCE
            if (TargetCircleRadius == PRELIMITER_TARGETCIRCLERADIUS) {
                TargetCircleRadius = PRELIMITER_TARGETCIRCLERADIUS - 3000;
            } else {
                TargetCircleRadius = PRELIMITER_TARGETCIRCLERADIUS;
            }

#endif

            //AUXIO_PIN2 = 0;
            break;


        case'q': //Reset iTemp4 to zero.
            iTemp2 = 0;
            //AUXIO_PIN2 = 0;
            break;

        case 'p': //Increase Proportional

            if (kCoeffs[0] <= Q15(1.0 - .002)) {
                kCoeffs[0] = _Q15add(kCoeffs[0], Q15(.002));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;


        case 'o': //Decrease Proportional

            if (kCoeffs[0] >= Q15(.002)) {
                kCoeffs[0] = _Q15sub(kCoeffs[0], Q15(.002));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;


        case 'l': //Increase Int
            if (kCoeffs[1] <= Q15(1.0 - .0005)) {
                kCoeffs[1] = _Q15add(kCoeffs[1], Q15(.0005));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;

        case 'k': //Decrease Int
            if (kCoeffs[1] >= Q15(.0005)) {
                kCoeffs[1] = _Q15sub(kCoeffs[1], Q15(.0005));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;

        case ',': //Increase Der
            if (kCoeffs[2] <= Q15(1.0 - .002)) {
                kCoeffs[2] = _Q15add(kCoeffs[2], Q15(.002));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;

        case 'm': //Decrease Der
            if (kCoeffs[2] >= Q15(.002)) {
                kCoeffs[2] = _Q15sub(kCoeffs[2], Q15(.002));
                asm volatile ("disi #0x3FFF");
                PIDCoeffCalc((fractional *) & kCoeffs[0], PIDptr); //PID is a pointer to one of the various tPID structures
                asm volatile ("disi #0x0000");
            }
            break;

        case '1':
            PR3 = PR3-5;
            break;
            
        case '2':
            PR3 = PR3+5;
            break;
            
//        case '4':
//            if(P1DTCON1 > 1)
//                P1DTCON1 = P1DTCON1 - 1;
//            break;
//            
//        case '5':
//            if(P1DTCON1 < 30)
//                P1DTCON1 = P1DTCON1 + 1;
//            
//                   case 'i':
//           EncoderStatus =  INDEX_PULSE_SEEN;
//           POSCNT = 0;
//            break;
//            
            
//                        
//        case 'i':
//            EncoderStatus = INDEX_PULSE_SEEN;
//            break;
//            
            
        default:
            break;
    }
    }

    _U1RXIF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}

#endif