#include "commun.h"

int gblAncCroche = 0, gblAncAngle = -1;

#define CENTRE_ECRAN_X        (120)
#define CENTRE_ECRAN_Y        (80)

int SIN(int angle)
{
        if (angle > 360)
                angle = angle % 360;
        else if (angle < 0)
                angle = (angle % 360) + 360;
        return sinus[angle];
}

int COS(int angle)
{
        if (angle > 360)
                angle = angle % 360;
        else if (angle < 0)
                angle = (angle % 360) + 360;
        return cosinus[angle];
}

void GerePersosRotation(SONIC *sonic, int angle)
{
        u32 i;
        int x, y, j;
        unsigned short dehors_ecran;
        unsigned int msg;
        int diffX, diffY;

        nSpritesSup = 0;

        //Prend beaucoup de temps car gère des persos inutiles.
        //Optimisable, en ne gérant que le nombre d'objets actifs.
        if (nbrePersos <= 0)
                return;
        for (j = nbrePersos - 1; j >= 0; j--)
        {
                i = id_perso[j];
                if (j < 0 || j >= 576)
                        Debug("Erreur grave #1! (ID %i,%i)", persos[i].noObjet, i, j);
                gblJ = j;

                x = persos[i].x + (persos[i].largeur >> 1), y = persos[i].y + (persos[i].hauteur >> 1);
                diffX = x - (sonic->x + 12);
                diffY = y - (sonic->y + 12);

                persos[i].x = ((CENTRE_ECRAN_X << 8) + cosinus[angle] * diffX + sinus[angle] * diffY) >> 8;
                persos[i].y = ((CENTRE_ECRAN_Y << 8) + cosinus[angle] * diffY - sinus[angle] * diffX) >> 8;
                persos[i].x -= persos[i].largeur, persos[i].y -= persos[i].hauteur;

                if (persos[i].fHandler == NULL)
                {
                        Message(0, "Erreur grave #2! (ID %i,%i,%i)", persos[i].noObjet, i, j);
                        Message(1, "%i %i %i", objetsUtilises, persos[i].x, persos[i].y);
                        while (1);
                }

                persos[i].fHandler(sonic, &persos[i], MSG_AFFICHER);
                persos[i].x = x - (persos[i].largeur >> 1), persos[i].y = y - (persos[i].hauteur >> 1);
        }
}

u32 TAILLE_X = 32, TAILLE_Y = 32;
u16 *Map_base = (u16 *) Stage01_map;

void RotationBG2(int scrollX, int scrollY, int centreX, int centreY, int angle)
{
        int _cos;
        int _sin;

        if (angle < 0)
                angle += 360;
        if (angle >= 360)
                angle -= 360;
        _cos = cosinus[angle];
        _sin = sinus[angle];

        REG_BG2X = (-(scrollY) * _sin - (scrollX) * _cos) + (centreX << 8);
        REG_BG2Y = (-(scrollY) * _cos + (scrollX) * _sin) + (centreY << 8);
        REG_BG2PA = _cos;
        REG_BG2PB = _sin;
        REG_BG2PC = -_sin;
        REG_BG2PD = _cos;
}

void CopieMap_16x16()
{
        u32 x, y, num;
        u16 *map = (u16 *) ScreenBaseBlock(22);

        for (y = 0; y < TAILLE_Y; y++)
        {
                map = (u16 *) (ScreenBaseBlock(22) + 128 * 2 * y);
                for (x = 0; x < TAILLE_X; x++)
                {
                        num = (*(Map_base + TAILLE_X * y + x)) << 2;
                        *map = (num + ((num + 1) << 8));
                        *(map + 64) = (num + 2 + ((num + 3) << 8));
                        map++;
                }
        }
}

int diffAngle(int a1, int a2)
{
        while (ABS(a1 - a2) > 180)
        {
                if (a1 < a2)
                        a2 -= 360;
                else
                        a2 += 360;
        }

        return a2 - a1;
}

void Sonic_sscroche(SONIC *s, int angle, int diffangle)
{

        /*    if (s->saut>1)
        return;*/

        //Donne à Sonic un élan lorsqu'il touche un sol incliné
        s->vx += vDIV(s->vy * SIN(diffangle));

        s->vy = 0;
        s->saut = 0;

        //*2.84 (256/90)
        //    s->vx=(diffangle*728*4)>>8;
        s->vangle = diffangle;
        s->angle = angle;
        s->croche = 1;

        /*    //Pour éviter la remise à zéro de vx
    if (!gblAncCroche)
        gblAncAngle=s->angle;*/

        //    Debug("%i",s->vx);
}

