/*
 * $LastChangedRevision: 3952 $
 * $LastChangedDate:: 2025-11-27 21:04:55 +0100#$
 *
 * This file is part of X2C. web: x2c.lcm.at
 *
 * Copyright (c) 2019, Linz Center of Mechatronics GmbH (LCM), web: x2c.lcm.at
 * All rights reserved.
 */
/*
 * This file is licensed according to the BSD 3-clause license as follows:
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the "Linz Center of Mechatronics GmbH" and "LCM" nor
 *       the names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL "Linz Center of Mechatronics GmbH" BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include <p33CK256MP508.h>

#include "GlobalDefines.h"
#include "Main.h"
#include "Hardware.h"

/* private prototypes */
static void sendSerial(tSerial* serial, uint8 data);
static uint8 receiveSerial(tSerial* serial);
static uint8 isReceiveDataAvailable(tSerial* serial);
static uint8 isSendReady(tSerial* serial);
void EnableAndCalibrate();

/* ICD Communication Channel Select bits (Communicate on PGEC3 and PGED3) */
#pragma config ICS = PGD3

/* Select internal FRC without PLL at POR */
#pragma config FNOSC = FRC              /* Oscillator Mode (Internal Fast RC (FRC)) */
/* Enable Clock Switching, disable primary oscillator and disable clock out function */
#pragma config FCKSM = CSECMD           /* Clock Switching and Monitor (Clock switching is enabled, Fail-Safe Clock Monitor is disabled) */
#pragma config POSCMD = NONE            /* Primary Oscillator Source (Primary Oscillator Disabled) */
#pragma config OSCIOFNC = OFF           /* OSC2 Pin Function (OSC2 pin has clock out function) */


/** 
 * @brief Hardware initialization.
 */
