/*
 * File:   main.c
 * Author: Bennett and Bec
 *
 * Created on 15 October 2019, 14:59
 */

#include <stdio.h>
#include <stdlib.h>
#include "ConfigRegsPIC18f4520.h"
#include <timers.h>
#include <math.h>
#include <string.h>
#include <p18cxxx.h>

#include "motor.h"

#define M_PI 3.14159265358979323846264338327950288

// Function declarations
void configurePORTB(void);
void configureISR(void);
void lowPriorityISR (void);
void low_interrupt(void);
float getLMotorSpeed(void);
void encoderISR(void);

void configureSerial(void);
int tx232C(unsigned char * txPtr);

// Global variables
float encoder_left_t1;
float encoder_left_t2;
int encoder_overflow_counter;
float encoder_time_diff;
char str[] = "hello";
char buffer[10];

const float wheel_radius = 0.045;
const int counts_per_rev = 32;







//Define the low priority interrupt at 0x0018
//start the service routine for low priority interrupt
#pragma interruptlow lowPriorityISR
void lowPriorityISR (void){
    encoderISR();
}

#pragma code lowPriorityInterruptAddress=0x0018
void low_interrupt(void){
    //All that needs to happen in the interrupt is to goto the service routine
    _asm
    GOTO lowPriorityISR
    _endasm
}

#pragma code //default code section



void main(void) {

    //Go to the configure function

    configurePORTB();
    configureSerial();
    configureISR();

    // init
    initMotors();

    // Set individually
    setMotorL(20);
    setMotorR(20);




    while(1)
    {
        //tx232C(str);


        itoa(getLMotorSpeed(), buffer);
        tx232C(buffer);

    }

}

void configureISR(void)
{
    RCONbits.IPEN = 1;      // enable priority interrupts


    INTCONbits.RBIE = 1;            // enables interrupt on PORTB change
    INTCON2bits.RBIP = 0;           // low priority interrupt

    //INTCON2bits.INTEDG0 = 1;        // interrupt on rising edge (RB0)
    //INTCON2bits.INTEDG1 = 1;

    T1CON = 0b10000001;               // enable timer 1, prescaler of 1, 16 bit resolution

    PIE1bits.TMR1IE = 1;            // enable timer 1 overflow interrupt

    INTCONbits.GIEH = 1;    //Enable high priority interrupts
    INTCONbits.GIEL = 1;    //Enable low priority interrupts

}

void configurePORTB(void)
{
    TRISB = 1;          // set PORTB to input (***change to bits 4-5***)
    PORTB = 0;          // clear PORTB
}


void encoderISR(void)
{
    if(PIR1bits.TMR1IF == 1)
    {
        encoder_overflow_counter++;         // increment counter
        //if(encoder_overflow_counter)
        PIR1bits.TMR1IF = 0;                // clear overflow bit flag
    }

    if(INTCONbits.RBIF == 1)
    {
        // find time difference
        encoder_left_t2 = ReadTimer1();
        encoder_time_diff = encoder_overflow_counter*(2^16) + encoder_left_t2 - encoder_left_t1;
        encoder_left_t1 = encoder_left_t2;
        encoder_overflow_counter = 0;

        INTCONbits.RBIF = 0;        // clear flag
    }

}


float getLMotorSpeed(void)
{
    return wheel_radius*(2*M_PI)/(counts_per_rev*encoder_time_diff);
}


void configureSerial(void){
    //High speed baud rate setting (baud = 9600)
    SPBRG = 15;
    //Configure for transmission
    TXSTA = 0b00100000;
    RCSTA = 0b10010000;

}

int tx232C(unsigned char * txPtr){
    //Check for null terminating character, stay in loop while not null
    while(*txPtr != '\0')
    {
        if(TXSTAbits.TRMT == 1)
        {
        TXREG = *txPtr;
        txPtr++;
        }
    }
    //When null is reached, transmit the newline and return characters
    while(1){
        if(TXSTAbits.TRMT == 1){
            TXREG = '\n';
            while(1){
                if(TXSTAbits.TRMT == 1){
                TXREG = '\r';
                return 1;
                }
            }
        }
    }
}


// ----------------------------------------------------------------------------





// Initialise the struct for the motors
motor motorL, motorR;

// Initialise the motors and relevant PWM shenanigans
void initMotors(void) {
    // Make the PWM and direction pins output
    MOTL_PWM_TRIS = 0;
    MOTR_PWM_TRIS = 0;
    MOTL_DIR_TRIS = 0;
    MOTR_DIR_TRIS = 0;

    MOTL_CCPCON = 0b00001100; // PWM mode
    MOTR_CCPCON = 0b00001100; // PWM mode
    // Maximum period (gives a 9.8kHz signal)
    // T = 1/(10*10^6) * 4 * 256 = 102.4us or 9.766kHz
    MOT_PER_REG = 0xFF;

    //T2CON =   0b00000100; // T2 on

    // Enable timer 2
    T2CONbits.TMR2ON = 1;

}

// Sets the speed of both motors with one function
void setMotors(char speedL, char speedR) {
    setMotorL(speedL);
    setMotorR(speedR);
}

// Sets the speed/duty cycle and direction of the left motor
// Speed is from -127 to 127
void setMotorL(char speed) {
    //if ((speed > SPEED_ABS_MAX) || (speed < SPEED_ABS_MIN)) return;
    motorL.speed = speed;
    if(speed < 0) {
        speed *= -1;
        MOTL_DIR = BACKWARD;
    } else {
        MOTL_DIR = FORWARD;
    }
    MOTL_DUTY = speed * PWM_SCALE;

}

// Sets the speed/duty cycle and direction of the right motor
// Speed is from -127 to 127
void setMotorR(char speed) {
    //if ((speed > SPEED_ABS_MAX) || (speed < SPEED_ABS_MIN)) return;
    motorR.speed = speed;
    if(speed < 0) {
        speed *= -1;
        MOTR_DIR = BACKWARD;
    } else {
        MOTR_DIR = FORWARD;
    }
    MOTR_DUTY = speed * PWM_SCALE;

}

//
char getMotSpeedL(void) {
    return motorL.speed;
}

char getMotSpeedR(void) {
    return motorR.speed;
}