void GereCollAngle(SONIC *s, int angle, int anglePF)
{
        int diff, vx;

        //Sonic se tape contre un mur (et pas en saut) -> vx=0
        if (diffAngle(anglePF, s->angle) && s->angle != 360)
        {

                //        Debug("ICI");
                s->vx = 0;
        }

        diff = diffAngle(angle, anglePF);
        if (ABS(diff) < 90)
                Sonic_sscroche(s, anglePF, diff);

        /*    else if (s->vy<0)        {
        Debug("ICI");
    }*/

        /*    else if (s->vy<0)        {
        if (!gblAncCroche)          {
//            Debug("IICI %i %i %i %i",s->vy,diff,vDIV(s->vy*SIN(diff)),s->vx);
            Sonic_sscroche(s,anglePF,diff);
            gblAncAngle=s->angle;
//            Debug("%i",s->vx);
        }

        gblAncCroche=1;
        s->saut=1;
        if (ABS(s->vx)<vMUL(1)/2)
            s->vy=vMUL(1);
        else    {
//        if (ABS(s->vx)>=vMUL(1))        {
            vx=s->vx;
            Sonic_sscroche(s,anglePF,diff);
            s->vx=vx;
//            Debug("%i",s->vx);
            s->vy=-vMUL(1);
        }
//        }
    }*/

        /*    if (ABS(diffAngle(anglePF,s->angle))>=90)
        Debug("ICI!! %i %i %i",s->angle,anglePF,s->vy);*/

        //        s->vx=0;
        //    else if (s->vy<0)
        //        s->vy=0;
}

#define Collision(x, y) Map_base[TAILLE_X * (y) + (x)]