void initHardware(void)
{
    /*------------------------------------------------------------------------*/
    /** - Configuration of oscillator
     *    - Internal oscillator (fast RC oscillator with PLL)
     *    - fCY = 50MHz
     *    .
     */
    /*------------------------------------------------------------------------*/
    /* Configure PLL prescaler, PLL postscaler, PLL divisor
     * FRC frequency: 8MHz
     * fCY = 8MHz * 125/(1*5*1)/2/2  = 50MHz
     */
    CLKDIVbits.PLLPRE = 1;      /* N1 = 1 */
    PLLFBDbits.PLLFBDIV = 125;  /* M = 125 */
    PLLDIVbits.POST1DIV = 5;    /* N2 = 5 */
    PLLDIVbits.POST2DIV = 1;    /* N3 = 1 */

    /* Initiate Clock Switch to FRC with PLL (NOSC=0b001) */
    __builtin_write_OSCCONH(0x01);
    __builtin_write_OSCCONL(OSCCON | 0x01);
    /* Wait for Clock switch to occur */
    while (OSCCONbits.OSWEN!= 0) {};
    /* Wait for PLL to lock */
    while(OSCCONbits.LOCK != 1) {};

    /* Enable watchdog */
    WDTCONLbits.ON = 1;


    /*------------------------------------------------------------------------*/
    /** - Configuration of serial port
     *    - Baudrate: 115.2kB/s
     *    - Data bits: 8
     *    - Parity: none
     *    - Stop bits: 1
     *    .
     */ 
    /*------------------------------------------------------------------------*/
    __builtin_write_RPCON(0x0000);  /* unlock registers */
    _U1RXR = 71;                    /* assign U1Rx to pin RP71 */
    _RP70R = 1;                      /* assign U1Tx to pin RP70 */
    __builtin_write_RPCON(0x0800);  /* lock registers */
    
    U1BRG = 26;                 /* set baudrate to 115200bps (26 = (fCY/baudrate/16)-1)*/
    U1MODEbits.ABAUD = 0;       /* disable automatic baud rate measurement */
    U1MODEbits.BRGH = 0;        /* low speed: baud rate is baudclk/16 */
    U1MODEbits.MOD = 0b0000;    /* asynchronous 8-bit UART, no parity */
    U1MODEHbits.STSEL = 0b00;   /* 1 stop bit */
    U1MODEHbits.BCLKMOD = 0b00; /* legacy mode */
    U1MODEHbits.BCLKSEL = 0b00; /* baud clock source: fosc/2=fP=fCY */
    U1MODEbits.UTXEN = 1;       /* enable transmitter */
    U1MODEbits.URXEN = 1;       /* enable receiver */
    U1MODEbits.UARTEN = 1;      /* enable UART 1 */
    

    /*------------------------------------------------------------------------*/
    /** - Configuration of IO ports */
    /*------------------------------------------------------------------------*/
    /* Debug LEDs */
    TRISEbits.TRISE8 = 0;  /* D17: digital output */
    LATEbits.LATE8 = 0;    /* D17: LED off */
    TRISEbits.TRISE9 = 0;  /* D2: digital output */
    LATEbits.LATE9 = 0;    /* D2: LED off */

    /* Hall and home inputs */
    TRISDbits.TRISD1 = 1;
    TRISDbits.TRISD2 = 1;
    TRISDbits.TRISD3 = 1;
    TRISDbits.TRISD4 = 1;
    
    /* Push button inputs */
    TRISDbits.TRISD5 = 1;
    TRISEbits.TRISE7 = 1;
    
    /* UART1 */
    TRISDbits.TRISD6 = 0;   /* Tx */
    TRISDbits.TRISD7 = 1;   /* Rx */
    
    /* Analog inputs */
    ANSELA = ANSELB = ANSELC = ANSELD = ANSELE = 0; /* disable all analog inputs */
    /* AN0: IMotor1 */
    TRISAbits.TRISA0 = 1;
    ANSELAbits.ANSELA0 = 1;
    /* ANA1: IMotor2 */
    TRISAbits.TRISA1 = 1;
    ANSELAbits.ANSELA1 = 1;
    /* AN15: IMotor_Sum */
    TRISCbits.TRISC3 = 1;
    ANSELCbits.ANSELC3 = 1;
    /* AN19: POT */
    TRISDbits.TRISD11 = 1;
    ANSELDbits.ANSELD11 = 1;
    /* AN18: VBUS */
    TRISDbits.TRISD10 = 1;
    ANSELDbits.ANSELD10 = 1;


    /*------------------------------------------------------------------------*/
    /** - Configuration of ADC
     *    - 12 bit resolution
     *    - internal RC clock source
     *    - AVdd as reference
     *    - unsigned, right justified output data format
     *    - Channels: AN0, ANA1, AN15, AN18, AN19
     *    .
     */
    /*------------------------------------------------------------------------*/
    /* Common settings */
    ADCON1Hbits.FORM = 0;       /* Integer output data format (right justified) */   
    ADCON2Lbits.EIEN = 1;       /* Enable early interrupts */
    ADCON3Lbits.REFSEL = 0;     /* AVdd/AVss reference voltage */
    ADCON3Hbits.CLKSEL = 0b00;  /* fP as clock source */
    ADCON3Hbits.CLKDIV = 3;     /* 1:4 clock divider */
    
    /* Shared core settings */
    ADCON3Hbits.SHREN = 1;          /* Enable shared ADC core */
    ADCON2Lbits.SHREISEL = 0b000;   /* Early interrupt is generated 1 tADCORE clock prior to when data is ready */
    ADCON2Lbits.SHRADCS = 2;        /* 1:4 clock divider */
    ADCON1Hbits.SHRRES = 0b11;      /* 12-bit resolution */
    
    /* Dedicated core 0 settings */
    ADCON3Hbits.C0EN = 1;       /* Enable dedicated ADC core 0 */
    ADCORE0Hbits.EISEL = 0b000; /* Early interrupt is generated 1 tADCORE clock prior to when data is ready */
    ADCORE0Hbits.ADCS = 2;      /* 1:4 clock divider */
    ADCORE0Hbits.RES = 0b11;    /* 12-bit resolution */
    ADCON4Hbits.C0CHS = 0b00;   /* AN0 channel select */
    
    /* Dedicated core 1 settings */
    ADCON3Hbits.C1EN = 1;       /* Enable dedicated ADC core 1 */
    ADCORE1Hbits.EISEL = 0b000; /* Early interrupt is generated 1 tADCORE clock prior to when data is ready */
    ADCORE1Hbits.ADCS = 2;      /* 1:4 clock divider */
    ADCORE1Hbits.RES = 0b11;    /* 12-bit resolution */
    ADCON4Hbits.C1CHS = 0b01;   /* ANA1 channel select */
    
    /* Enable early interrupt */
    ADEIELbits.EIEN0 = 1;
    ADEIELbits.EIEN1 = 1;
    ADEIELbits.EIEN15 = 1;
    ADEIEHbits.EIEN18 = 1;
    ADEIEHbits.EIEN19 = 1;
    
    /* Channel output data is unsigned */
    ADMOD0Lbits.SIGN0 = 0;
    ADMOD0Lbits.SIGN1 = 0;
    ADMOD0Hbits.SIGN15 = 0;
    ADMOD1Lbits.SIGN18 = 0;
    ADMOD1Lbits.SIGN19 = 0;
    
    /* Enable and calibrate the module */
    EnableAndCalibrate();
    
    /* Set same trigger source (PWM1) for all inputs to sample signals simultaneously */
    ADTRIG0Lbits.TRGSRC0 = 4;
    ADTRIG0Lbits.TRGSRC1 = 4;
    ADTRIG3Hbits.TRGSRC15 = 4;
    ADTRIG4Hbits.TRGSRC18 = 4;
    ADTRIG4Hbits.TRGSRC19 = 4;   

    /*------------------------------------------------------------------------*/
    /** - Configuration of Timer 1 unit for sampling time (100us) */
    /*------------------------------------------------------------------------*/
    T1CONbits.TCS = 0;      /* internal clock (fCY) */
    T1CONbits.TGATE = 0;    /* timer in counter mode */
    T1CONbits.TCKPS = 0b00; /* 1:1 prescaler */
    PR1 = X2C_SAMPLETIME;   /* X2C sample time */
    TMR1 = 0x0000;          /* clear timer 1 value */
    IFS0bits.T1IF = 0;      /* clear interrupt flag */
    IEC0bits.T1IE = 1;      /* set interrupt enable bit */
    T1CONbits.TON = 1;      /* start timer 1 */
    
    /*------------------------------------------------------------------------*/
    /** - Configuration of Timer 3 unit for CPU load measurement */
    /*------------------------------------------------------------------------*/
    CCP3CON1Lbits.MOD = 0b0000;     /* timer mode, output functions are disabled */
    CCP3CON1Lbits.T32 = 0;          /* 16-bit time base */
    CCP3CON1Lbits.TMRPS = 0b00;     /* 1:1 prescaler */
    CCP3CON1Lbits.CLKSEL = 0b000;   /* clock select: fOSC/2(fP) */
    CCP3TMRL = 0x0000;              /* clear timer 3 value */
    CCP3CON1Lbits.CCPON = 1;        /* enable module (start timer) */

    /*------------------------------------------------------------------------*/
    /** - Configuration of PWM */
    /*------------------------------------------------------------------------*/   
    /* Master clock source is selected as High speed PLL clock */
    PCLKCONbits.MCLKSEL = 0b00;
    
    /* Set PWM Phase Register - No phase shift */
    MPHASE = 0;
    
    /* Set PWM Period */
    MPER = PWM_FREQUENCY;
    
    /* Set PWM Duty Cycles to 50% */
    PG1DC = PWM_FREQUENCY >> 1;
    PG2DC = PWM_FREQUENCY >> 1;
    PG3DC = PWM_FREQUENCY >> 1;
    
    /* Set Dead Time Registers */
    PG1DTL = DEADTIME;
    PG1DTH = DEADTIME;
    PG2DTL = DEADTIME;
    PG2DTH = DEADTIME;
    PG3DTL = DEADTIME;
    PG3DTH = DEADTIME;
    
    /* PWM Generator 1 */
    PG1CONHbits.MDCSEL = 0;     /* PWM Generator uses PGxDC register */
    PG1CONHbits.MPERSEL = 1;    /* PWM Generator uses MPER register */
    PG1CONHbits.MPHSEL = 1;     /* PWM Generator uses MPHASE register */
    PG1CONHbits.MSTEN = 1;      /* PWM Generator broadcasts software set of UPDREQ control bit and EOC signal to other PWM generators */
    PG2CONHbits.UPDMOD = 0b000; /* SOC update */
    PG1CONHbits.SOCS = 0b0000;  /* PWM Generator is self-triggered */
    PG1CONLbits.ON = 0;         /* PWM Generator is disabled (for now) */
    PG1CONLbits.CLKSEL = 0b01;  /* PWM Generator uses Master Clock selected by the PCLKCONbits.MCLKSEL bits */
    PG1CONLbits.MODSEL = 0b100; /* Center-Aligned PWM mode */
    PG1IOCONHbits.PMOD = 0b00;  /* PWM Generator outputs operate in Complementary mode */
    PG1IOCONHbits.PENH = 1;     /* PWM Generator controls the PWMxH output pin */
    PG1IOCONHbits.PENL = 1;     /* PWM Generator controls the PWMxL output pin */
    PG1EVTLbits.UPDTRG = 0b01;  /* A write of the PGxDC register automatically sets the UPDATE bit */
    PG1EVTLbits.ADTR1EN1 = 1;   /* PGxTRIGA register compare event is enabled as trigger source for ADC Trigger 1 */
    
    /* PWM Generator 2 */
    PG2CONHbits.MDCSEL = 0;     /* PWM Generator uses PGxDC register */
    PG2CONHbits.MPERSEL = 1;    /* PWM Generator uses MPER register */
    PG2CONHbits.MPHSEL = 1;     /* PWM Generator uses MPHASE register */
    PG2CONHbits.MSTEN = 0;      /* PWM Generator does not broadcast the UPDREQ status bit state or EOC signal */
    PG2CONHbits.UPDMOD = 0b011; /* Slaved immediate update */
    PG2CONHbits.SOCS = 0b0001;  /* Trigger output selected by PG1 */
    PG2CONLbits.ON = 1;         /* PWM Generator is enabled */
    PG2CONLbits.CLKSEL = 0b01;  /* PWM Generator uses Master Clock selected by the PCLKCONbits.MCLKSEL bits */
    PG2CONLbits.MODSEL = 0b100; /* Center-Aligned PWM mode */
    PG2IOCONHbits.PMOD = 0b00;  /* PWM Generator outputs operate in Complementary mode */
    PG2IOCONHbits.PENH = 1;     /* PWM Generator controls the PWMxH output pin */
    PG2IOCONHbits.PENL = 1;     /* PWM Generator controls the PWMxL output pin */
    PG2EVTLbits.UPDTRG = 0b01;  /* A write of the PGxDC register automatically sets the UPDATE bit */
    PG2EVTLbits.ADTR1EN1 = 0;   /* PGxTRIGA register compare event is disabled as trigger source for ADC Trigger 1 */
    
    /* PWM Generator 3 */
    PG3CONHbits.MDCSEL = 0;     /* PWM Generator uses PGxDC register */
    PG3CONHbits.MPERSEL = 1;    /* PWM Generator uses MPER register */
    PG3CONHbits.MPHSEL = 1;     /* PWM Generator uses MPHASE register */
    PG3CONHbits.MSTEN = 0;      /* PWM Generator does not broadcast the UPDREQ status bit state or EOC signal */
    PG3CONHbits.UPDMOD = 0b011; /* Slaved immediate update */
    PG3CONHbits.SOCS = 0b0001;  /* Trigger output selected by PG1 */
    PG3CONLbits.ON = 1;         /* PWM Generator is enabled */
    PG3CONLbits.CLKSEL = 0b01;  /* PWM Generator uses Master Clock selected by the PCLKCONbits.MCLKSEL bits */
    PG3CONLbits.MODSEL = 0b100; /* Center-Aligned PWM mode */
    PG3IOCONHbits.PMOD = 0b00;  /* PWM Generator outputs operate in Complementary mode */
    PG3IOCONHbits.PENH = 1;     /* PWM Generator controls the PWMxH output pin */
    PG3IOCONHbits.PENL = 1;     /* PWM Generator controls the PWMxL output pin */
    PG3EVTLbits.UPDTRG = 0b01;  /* A write of the PGxDC register automatically sets the UPDATE bit */
    PG3EVTLbits.ADTR1EN1 = 0;   /* PGxTRIGA register compare event is disabled as trigger source for ADC Trigger 1 */
    
    /* Turning ON the PWM Generator 1 -> starting all the PWM modules in unison */
    PG1CONLbits.ON = 1;
}


