#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <plib/timers.h>
#include <plib/pwm.h>
#include <plib/usart.h>
#include <stdbool.h>
#include "TractorBot2_Servo.h"
#include "TractorBot2.h"

// PIC18F46K22 configurations

#pragma config FOSC=INTIO67,PLLCFG=ON,PRICLKEN=OFF,FCMEN=OFF,IESO=OFF,PWRTEN=ON
#pragma config BOREN=OFF,BORV=190,WDTEN=OFF,WDTPS=512,PBADEN=OFF,MCLRE=EXTMCLR,CCP3MX=PORTE0
#pragma config STVREN=OFF,LVP=OFF,DEBUG=OFF,CP0=OFF,CP1=OFF,CP2=OFF,CP3=OFF
#pragma config CPB=OFF,CPD=OFF,WRT0=OFF,WRT1=OFF,WRT2=OFF,WRT3=OFF,WRTC=OFF
#pragma config WRTB=OFF,WRTD=OFF,EBTR0=OFF,EBTR1=OFF,EBTR2=OFF,EBTR3=OFF,EBTRB=OFF

// Pin definitions

#define MDC3b                LATAbits.LATA0     
#define MDC3a                LATAbits.LATA1
#define MDC2b                LATAbits.LATA2
#define MDC2a                LATAbits.LATA3
#define MDC1b                LATAbits.LATA4
#define MDC1a                LATAbits.LATA5
#define M2step1              LATAbits.LATA6
#define M2step2              LATAbits.LATA7

#define LSW_OPN             PORTBbits.RB5
#define LSW_CLS             PORTBbits.RB6

#define M1Step4             LATCbits.LATC0
#define M1Step3             LATCbits.LATC1
#define M1Step2             LATCbits.LATC2
#define M1Step1             LATCbits.LATC3
#define Res4                LATCbits.LATC4
#define Res5                LATCbits.LATC5
#define Res6                LATCbits.LATC6
#define Res7                LATCbits.LATC7

//#define VOLTAGE          PORTDbits.RD0
//#define CURRENT          PORTDbits.RD1
#define Res8               LATDbits.LATD2   
#define Res9               LATDbits.LATD3   
#define Res10              LATDbits.LATD4   
#define Res11              LATDbits.LATD5   
//RD6 UART TX
//RD7 UART RX

#define sirena             LATEbits.LATE0    
#define M2step4            LATEbits.LATE1    
#define M2step3            LATEbits.LATE2       

#define M2stepTo_0 M2step1=0;M2step2=0;M2step3=0;M2step4=0;

#define   Close   0
#define   Open    1
#define   Stop    2

//sound bytes
#define hello   4105
#define dog     2212
#define battery 3224
#define OK      6709
#define ohno    7150

#define forward       1
#define Reverse       2
#define Stop_drive    3
#define Turn_right    10
#define Turn_left     20
#define Stop_turning  30

#define s       1000000
#define ms      1000
#define us      1

#define not_black  0
#define black      1

#define _XTAL_FREQ  64000000
#define USE_AND_MASKS

//variable declarations

bool UARTanalysisZap=0;

unsigned long TMR0_interrupt_number, TMR0_total, endstep=0, num,numSuf1,numSuf2,numPref;
unsigned char UARTConfig = 0, baud = 0, UART_RX_Char = 0, UART_RX_Buffer[100],UART_TX_Buffer[100],OSCFrequency, UARTflag=0;

//procedure declarations
void PICinit();

int Get_Battery(int samples);

int Speaker(unsigned int sound);

int MotorsControl(int Motor_move, bool blink);
int Gripper(char Move);
void Wait(float time,long unit);
void Stopwatch_ON();
unsigned long Stopwatch_OFF();
void interrupt Interruption();
void UARTanalysis();

void ReadMaster(char *buffer, unsigned char len);
/*******************************************************************************/
/*-----------------------------------------------------------------------------*/
/*******************************************************************************/

int Get_Battery(int samples)
{
    long j2,result=0;

    for(j2=0;j2<samples;j2++)
    {
        ADCON0=0b1010001;    // set channel RD0(AN20)(PIN 19)
	ADCON0=0b1010011;    // Conversion ON
    	while(ADCON0==0b01010011);	// waiting for conversion
        result+=ADRES;
    }
    return (result/samples);

}