void MoteurSonic(SONIC *s, int angle)
{
        int dx, dy;
        u32 i, iterations;
        int augVitesse;
        static u16 TOUCHES, ancienRegTouches = ~0;

        Message(0, "%i %i %i %i %i", s->vx, s->vy, s->croche, s->saut, s->vangle, s->angle);

        TOUCHES = REG_TOUCHES &~ancienRegTouches;
        ancienRegTouches = REG_TOUCHES;
        if (TOUCHES & TOUCHE_A && s->croche)
        {
                s->croche = 0;
                s->angle = 360;
                s->vy = -vMUL(5);
                s->vx = 0;

                /*        s->vy=(-ABS(COS(s->vangle)*vMUL(5)))>>8;
        s->vx=(SIN(s->vangle)*vMUL(5))>>8;*/

                //        s->vx=-s->vx;
                s->saut = 20;
                s->vangle = 0;
        }

        if (REG_TOUCHES & TOUCHE_GAUCHE)
                s->A = -1;
        else if (REG_TOUCHES & TOUCHE_DROITE)
                s->A = 1;
        else
                s->A = 0;

        if (s->croche)
        {
                augVitesse = (s->A * ABS(COS(s->vangle)) + G * SIN(s->vangle)) / DIV;
                s->vx += augVitesse;
        }
        else
                s->vx += s->A * 16;

        if (s->saut > 1)
                s->saut--;

        if (!s->croche && !s->saut && gblAncCroche)
        {
                if (s->vx)
                        s->vy += ABS(s->vx);
                s->vx = 0;
                s->angle = 0;
                gblAncAngle = 0;
        }

        //Lissage du croche pour éviter les problèmes
        if (s->croche)
                gblAncCroche = 3;
        if (gblAncCroche)
                gblAncCroche--;

        /*    if (s->angle!=gblAncAngle)      {
        //Changement de l'angle -> plus de vitesse horizontale
        s->vx=0;
//        Debug("#3 %i %i",gblAncAngle,s->angle);
        gblAncAngle=s->angle;
    }*/
        s->croche = 0;
        s->vy += vMUL(1) / 8;
        if (s->vy >= vMUL(8))
                s->vy = vMUL(8);

        if (REG_TOUCHES & TOUCHE_SELECT)
        {
                s->vy = 0;
                s->vx = 0;
                if (REG_TOUCHES & TOUCHE_GAUCHE)
                        s->vx = vMUL(-1);
                else if (REG_TOUCHES & TOUCHE_DROITE)
                        s->vx = vMUL(1);
                else if (REG_TOUCHES & TOUCHE_HAUT)
                        s->vy = vMUL(-1);
                else if (REG_TOUCHES & TOUCHE_BAS)
                        s->vy = vMUL(1);
        }

        if (s->saut || !gblAncCroche)
        {
                dx = vDIV(COS(angle + 90) * s->vy + COS(angle) * s->vx);
                dy = vDIV(SIN(angle + 90) * s->vy + SIN(angle) * s->vx);
        }
        else
        {
                dx = vDIV(COS(angle + 90) * s->vy + COS(s->angle) * s->vx);
                dy = vDIV(SIN(angle + 90) * s->vy + SIN(s->angle) * s->vx);
        }

        s->x += vDIV(dx);
        s->y += vDIV(dy);
        s->virgule_x += dx & (UNITE - 1);
        s->virgule_y += dy & (UNITE - 1);

        if (s->virgule_x > vMUL(1))
        {
                s->virgule_x -= vMUL(1);
                s->x++;
        }

        if (s->virgule_x < -vMUL(1))
        {
                s->virgule_x += vMUL(1);
                s->x--;
        }

        if (s->virgule_y > vMUL(1))
        {
                s->virgule_y -= vMUL(1);
                s->y++;
        }

        if (s->virgule_y < -vMUL(1))
        {
                s->virgule_y += vMUL(1);
                s->y--;
        }

        for (i = 9;; i += 16)
        {
                if (i > s->hauteur)
                        i = s->hauteur - 9;

                //Détection à gauche (angle=270?)
                if (Collision((s->x + s->largeur) >> 4, (s->y + i) >> 4))
                {
                        s->croche = 1;
                        s->x = (s->x + s->largeur) &~15;
                        s->x -= s->largeur;
                        GereCollAngle(s, angle, 270);
                }

                //Détection à droite (angle=90?)
                if (Collision(s->x >> 4, (s->y + i) >> 4))
                {
                        s->croche = 1;
                        s->x = (s->x &~15) + 15;
                        GereCollAngle(s, angle, 90);
                }

                if (i == s->hauteur - 9)
                        break;
        }

        for (i = 9;; i += 16)
        {
                if (i > s->largeur)
                        i = s->largeur - 9;

                //Détection en bas (angle=0)
                if (Collision((s->x + i) >> 4, (s->y + s->hauteur) >> 4))
                {
                        s->y = (s->y + s->hauteur) &~15;
                        s->y -= s->hauteur;
                        s->croche = 1;
                        GereCollAngle(s, angle, 0);
                }

                //Détection en haut (angle=180)
                if (Collision((s->x + i) >> 4, s->y >> 4))
                {
                        s->y = (s->y &~15) + 15;
                        s->croche = 1;
                        GereCollAngle(s, angle, 180);
                }

                if (i == s->largeur - 9)
                        break;
        }

        /*    iterations=0;                       //Au moins une itération
    while(iterations<1 || ABS(s->virgule_x)>vMUL(1) || ABS(s->virgule_y)>vMUL(1))        {
        iterations++;
        if (s->virgule_x>vMUL(1))       {
            s->virgule_x-=vMUL(1);
            s->x++;
        }
        if (s->virgule_x<-vMUL(1))       {
            s->virgule_x+=vMUL(1);
            s->x--;
        }
        if (s->virgule_y>vMUL(1))       {
            s->virgule_y-=vMUL(1);
            s->y++;
        }
        if (s->virgule_y<-vMUL(1))       {
            s->virgule_y+=vMUL(1);
            s->y--;
        }

        for (i=2;;i+=16)            {
            if (i>s->hauteur)
                i=s->hauteur-2;
            //Détection à gauche (angle=270?)
            if (Collision((s->x+s->largeur)>>4,(s->y+i)>>4))            {
                s->croche=1;
                s->x=(s->x+s->largeur)&~15;
                s->x-=s->largeur;
                GereCollAngle(s,angle,270);
            }
            //Détection à droite (angle=90?)
            if (Collision(s->x>>4,(s->y+i)>>4))            {
                s->croche=1;
                s->x=(s->x&~15)+15;
                GereCollAngle(s,angle,90);
            }

            if (i==s->hauteur-2)
                break;
        }

        for (i=2;;i+=16)            {
            if (i>s->largeur)
                i=s->largeur-2;
            //Détection en bas (angle=0)
            if (Collision((s->x+i)>>4,(s->y+s->hauteur)>>4))            {
                s->y=(s->y+s->hauteur)&~15;
                s->y-=s->hauteur;
                s->croche=1;
                GereCollAngle(s,angle,0);
            }
            //Détection en haut (angle=180)
            if (Collision((s->x+i)>>4,s->y>>4))            {
                s->y=(s->y&~15)+15;
                s->croche=1;
                GereCollAngle(s,angle,180);
            }

            if (i==s->largeur-2)
                break;
        }
    }
*/
}