/*============================================================================*/
/* Initialization of serial interface (overriding methods) */
/*============================================================================*/
/**
 * @brief Initialization of serial interface.
 * 
 * @param serial Serial interface object.
 */
void initSerial(tSerial* serial)
{
    serial->send = (void (*)(tInterface*, uint8))sendSerial;
    serial->receive = (uint8 (*)(tInterface*))receiveSerial;
    serial->isReceiveDataAvailable = (uint8 (*)(tInterface*))isReceiveDataAvailable;
    serial->isSendReady = (uint8 (*)(tInterface*))isSendReady;
}


/**
 * @brief Routine to send data over serial interface.
 * 
 * @param serial Serial interface object.
 * @param data Data byte to send.
 */
static void sendSerial(tSerial* serial, uint8 data)
{
    U1TXREG = data;
}


/**
 * @brief Routine to fetch data from the serial interface.
 * 
 * @param serial Serial interface object.
 * @return Received data byte.
 */
static uint8 receiveSerial(tSerial* serial)
{
    if (U1STA & 0x0E)
    {
        U1STAbits.OERR = 0; /* reset error */
        return ((uint8)0);
    }
    return (U1RXREG);
}


/**
 * @brief Routine to check if data from the serial interface is available.
 * 
 * @param serial Serial interface object.
 * @return 1 when data is available, 0 otherwise.
 */
