#pragma config FEXTOSC = OFF
#pragma config RSTOSC = HFINTOSC_64MHZ
#pragma config WDTE = OFF
#include <xc.h>

#define CCP2_INDEX 0
#define PWM3_INDEX 1
#define PWM4_INDEX 2
#define REG_OFF 3
#define CITAC_MAX 155
#define STRIDA_MIN 0
#define STRIDA_MAX 65535
#define KONEC 2500

unsigned short i_zadany_kladny[] = {43884,44015,44145,44274,44404,44532,44660,44788,44914,45041,
                                    45167,45292,45416,45540,45664,45787,45909,46031,46152,46272,
                                    46392,46511,46629,46747,46865,46981,47097,47212,47327,47441,
                                    47554,47667,47778,47890,48000,48110,48219,48327,48435,48542,
                                    48648,48753,48858,48962,49065,49168,49269,49370,49470,49570,
                                    49668,49766,49863,49960,50055,50150,50244,50337,50429,50520,
                                    50611,50701,50790,50878,50965,51051,51137,51222,51306,51389,
                                    51471,51552,51632,51712,51791,51869,51945,52021,52097,52171,
                                    52244,52317,52388,52459,52529,52597,52665,52732,52798,52863,
                                    52928,52991,53053,53115,53175,53235,53293,53351,53407,53463,
                                    53518,53572,53625,53676,53727,53777,53826,53874,53921,53967,
                                    54012,54056,54099,54142,54183,54223,54262,54300,54337,54373,
                                    54408,54443,54476,54508,54539,54569,54598,54626,54653,54680,
                                    54705,54729,54752,54774,54795,54815,54834,54852,54869,54884,
                                    54899,54913,54926,54938,54949,54958,54967,54975,54981,54987,
                                    54992,54995,54998,54999,55000};

unsigned short i_zadany_zaporny[] = {21652,21521,21391,21262,21132,21004,20876,20748,20622,20495,
                                     20369,20244,20120,19996,19872,19749,19627,19505,19384,19264,
                                     19144,19025,18907,18789,18671,18555,18439,18324,18209,18095,
                                     17982,17869,17758,17646,17536,17426,17317,17209,17101,16994,
                                     16888,16783,16678,16574,16471,16368,16267,16166,16066,15966,
                                     15868,15770,15673,15576,15481,15386,15292,15199,15107,15016,
                                     14925,14835,14746,14658,14571,14485,14399,14314,14230,14147,
                                     14065,13984,13904,13824,13745,13667,13591,13515,13439,13365,
                                     13292,13219,13148,13077,13007,12939,12871,12804,12738,12673,
                                     12608,12545,12483,12421,12361,12301,12243,12185,12129,12073,
                                     12018,11964,11911,11860,11809,11759,11710,11662,11615,11569,
                                     11524,11480,11437,11394,11353,11313,11274,11236,11199,11163,
                                     11128,11093,11060,11028,10997,10967,10938,10910,10883,10856,
                                     10831,10807,10784,10762,10741,10721,10702,10684,10667,10652,
                                     10637,10623,10610,10598,10587,10578,10569,10561,10555,10549,
                                     10544,10541,10538,10537,10536};

long iodchylka = 0, prop = 0, integ = 0;
long isuma[] = {0, 0, 0};
long i_suma_souc = 0, strida = 0;
unsigned char stav1 = 1, stav2 = 1, faze = 0xFF, spusteno = 0;
unsigned char indexSuma = 0, citac = 0, zmena = 0;
unsigned short ukonceni = 0;
unsigned char index_i1 = 0, index_i2 = 0, index_i3 = 0;