void Special_stage()
{
        u32 i, zero = 0, CPU = 0;
        s32 angle, angle2;
        SONIC s;

        fade = 24;
        GereFadeBlanc();
        REG_BLDMOD = 0x3fbf;
        REG_COLEY = 31;

        //Reparamétrage des interruptions
        DesactiveInterruptions();

        //Reinitialisation partielle de la VRAM
        _memset((void *) ScreenBaseBlock(0), &zero, 98304);
        _memset((void *) Palette, &zero, 512 * 2);

        // Configure les différents plans
        REG_DISPCNT = (VIDEO_MODE1 | VIDEO_BG1_ENABLED | VIDEO_BG2_ENABLED | OBJ_ENABLED | 0x40);
        REG_BG1CNT = BG_TEXTE_256x256 | BG_16_PALETTES | BG_WRAP | 30 << 8 | 0 | BG_CHARBLOCK_2;
        REG_BG2CNT = BG_ROTATION_1024x1024 | BG_1_PALETTE | 22 << 8 | 2;
        REG_BG1VOFS = 0, REG_BG1HOFS = 0;

        //Chargement des graphismes
        CopieDMA((void *) charset_tiles, (void *) CharBaseBlock(2), sizeof(charset_tiles) >> 1, DMA_16im);
        ChargePalette(patterns_ss_palette, Palette, 64);
        CopieDMA((void *) patterns_ss_tiles, (void *) CharBaseBlock(0), sizeof(patterns_ss_tiles) >> 1, DMA_16im);
        CopieMap_16x16();

        //Pour le texte
        Palette[0xf1] = RGB(31, 31, 31);
        ChargePalette(Pal_0_palette, nPalObj(0), 16);
        ChargePalette(Pal_2_palette, nPalObj(2), 16);

        //Initialisation
        angle = 0;
        nbrePersos = super_vitesse = 0;
        InitSonic(&s, 16, 16, 24, 24, 0);
        s.x = 16 * 8, s.y = 16 * 13;
        CreePerso(13 * 16, 19 * 16, 1, 16, 16, hrBague, 0);
        fade = 0;

        //Pour les interruptions
        CopieDMA(Palette, PaletteNormale, 512, DMA_16im);

        CopieDMA(PaletteNormale, PaletteNormaleSauv, 512, DMA_16im);
        for (i = 0; i < 512; i++)
                PaletteNormale[i] = RGB(31, 31, 31);
        CopieDMA(PaletteNormale, Palette, 512, DMA_16im);
        fade = -24;

        initInterrupt();
        REG_INTERRUPT = (u32) & SSInterruptHandler;
        REG_IME = 1;

        while (1)
        {
                InitSprites();
                angle2 = 360 - angle;
                if (angle2 < 0)
                        angle2 += 360;
                if (angle2 >= 360)
                        angle2 -= 360;
                MoteurSonic(&s, angle2);
                CreeObjetStandardSansEcran(120 - 12, 80 - 12, AjSprite(Pal_0_32x32_tiles + 16 * 32, 32 * 32), 0, TAILLE_32x32);
                GerePersosRotation(&s, angle2);

                //Consommation CPU
                i = UtilCPU();
                if (i >= CPU - 5 && i <= CPU)
                        i = CPU;
                else
                        CPU = i;
                printf_xy(17, 0, "CPU  : %02i.%i%%  ", i / 10, i % 10);
                while (*CompteurScanline >= 148);
                while (*CompteurScanline < 148);
                NouvelleTrame();
                while (*CompteurScanline < 160);

                rotData[1].pa = cosinus[angle2];
                rotData[1].pb = -sinus[angle2];
                rotData[1].pc = sinus[angle2];
                rotData[1].pd = cosinus[angle2];

                //centreX et centreY -> positions de Sonic
                RotationBG2(120, 80, s.x + 12, s.y + 12, angle);
                CopieSprites();
                CopieDMA(MonOAM, pOAM, 1024 >> 2, DMA_32im);

                //        Debug("%i",nSpritesSup);
                nTimer++;
                if (nTimer % 2 == 0)
                        angle += 1;
                if (angle < 0)
                        angle += 360;
                if (angle >= 360)
                        angle -= 360;

                if (fade < 0)
                {
                        REG_BLDMOD = 0;
                        GereFadeBlanc();
                }

                if (REG_TOUCHES & TOUCHE_START)
                {
                        fade = 24;
                        GereFadeBlanc();
                        fade = 24;
                        CopieDMA(Palette, PaletteNormaleSauv, 512, DMA_16im);
                        GereFade();
                        while (REG_TOUCHES & TOUCHE_START);
                        break;
                }
        }
}