static uint8 isReceiveDataAvailable(tSerial* serial)
{
    return (!U1STAHbits.URXBE);
}


/**
 * @brief Routine to check if data can be send over the serial interface.
 *  
 * @param serial Serial interface object.
 * @return 1 when data can be send, 0 otherwise.
 */
static uint8 isSendReady(tSerial* serial)
{
    return (!U1STAHbits.UTXBF);
}

/*============================================================================*/
/* Interrupt service routines */
/*============================================================================*/
/**
 * @brief Interrupt service routine for Timer 1.
 * 
 * This routine will be called every 100us (= sample time) and calls the main
 * application task.
 */
void __attribute__((__interrupt__, no_auto_psv)) _T1Interrupt(void)
{
    IFS0bits.T1IF = 0;      /* clear interrupt flag */
    
    START_CPU_MEASUREMENT;	/* start CPU load measurement */
    mainTask();
    END_CPU_MEASUREMENT;	/* end CPU load measurement */
}

/**
 * @brief Interrupt service routine for AN0 (not used).
 */
void __attribute__((interrupt, no_auto_psv)) _ADCAN0Interrupt(void)
{
    _ADCAN0IF = 0;      /* clear interrupt flag */
}

/**
 * @brief Interrupt service routine for AN1 (not used).
 */