void main()
{
    ANSELA = 0;
    TRISA = 0;
    PORTA = 0;
    
    ANSELB = 0x0E;
    TRISB = 0xFF;
    
    ANSELC = 0;
    TRISC = 0x3F;
    PORTC = 0;
    
    ANSELD = 0;
    TRISD = 0;
    PORTD = 0;
    
    T2CLKCONbits.CS = 0b0001;       // Zdrojem hodin je Fosc/4
    T2CONbits.CKPS = 0b001;         // Prescaler 1:2
    T2CONbits.OUTPS = 0b0000;       // Postscaler 1:1
    T2PR = 0xFE;                    // Perioda 32 us
    T2CONbits.ON = 1;               // Timer2 je zapnut
    PIE4bits.TMR2IE = 0;            // Nepovoleni preruseni
    
    T4CLKCONbits.CS = 0b0001;       // Zdrojem hodin je Fosc/4
    T4CONbits.CKPS = 0b010;         // Prescaler 1:4
    T4CONbits.OUTPS = 0b0000;       // Postscaler 1:1
    T4PR = 85;                      // Perioda 21.50 us
    T4CONbits.ON = 1;               // Timer4 je zapnut
    PIE4bits.TMR4IE = 0;            // Nepovoleni preruseni
    
    CCP1CONbits.MODE = 0b0100;      // Capture mode, sestupna hrana
    CCP1PPS = 0x08;                 // Vstup CCP1 je pin RB0
    CCP1CONbits.EN = 1;             // Jednotka CCP1 je zapnuta
    PIE6bits.CCP1IE = 0;            // Nepovoleni preruseni
    
    CCP2CONbits.MODE = 0b1111;      // PWM mode
    CCP2CONbits.FMT = 1;            // Zarovnani doleva
    CCPTMRSbits.C2TSEL = 0b01;      // Zdrojem hodin je Timer2
    RC6PPS = 0x06;                  // Vystup CCP2 je pin RC6
    CCPR2 = 0;                      // Nulova strida
    CCP2CONbits.EN = 1;             // Jednotka CCP2 je zapnuta
    
    CCPTMRSbits.P3TSEL = 0b01;      // Zdrojem hodin je Timer2
    RD4PPS = 0x07;                  // Vystup PWM3 je pin RD4
    PWM3DC = 0;                     // Nulova strida
    PWM3CONbits.EN = 1;             // Jednotka PWM3 je zapnuta
    
    CCPTMRSbits.P4TSEL = 0b01;      // Zdrojem hodin je Timer2
    RC7PPS = 0x08;                  // Vystup PWM4 je pin RC7
    PWM4DC = 0;                     // Nulova strida
    PWM4CONbits.EN = 1;             // Jednotka PWM4 je zapnuta
    
    ADREFbits.ADNREF = 0;           // Negative reference Vss
    ADREFbits.ADPREF = 0;           // Positive reference Vdd
    ADCLK = 0b011111;               // ADC clock source Fosc/64
    ADCON0bits.ADCS = 0;            // Clock supplied by Fosc
    ADCON0bits.ADFM = 0;            // Zarovnani doleva
    ADCON0bits.ADON = 1;            // Jednotka ADC je zapnuta
    PIE1bits.ADIE = 0;              // Nepovoleni preruseni
    
    INTCONbits.PEIE = 1;
    INTCONbits.GIE = 1;
    
    while (1)
    {
        if (!RB4 && spusteno)
        {
            spusteno = 0;
            RA4 = 1;
        }
        if (!RC5 && !spusteno)
        {
            spusteno = 1;
            CCP1CONbits.EN = 1;
            PIR6bits.CCP1IF = 0;
            PIE6bits.CCP1IE = 1;
            INTCONbits.PEIE = 1;
            INTCONbits.GIE = 1;
            RA5 = 1;
        }
    }
}