int Get_Current(int samples)
{
    long j2,result=0;

    for(j2=0;j2<samples;j2++)
    {
        ADCON0=0b1010101;    // set channel RD1(AN21)(PIN 20)
	ADCON0=0b1010111;    // Conversion ON
    	while(ADCON0==0b1010111);	// waiting for conversion
        result+=ADRES;
    }
    return (result/samples);

}
//******************************************************************************
//------------------------------------------------------------------------------
//******************************************************************************
void PICinit()
{
 BSR=15;

 TRISA=0;  //all output=0,input=1
 ANSELA=0; //all digital, 0 = digi, 1 = analog
 PORTA=0;
 LATA=0;   

 TRISB=0b11111111;
 ANSELB=0;  
 PORTB=0;
 LATB=0;

 INTCON2=0;
 WPUB=0b01100000; //pull-up on RB6,RB5

 TRISC=0;
 ANSELC=0;
 PORTC=0;
 LATC=0;

 TRISD= 0b11000011;
 ANSELD=0b00000011;
 PORTD=0;
 LATD=0;

 TRISE=0b1000;
 ANSELE=0;
 PORTE=0;
 LATE=0;

 OSCCON=0b01110000;  // 16MHz internal oscillator
 OSCCON2=0b10000000; // 4xPLL=64MHz
 OSCTUNEbits.PLLEN=1; //PLL on
 OSCFrequency=64;
 switch(OSCFrequency) {
     case 4:  T0CON=0b10001000; break;
     case 8:  T0CON=0b10000000; break;
     case 16: T0CON=0b10000001; break;
     case 32: T0CON=0b10000010; break;
     case 64: T0CON=0b10000011; break; }

 ADCON2=0b10111110; // right adjustment, delay 20TAD, frequency conversion FOSC/64
 ADCON1=0; //VDD voltage reference

// UART2 configuration
    UARTConfig = USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH ;
    baud = 34;
    Open2USART(UARTConfig,baud);

 // setting interrupts
    INTCONbits.GIE=1;  // global enable all interrupts

    // UART2 interrupts
    PIR3bits.RC2IF=0;  // interrupt flag
    IPR3bits.RC2IP=1;  // high interrupt priority
    PIE3bits.RC2IE=1;  // interrupt enable
    INTCONbits.PEIE=1; // peripheral interrupts enable
    
    
}

//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************
void main()
{
    PICinit();
    while(1)
    {
        if(UARTflag!=0)
        {
        
             num=atol(UART_RX_Buffer);
             if(num>100000000) {numSuf2=(int)num%10; num/=10;}
             numSuf1=num%10000; num/=10000;
             numPref=num%10000;

        switch(numPref)
        {
            case Ureverse: MotorsControl(Reverse, true);break;
            case Uforward: MotorsControl(forward, true); break;

            case UStop_drive: MotorsControl(Stop_turning, true); Gripper(Stop); break;

            case UTurn_right: MotorsControl(Turn_right, true);break;
            case UTurn_left:  MotorsControl(Turn_left, true); break;

            case GripperOpen:
                 Gripper(Open);
                 while(LSW_OPN==1);
                 Gripper(Stop);
                 break;
                 
            /*case GripperClose:
                 Gripper(Close);
                 while(LSW_CLS==0);
                 Gripper(Stop); break;*/
            case GripperClose:
                 Gripper(Close);break;
            
            case StepRight:
                 M2stepTo_0 M2step2=1; Wait(20,ms);
                 M2stepTo_0 M2step4=1; Wait(20,ms);
                 M2stepTo_0 M2step1=1; Wait(20,ms);
                 M2stepTo_0 M2step3=1; Wait(20,ms);

                 M2stepTo_0 M2step2=1; Wait(20,ms);
                 M2stepTo_0 M2step4=1; Wait(20,ms);
                 M2stepTo_0 M2step1=1; Wait(20,ms);
                 M2stepTo_0 M2step3=1; Wait(20,ms);

                 M2stepTo_0 M2step2=1; Wait(20,ms);
                 M2stepTo_0 M2step4=1; Wait(20,ms);
                 M2stepTo_0 M2step1=1; Wait(20,ms);
                 M2stepTo_0 M2step3=1; Wait(20,ms);

                 M2stepTo_0 M2step2=1; Wait(20,ms);
                 M2stepTo_0 M2step4=1; Wait(20,ms);
                 M2stepTo_0 M2step1=1; Wait(20,ms);
                 M2stepTo_0 M2step3=1; Wait(20,ms);

                 M2stepTo_0
                break;
            case StepLeft:
                M2stepTo_0 M2step3=1; Wait(20,ms);
                M2stepTo_0 M2step1=1; Wait(20,ms);
                M2stepTo_0 M2step4=1; Wait(20,ms);
                M2stepTo_0 M2step2=1; Wait(20,ms);

                M2stepTo_0 M2step3=1; Wait(20,ms);
                M2stepTo_0 M2step1=1; Wait(20,ms);
                M2stepTo_0 M2step4=1; Wait(20,ms);
                M2stepTo_0 M2step2=1; Wait(20,ms);

                M2stepTo_0 M2step3=1; Wait(20,ms);
                M2stepTo_0 M2step1=1; Wait(20,ms);
                M2stepTo_0 M2step4=1; Wait(20,ms);
                M2stepTo_0 M2step2=1; Wait(20,ms);

                M2stepTo_0 M2step3=1; Wait(20,ms);
                M2stepTo_0 M2step1=1; Wait(20,ms);
                M2stepTo_0 M2step4=1; Wait(20,ms);
                M2stepTo_0 M2step2=1; Wait(20,ms);

                M2stepTo_0
                break;

            case Shello    :   Speaker(hello);   break;
            case Sdog      :   Speaker(dog);     break;
            case Sbattery  :   Speaker(battery); break;
            case SOk       :   Speaker(OK);      break;
            case Sohno     :   Speaker(ohno);    break;

            case Power: sprintf(UART_TX_Buffer,"%04i%04i", Get_Battery(100), Get_Current(100));
                        while(Busy2USART());
                        puts2USART(UART_TX_Buffer); break;
        }
        UARTflag=0;
            
        }
    }
}
//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************