void __attribute__((interrupt, no_auto_psv)) _ADCAN1Interrupt(void)
{
    _ADCAN1IF = 0;      /* clear interrupt flag */

}

/**
 * @brief Interrupt service routine for AN15 (not used).
 */
void __attribute__((interrupt, no_auto_psv)) _ADCAN15Interrupt(void)
{
    _ADCAN15IF = 0;     /* clear interrupt flag */
}

/**
 * @brief Interrupt service routine for AN18 (not used).
 */
void __attribute__((interrupt, no_auto_psv)) _ADCAN18Interrupt(void)
{
    _ADCAN18IF = 0;     /* clear interrupt flag */
}

/**
 * @brief Interrupt service routine for AN19 (not used).
 */
void __attribute__((interrupt, no_auto_psv)) _ADCAN19Interrupt(void)
{
    _ADCAN19IF = 0;     /* clear interrupt flag */
}


/*============================================================================*/
/* Private functions */
/*============================================================================*/
/**
 * @brief Routine to enable and calibrate the analog-to-digital peripheral.
 */
void EnableAndCalibrate()
{
    /** - Set initialization time */
    ADCON5Hbits.WARMTIME = 15;

    /** - Turn on ADC module */
    ADCON1Lbits.ADON = 1;
    
    /** - Dedicated core 0 */
    ADCON5Lbits.C0PWR = 1;  /* Turn on analog power */
    while(ADCON5Lbits.C0RDY == 0);  /* Wait until core is ready for operation */
    ADCON3Hbits.C0EN = 1;   /* Turn on digital power to enable triggers to the core */
    
    /** - Dedicated core 1 */
    ADCON5Lbits.C1PWR = 1;  /* Turn on analog power */
    while(ADCON5Lbits.C1RDY == 0);  /* Wait until core is ready for operation */
    ADCON3Hbits.C1EN = 1;   /* Turn on digital power to enable triggers to the core */
    
    /** - Shared core */
    ADCON5Lbits.SHRPWR = 1;  /* Turn on analog power */
    while(ADCON5Lbits.SHRRDY == 0);  /* Wait until core is ready for operation */
    ADCON3Hbits.SHREN = 1;   /* Turn on digital power to enable triggers to the core */
    
    /** - Calibration (not possible) */
}