void __interrupt() preruseni()
{
    if (PIR6bits.CCP1IF)
    {
        PIR6bits.CCP1IF = 0;
        CCP1CONbits.EN = 0;
        T4TMR = 0;
        PIR4bits.TMR4IF = 0;
        PIE4bits.TMR4IE = 1;
        ADPCH = 0b001010;   // Zmena kanalu na pin RB2
        CCPR2 = 0xFFFF;     // Trvale sepnuta faze 1
        stav1 = 1;
        stav2 = 1;
        zmena = 0;
        faze = 0xFF;
        citac = (CITAC_MAX / 2) + 1;
        index_i2 = (CITAC_MAX / 2) - 1;
        index_i3 = CITAC_MAX / 2;
        ADCON0bits.ADGO = 1;
    }
    if (PIR4bits.TMR4IF)
    {
        PIR4bits.TMR4IF = 0;
        if (PIR1bits.ADIF)
        {
            PIR1bits.ADIF = 0;
            if (stav1 == 1)     // Trvale sepnuta faze 1 (pin RC6, cervena)
            {
                if (zmena)
                {
                    zmena = 0;
                    stav1 = 2;
                    stav2 = 1;
                    ADPCH = 0b001001;   // Zmena kanalu na pin RB1
                    CCPR2 = isuma[CCP2_INDEX];
                    PWM3DC = 0xFFFF;
                    PWM4DC = isuma[PWM4_INDEX];
                    indexSuma = REG_OFF;
                    index_i1 = 0;
                    index_i3 = CITAC_MAX - 1;
                    goto konec_podminek;
                }
                if (stav2)  // Byl zmeren proud faze 2 (pin RB2, modra)
                {
                    stav2 = 0;
                    ADPCH = 0b001011;   // Zmena kanalu na pin RB3
                    if (faze)
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i2];
                    else
                        iodchylka = (long)i_zadany_kladny[index_i2] - (long)ADRES;
                    indexSuma = PWM3_INDEX;
                }
                else        // Byl zmeren proud faze 3 (pin RB3, oranzova)
                {
                    stav2 = 1;
                    ADPCH = 0b001010;   // Zmena kanalu na pin RB2
                    if (faze)
                        iodchylka = (long)i_zadany_kladny[index_i3] - (long)ADRES;
                    else
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i3];
                    indexSuma = PWM4_INDEX;
                }
                index_i2--;
                index_i3++;
            }
            
            else if (stav1 == 2)    // Trvale sepnuta faze 2 (pin RD4, modra)
            {
                if (zmena)
                {
                    zmena = 0;
                    stav1 = 3;
                    stav2 = 1;
                    ADPCH = 0b001001;   // Zmena kanalu na pin RB1
                    CCPR2 = isuma[CCP2_INDEX];
                    PWM3DC = isuma[PWM3_INDEX];
                    PWM4DC = 0xFFFF;
                    indexSuma = REG_OFF;
                    index_i1 = CITAC_MAX - 1;
                    index_i2 = 0;
                    goto konec_podminek;
                }
                if (stav2)  // Byl zmeren proud faze 1 (pin RB1, cervena)
                {
                    stav2 = 0;
                    ADPCH = 0b001011;   // Zmena kanalu na pin RB3
                    if (faze)
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i1];
                    else
                        iodchylka = (long)i_zadany_kladny[index_i1] - (long)ADRES;
                    indexSuma = CCP2_INDEX;
                }
                else        // Byl zmeren proud faze 3 (pin RB3, oranzova)
                {
                    stav2 = 1;
                    ADPCH = 0b001001;   // Zmena kanalu na pin RB1
                    if (faze)
                        iodchylka = (long)i_zadany_kladny[index_i3] - (long)ADRES;
                    else
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i3];
                    indexSuma = PWM4_INDEX;
                }
                index_i1++;
                index_i3--;
            }
            
            else        // Trvale sepnuta faze 3 (pin RC7, oranzova)
            {
                if (zmena)
                {
                    zmena = 0;
                    stav1 = 1;
                    stav2 = 1;
                    ADPCH = 0b001010;   // Zmena kanalu na pin RB2
                    CCPR2 = 0xFFFF;
                    PWM3DC = isuma[PWM3_INDEX];
                    PWM4DC = isuma[PWM4_INDEX];
                    indexSuma = REG_OFF;
                    index_i2 = CITAC_MAX - 1;
                    index_i3 = 0;
                    faze = ~faze;
                    goto konec_podminek;
                }
                if (stav2)  // Byl zmeren proud faze 1 (pin RB1, cervena)
                {
                    stav2 = 0;
                    ADPCH = 0b001010;   // Zmena kanalu na pin RB2
                    if (faze)
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i1];
                    else
                        iodchylka = (long)i_zadany_kladny[index_i1] - (long)ADRES;
                    indexSuma = CCP2_INDEX;
                }
                else        // Byl zmeren proud faze 2 (pin RB2, modra)
                {
                    stav2 = 1;
                    ADPCH = 0b001001;   // Zmena kanalu na pin RB1
                    if (faze)
                        iodchylka = (long)i_zadany_kladny[index_i2] - (long)ADRES;
                    else
                        iodchylka = (long)ADRES - (long)i_zadany_zaporny[index_i2];
                    indexSuma = PWM3_INDEX;
                }
                index_i1--;
                index_i2++;
            }
            
            konec_podminek:
            ADCON0bits.ADGO = 1;
            prop = iodchylka;
            
            integ = iodchylka;
            integ = integ >> 1;
            i_suma_souc = isuma[indexSuma] + integ;
            if (i_suma_souc > STRIDA_MAX)
                i_suma_souc = STRIDA_MAX;
            else if (i_suma_souc < STRIDA_MIN)
                i_suma_souc = STRIDA_MIN;
            
            if (spusteno)
            {
                isuma[indexSuma] = i_suma_souc;
                strida = prop + i_suma_souc;
                if (strida > STRIDA_MAX)
                    strida = STRIDA_MAX;
                else if (strida < STRIDA_MIN)
                    strida = STRIDA_MIN;
                ukonceni++;
                if (ukonceni == KONEC)
                    goto ukoncit;
            }
            else
            {
                isuma[indexSuma] = 0;
                strida = 0;
            }
            
            if (indexSuma == CCP2_INDEX)
                CCPR2 = (unsigned short)strida;
            else if (indexSuma == PWM3_INDEX)
                PWM3DC = (unsigned short)strida;
            else if (indexSuma == PWM4_INDEX)
                PWM4DC = (unsigned short)strida;
            
            citac++;
            if (citac == CITAC_MAX)
            {
                citac = 0;
                zmena = 1;
            }
            
            if (PIR4bits.TMR4IF)
            {
                RA6 = 1;
                goto ukoncit;
            }
        }
        else    // AD prevod nebyl dokoncen
        {
            RA7 = 1;
            ukoncit:
            CCPR2 = 0;
            PWM3DC = 0;
            PWM4DC = 0;
            PIE4bits.TMR4IE = 0;
            INTCONbits.PEIE = 0;
            INTCONbits.GIE = 0;
            isuma[CCP2_INDEX] = 0;
            isuma[PWM3_INDEX] = 0;
            isuma[PWM4_INDEX] = 0;
            ukonceni = 0;
        }
    }
}