void Wait(float timefloat, long unit)
{
     unsigned long timelong, remainingtime;
     int overflow=0;

     timelong=(unsigned long)(timefloat*unit);
     overflow=timelong/65535;
     remainingtime=65535-(timelong-(overflow*65535));

     WRITETIMER0(remainingtime);
     INTCONbits.TMR0IF=0;

    do
    {
        while(INTCONbits.TMR0IF==0);
        INTCONbits.TMR0IF=0;
        overflow--;
    }
    while(overflow>=0);
}

void Stopwatch_ON()
{
    INTCONbits.TMR0IE=1;
    INTCONbits.TMR0IF=0;
    TMR0_interrupt_number=0;
    WRITETIMER0(0);
}

unsigned long Stopwatch_OFF()
{
    TMR0_total=READTIMER0();
    INTCONbits.TMR0IE=0;
    INTCONbits.TMR0IF=0;

    return ((TMR0_interrupt_number*65536)+ TMR0_total);
}

void interrupt Interruption(void)
{
    
    if(INTCONbits.TMR0IF==1 && INTCONbits.TMR0IE==1)
    {
        TMR0_interrupt_number++;
        INTCONbits.TMR0IF=0;
        return;
    }

    if(PIR3bits.RC2IF == 1 && PIE3bits.RC2IE==1)
    {
        PIE3bits.RC2IE=0;
        UARTflag=1;
        while(Busy2USART());
        ReadMaster(UART_RX_Buffer, 9);
        PIR3bits.RC2IF=0;
        PIE3bits.RC2IE=1;
        return;
    }
}
//******************************************************************************
int MotorsControl(int Motor_move, bool blink)
{
        switch(Motor_move)
        {
            case forward:
                MDC1a=0;MDC1b=1;
                MDC3a=0;MDC3b=1;
                break;
            case Reverse:
                MDC1a=1;MDC1b=0;
                MDC3a=1;MDC3b=0;
                break;
                
            case Stop_turning:
            case Stop_drive:
                MDC1a=0;  MDC1b=0;
                MDC3b=0; MDC3a=0;
                break;
                
            case Turn_right:
                MDC1a=0;MDC1b=1;
                MDC3a=1;MDC3b=0;
                break;
            case Turn_left:
                MDC1a=1;MDC1b=0;
                MDC3a=0;MDC3b=1;
                break;
        }
        return Motor_move;
}

int Gripper(char Move)
{
        if(Move==Open) {MDC2a=1;MDC2b=0;}
   else if(Move==Close) {MDC2a=0;MDC2b=1;}
        else {MDC2a=0;MDC2b=0;}
        return 0;
}

int Speaker(unsigned int sound)
{
    unsigned int i;
    
    OSCCON=0b01100000; //8 MHz internal oscillator
    OSCTUNEbits.PLLEN=0;
    PMD1=0;

    // setting PWM
    TRISEbits.TRISE1=1;
    CCPTMRS1=0;  // PWM uses Timer2

    PR2=249;

    CCP3CON=0b00001111; //PWM on
    CCPR3L=0;
    PIR1bits.TMR2IF=0;
    T2CON=0b00000100; //prescaler 1:1
    TRISEbits.TRISE1=0;
    if(sound==hello)
    {
        for(i=0;i<hello;i++)
        {
            CCPR3L=hello1b[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }

    if(sound==dog)
    {
        for(i=0;i<dog;i++)
        {
            CCPR3L=dog1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==battery)
    {
        for(i=0;i<battery;i++)
        {
            CCPR3L=battery1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==OK)
    {
        for(i=0;i<OK;i++)
        {
            CCPR3L=OK1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==ohno)
    {
        for(i=0;i<ohno;i++)
        {
            CCPR3L=ohno1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    OSCCON=0b01110000; //16 MHz internal oscillator
    OSCTUNEbits.PLLEN=1;
    CCPR3L=0;
    CCP3CON=0;
    return 0;
}

void ReadMaster(char *buffer, unsigned char len)
{
  char i;    // Length counter
  unsigned char data;

  for(i=0;i<len;i++)  // Only retrieve len characters
  {
   while(PIR3bits.RC2IF == 0);// Wait for data to be received

 //   data = getc2USART();    // Get a character from the USART
                           // and save in the string
   data = RCREG2;

   *buffer = data;
    buffer++;              // Increment the string pointer
  }
}