#include <fxlib.h>

#include <stdlib.h>

#include <addresses.h>
#include <Sound4Calc.h>

#include <syscall.h>

//#define DEBUG

//#define PI 3.14159265358

unsigned char *itoa(int n, unsigned char* str, int base)
{
    int i=1, j=0, x;
    if(n<0) str[j++] = '-', n = -n;
    for(x=n;x;x/=base) j++;
    for(x=n;x;x/=base) str[j-i++] = x%base + '0' + 39*(x%base>9);
    str[j] = 0;
    return str;
}

int main(void)
{
    unsigned int key;
    // char buffer[50];
    unsigned char str[20];

    int sleep = 2000;
    int i;
    #ifdef DEBUG
    char before = 0, during = 0, after = 0;
    #endif

    setup();
    //ResetPin();
    while(1)
    {
        Bdisp_AllClr_VRAM();
               
        PrintMini(1, 1, itoa(sleep, str, 10), 0);

        PrintMini(1, 10, itoa(is_SH4, str, 10), 0);

        #ifdef DEBUG
        PrintMini(1, 20, itoa(before, str, 16), 0);
        PrintMini(1, 28, itoa(during, str, 16), 0);
        PrintMini(1, 36, itoa(after, str, 16), 0);
        #endif

        GetKey(&key);

        switch(key)
        {
            case KEY_CTRL_RIGHT : sleep+=50; break;
            case KEY_CTRL_LEFT : sleep-=50; break;
            case KEY_CTRL_EXE :
            while(Keyboard_KeyDown())
            {
            #ifdef DEBUG

            before=*(volatile unsigned char*)SH7305_PJDR;
            ResetPin();
            during=*(volatile unsigned char*)SH7305_PJDR;
            for(i=0;i<sleep;i++);
            SetPin();
            after=*(volatile unsigned char*)SH7305_PJDR;

            #else

            ResetPin();
            for(i=0;i<sleep;i++);
            SetPin();

            #endif
            }
            break;
            case KEY_CTRL_EXIT :
            return 1;
        } 
    }
return 1; // this point is never reached
}

void setup()
{
    is_SH4 = getMPU();

    if(is_SH4)
    {
    // SCIF2 clock on (MSTPCR0.MSTP007) 
        *(volatile unsigned int*)SH7305_MSTPCR0 &= ~0x00000080;
    // switch off SCSMR_2.TE and SCSMR_2.RE 
        *(volatile unsigned short*)SH7305_SCSCR &= ~0x0030;
    // SCIF2 clock off (MSTPCR0.MSTP007)
        *(volatile unsigned int*)SH7305_MSTPCR0 |= 0x00000080;

    // set bit 3 of port U to output mode
        *(volatile unsigned short*)SH7305_PUCR = ( *(volatile unsigned short*)SH7305_PUCR & ~0x00C0 ) | 0x0040;
    // set bit 4 and 5 of port U
        *(volatile unsigned char*)SH7305_PUDR |= 0x0C;

    // set port J bit 2 to output mode  
        *(volatile unsigned short*)SH7305_PJCR = ( *(volatile unsigned short*)SH7305_PJCR & ~0x0030 ) | 0x0010;
    // set port J bit 3 to output mode
        *(volatile unsigned short*)SH7305_PJCR = ( *(volatile unsigned short*)SH7305_PJCR & ~0x00C0 ) | 0x0040;
    }
    else
    {
    // SCIF2 clock on (STBCR3.MSTP31)
        *(volatile unsigned char*)SH7337_STBCR3 &= ~0x02;
    // switch off SCSMR_2.TE and SCSMR_2.RE 
        *(volatile unsigned short*)SH7337_SCSCR2 &= ~0x0030;
    // SCIF2 clock off (STBCR3.MSTP31)
        *(volatile unsigned char*)SH7337_STBCR3 |= 0x02;
    // set bit 6 of port G to output mode
        *(volatile unsigned short*)SH7337_PGCR = ( *(volatile unsigned short*)SH7337_PGCR & ~0x3000 ) | 0x1000;
    // set bit 5 and 6 of port G
        *(volatile unsigned char*)SH7337_PGDR |= 0x60;
    // set port SC bit 0 to output  
        *(volatile unsigned short*)SH7337_SCPCR = ( *(volatile unsigned short*)SH7337_SCPCR & ~0x0003 ) | 0x0001;
    }
    /*
    // set port J bit 2 to output   
    *(unsigned short*)0xA4050110 = ( *(unsigned short*)0xA4050110 & ~0x0030 ) | 0x0010;
    // set port J bit 3 to input    
    *(unsigned short*)0xA4050110 = ( *(unsigned short*)0xA4050110 & ~0x00C0 ) | 0x0080;*/
}

void SetPin()
{
    if(is_SH4)
    {
        *(volatile unsigned char*)SH7305_PJDR |= 0x04;
        *(volatile unsigned char*)SH7305_PJDR &= ~0x08;
        //set pin to 0x4B
    }
    else 
    {
        *(volatile unsigned char*)SH7337_SCPDR |= 0x01;
    }
}

void ResetPin()
{
if(is_SH4)
    {
        *(volatile unsigned char*)SH7305_PJDR &= ~0x04;
        *(volatile unsigned char*)SH7305_PJDR |= 0x08;
        // set the pin to 0x47
    }
    else 
    {

        *(volatile unsigned char*)SH7337_SCPDR &= ~0x01;
    }   
}

char getMPU(void)
{
    // Port L control register.
    volatile unsigned short *plcr = (unsigned short *)0xA4000114;
    // Saved value for PLCR.
    unsigned short saved_plcr;
    unsigned int tested_plcr;

    saved_plcr = *plcr;
    *plcr = 0xffff;

    tested_plcr = *plcr;
    *plcr = saved_plcr;

    if(tested_plcr == 0x00ff || tested_plcr == 0x0fff)
    {
        return 0; // MPU_SH3
    }

    return 1; // MPU_SH4
}