/*
 *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
 * 
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  Revision : $Id$
 *
 */
#include <uart.h>
#include <wait.h>
#include <stdio.h>
#include <i2c_config.h>
#include <i2cm_genric.h>
#include <compat/twi.h>
#include <string.h>

#define SOH 0x01
#define NAK 0x15
#define ACK 0x06
#define EOT 0x04
#define CTRLZ 26

#define BASE_RAMV 0x00000000
#define BASE_RAMT 0x00100000
#define BASE_FLASHA 0x00200000
#define BASE_FLASHB 0x00300000

u08 xmodem_send(u32 memory_address, u32 taille);

void* mem2add(u32 memory_address)
{
    void *pointeur;
    unsigned char *pages = (unsigned char *) (0x1FFF);

    // memory_address 22 bits:
    // 31 downto 22: rien donc 0
    // 21 downto 20: pages (00=ramV, 01=ramT, 10=flashA, 11=flashB)
    // 19 downto 1: adresse physique
    // 0 : poids fort / faible (dans le cas des flash, poids forts et faibles accedent a la meme addresse)

    // "*pages":
    // 7 inutilisé
    // 6 downto 5: pages donc 21 downto 20
    // 4 downto 0: adresse haute donc 19 downto 15

    *pages = (memory_address & 0x003F8000) >> 15;
    pointeur = (void*)((u16)(memory_address & 0x00007FFF) + (u16)0x00008000);
    return pointeur;
}

void mem_fill(u32 memory_address, u08 data, u32 taille)
{
    u08 *buffer;
    u32 cpt;

    for (cpt=0; cpt<taille; cpt++)
    {
        buffer = (unsigned char *)mem2add(memory_address+cpt);
        *buffer = data;
    }
}

void set_cross(u08 pos_x, u08 pos_y)
{
    u08 * cross_x = (u08 *) (0x1F00 + 24);
    u08 * cross_y = (u08 *) (0x1F00 + 25);

    *cross_x = pos_x;
    *cross_y = pos_y;
}

void set_seuils(u08 car, u08 code, u16 hmin, u16 hmax, u16 smin, u16 smax, u16 lmin, u16 lmax)
{
    volatile u08 * select; // B: +4, A: +1
    volatile u16 * data; // B: +49, A: +47
    volatile u08 * write; // B: +5, A: +2
    u08 selection;

    if (car == 'A')
    {
        data   = (volatile u16 *) (0x1F00+47);
        select = (volatile u08 *) (0x1F01);
        write  = (volatile u08 *) (0x1F02);
    } else {
        data   = (volatile u16 *) (0x1F00+49);
        select = (volatile u08 *) (0x1F04);
        write  = (volatile u08 *) (0x1F05);
    }
    
    *write = 0;
    selection = (code << 3) & 0x38;

    *select = selection;
    *data = hmin;
    *write = 1;
    *write = 0;

    selection++;
    *select = selection;
    *data = hmax;
    *write = 1;
    *write = 0;

    selection++;
    *select = selection;
    *data = smin;
    *write = 1;
    *write = 0;

    selection++;
    *select = selection;
    *data = smax;
    *write = 1;
    *write = 0;

    selection++;
    *select = selection;
    *data = lmin;
    *write = 1;
    *write = 0;

    selection++;
    *select = selection;
    *data = lmax;
    *write = 1;
    *write = 0;

}

void start_acq(u08 car, u08 whichram, u08 use_32_bits)
{
    // whichram = 0 : RAMV
    // whichram = 1 : RAMT

    volatile u08 * recorder;
    u08 current_mode=0;

    if (car == 'A')
    {
        recorder = (volatile u08*)0x1F1D;
        current_mode = 0;
    }
    else
    {
        recorder = (volatile u08*)0x1F1C;
        current_mode = 1;
    }

    // Arret d'éventuels demandes d'enregistrement
    *recorder = 0;
    // Attente arret effectif
    while( ((*recorder)&0x03) != 0 );

    // Selection de la bonne RAM
    if (whichram == 0) current_mode |= 2;

    // Activation d'un enregistrement sur 32 bits, si demandé
    if (use_32_bits) current_mode |= 4;

    // Demande transmise au FPGA
    *recorder = current_mode;

    // Attente de l'accusé de réception
    while( ((*recorder)&0x03) != 0x01 );

    // Plus besoin de demander l'enregistrement, c'est parti
    current_mode &= 0xfe;

    // Transmission au FPGA
    *recorder = current_mode;
}

u16 calcrc(unsigned char *ptr, int count)
{
    int crc;
    char i;

    crc = 0;
    while (--count >= 0)
    {
        crc = crc ^ (int) *ptr++ << 8;
        i = 8;
        do
        {
            if (crc & 0x8000)
                crc = crc << 1 ^ 0x1021;
            else
                crc = crc << 1;
        } while(--i);
    }
    return (crc);
}

void set_pixel(u08 ligne, u08 pixel, u08 etat)
{
     // Résolution minimale (comme la croix)
     // 4 pixels par 2 lignes
     u16 add;
     u08 *overlay;
     u08 which;
     
     add = (pixel >> 3) + (ligne << 5);
     printf("(%d)\r\n", add);
     which = pixel&0x07; // Numéro du bit à toucher
     
     overlay = (u08*)(0x2000 + add);
     if (etat == 0)
         *overlay &= ~(1<<which);
     else
         *overlay |= 1<<which;
}

void saa_write(u08 address, u08 subaddress, u08 data)
{
    i2cm_send_start();
    i2c_wait_for_ready();
    i2cm_send_address(address & 0xfe);
    i2c_wait_for_ready();
    i2cm_send_byte(subaddress);
    i2c_wait_for_ready();
    i2cm_send_byte(data);
    i2c_wait_for_ready();
    i2cm_send_stop();
}

u08 saa_read(u08 address, u08 subaddress)
{
    u08 received_byte;
    i2cm_send_start();
    i2c_wait_for_ready();
    i2cm_send_address(address & 0xfe);
    i2c_wait_for_ready();
    i2cm_send_byte(subaddress);
    i2c_wait_for_ready();
    i2cm_send_start();
    i2c_wait_for_ready();
    i2cm_send_address(address | 0x01);
    i2c_wait_for_ready();
    i2cm_want_last_byte();
    i2c_wait_for_ready();
    i2cm_get_received_byte(&received_byte);
    i2c_wait_for_ready();
    i2cm_send_stop();
    return received_byte;
}

void init_acquisition(u08 use_24_bits)
{
    volatile u08 * const mode_saa = (volatile u08*) (0x1F00 + 41);

    // 16 bits
    u08 config16[0x20]={0x00, 0x00, 0xC0, 0x03, 0x00, 0x00, 0x6C, 0x6C, 0x88, 0x01, 0x80, 0x47, 0x40, 0x00, 0x01, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    // 24 bits
    u08 config24[0x20]={0x00, 0x00, 0xC0, 0x03, 0x00, 0x00, 0x6C, 0x6C, 0x88, 0x01, 0x80, 0x47, 0x40, 0x00, 0x01, 0x00, 0x00, 0x0C, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

    u08 *config;
    u08 i;

    if (use_24_bits == 1)
    {
        config = config24;
        *mode_saa = 1;
    }
    else
    {
        config = config16;
        *mode_saa = 0;
    }

    i2cm_init();
    //printf(" i2c initialise\r\n");
    
    for (i=0; i<0x20; i++)
        saa_write(0x48,i,config[i]);

    for (i=0; i<0x20; i++)
        saa_write(0x4A,i,config[i]);

}

int main(void)
{
    u08 i=0;
    u08 k,t;
    u08 zou;
    u32 j=0;
    u16 hmin=0,hmax=0,smin=0,smax=0,lmin=0,lmax=0;
    u08 code=0;

    volatile u08 *entree = (volatile u08*) (0x1F00 + 30);
    volatile u08 *test = (volatile u08*) (0x1F00 + 15);
    volatile u08 *sortie = (volatile u08*) (0x1F00 + 31);

    volatile u08 *access = (volatile u08 *) (0x1FFE);

    volatile u08 * const recorderA = (volatile u08 *) (0x1F1D);
    volatile u08 * const recorderB = (volatile u08 *) (0x1F1C);

    volatile u32 * const rgb = (volatile u32*) (0x1F00 + 11);
    volatile u32 * const hsl = (volatile u32*) (0x1F00 + 11);

    volatile u08 * const test_val = (volatile u08 *) (0x1F00 + 15);
    volatile u08 * const sw_val = (volatile u08 *) (0x1F00 + 16);

    volatile u08 * const color_conf = (volatile u08 *) (0x1F00 + 30);

    volatile u16 * const donneeB1 = (volatile u16 *) (0x1F00 + 2);
    volatile u08 * const fifoB_add = (volatile u08 *) (0x1F00 + 23);
    volatile u08 * const request_fifoB = (volatile u08 *) (0x1F00 + 21);
    volatile u08 * const fifoB_granted = (volatile u08 *) (0x1F00 + 5);
    volatile u08 * const statusfifoB = (volatile u08 *) (0x1F00 + 7);
    volatile u08 * const statuschoixB = (volatile u08 *) (0x1F00 + 8);
    volatile u08 * const codeA = (volatile u08 *) (0x1F00 + 8);
    volatile u08 * const codeB = (volatile u08 *) (0x1F00 + 6);
    volatile u08 * const tailleB = (volatile u08 *) (0x1F00 + 7);
    volatile u08 * const tailleA = (volatile u08 *) (0x1F00 + 9);
    volatile u08 * const codeA_write = (volatile u08 *) (0x1F00 + 17);
    volatile u08 * const codeB_write = (volatile u08 *) (0x1F00 + 10);
    volatile u08 * const codeA_select = (volatile u08 *) (0x1F00 + 26);
    volatile u08 * const codeB_select = (volatile u08 *) (0x1F00 + 27);

    volatile u08 * ram_4k = (volatile u08 *) (0x4000);


    volatile u32 * const lba_sector = (volatile u32*) (0x1F00 + 42);
    volatile u08 * const ata = (volatile u08*) (0x1F00 + 46);
    volatile u16 * const ata_buffer = (volatile u16*) (0x6000);
    volatile u08 * const ata_status = (u08*) (0x1F00 + 31);

    sei();
    uart_init();  
    wait_ms(100);

    // pour faire marcher printf 
    fdevopen((int(*)(char))uart0_send,(int(*)(void))uart0_recv,0);

    printf("Cam-ass-ultra has Booted !\r\n"); 
 
    // Init des SAA et du FPGA pour acquisition en 24 bits
    init_acquisition(1);
    
    //*sw_val = 0x84|0x40;
    *sw_val = 0x08|0x40|0x10;
    *color_conf = 3;

    while(0)
    {
      *sw_val = 0x08|0x40|0x10;
      uart0_send(uart0_recv());
      *sw_val = 0x08|0x40|0x20;
      uart0_send(uart0_recv());
    }

    *sw_val = 0x08|0x40|0x20;
    k=0;
    t=0;
    zou=0;
    while (0) // Pour dessiner au clavier grace à l'overlay
    {
        i = uart0_recv();
        if (i=='2') k++;
        if (i=='8') k--;
        if (i=='4') t--;
        if (i=='6') t++;
        if (i=='5') zou = 1-zou;
        set_pixel(k, t, zou);
        printf("%d , %d\r\n", k,t);
    }
    
    *sw_val = 0;

    set_seuils('A', 0, 0, 255, 0, 255, 2, 1);
    set_seuils('A', 2, 50, 140, 0, 255, 0, 31);
    set_seuils('A', 1, 0, 48, 0, 255, 0, 31);
    set_seuils('A', 3, 0, 255, 0, 255, 2, 1);
    set_seuils('A', 5, 0, 255, 0, 255, 2, 1);
    set_seuils('A', 4, 143, 165, 0, 255, 0, 31);
    set_seuils('A', 6, 29, 48, 0, 255, 100, 32);

    set_seuils('B', 0, 0, 255, 0, 255, 2, 1);
    set_seuils('B', 2, 50, 140, 0, 255, 0, 31);
    set_seuils('B', 1, 0, 48, 0, 255, 0, 31);
    set_seuils('B', 3, 0, 255, 0, 255, 2, 1);
    set_seuils('B', 5, 0, 255, 0, 255, 2, 1);
    set_seuils('B', 4, 143, 165, 0, 255, 0, 31);
    set_seuils('B', 6, 29, 48, 0, 255, 100, 32);

    *tailleB = 0xF3;
    *tailleA = 0xF3;

    for (i=0; i<64; i++)
    {
        *codeB_select = i;
        *codeB = 0x77;
        *codeB_write = 1;
        *codeB_write = 0;
    }

    for (i=0; i<8; i++)
    {
        *codeA_select = i;
        *codeA = 0x77;
        *codeA_write = 1;
        *codeA_write = 0;
    }

    while (1) // Pour régler les seuils vite fait
    {
        i = uart0_recv();
        if (i=='4') *sw_val = 0x85;
        if (i=='5') *sw_val = 0x86;
        if (i=='6') *sw_val = 0x87;
        if (i=='0') *sw_val = 0x84;
        if (i=='q') hmin++;
        if (i=='w') hmin--;
        if (i=='s') hmax++;
        if (i=='x') hmax--;
        if (i=='d') smin++;
        if (i=='c') smin--;
        if (i=='f') smax++;
        if (i=='v') smax--;
        if (i=='g') lmin++;
        if (i=='b') lmin--;
        if (i=='h') lmax++;
        if (i=='n') lmax--;
        if (i=='1') code++;
        if (i=='2') code--;
        set_seuils('B', code, hmin, hmax, smin, smax, lmin, lmax);
        printf("%d:  %d-%d  %d-%d  %d-%d\r\n", code, hmin, hmax, smin, smax, lmin, lmax);
    }

    *sw_val = 3;
    *ata = 4;
    wait_ms(3000);
    *ata = 0;
    
    do {
       i=0;
        do {
            *(ata_buffer+i) = (u16)i;
            i++;
        } while (i != 0);

        i=0;
        do {
            printf("%04x", (volatile u16)*(ata_buffer+i));
            if (!((i+1)%16)) printf("\r\n");
            i++;
        } while (i != 0);
        printf("\r\n");

        printf("!%d!\r\n", *ata_status);

        *lba_sector = 40 | ((u32)1<<28);
        *ata = 1;
        *ata = 0;

        k = *ata_status;
        t = *ata_status;
        i=0;
        while((*ata_status&0x01) == 1) i++;
        printf(":%d(%d).\r\n", *ata_status, i);

        printf("%d - %d.\r\n", k ,t);
    
        i=0;
        do {
            printf("%04x", (volatile u16)*(ata_buffer+i));
            if (!((i+1)%16)) printf("\r\n");
            i++;
        } while (i != 0);
        printf("\r\n");
        uart0_send(uart0_recv());
    } while(1);

    // On libère l'accès aux RAM
    *access = 0;
    // Lancement d'une acquisition sur la camera B, dans la RAMV en RGB24bits
    start_acq('B', 0, 1);
    // on attend qu'il ait fini
    while(((*recorderB)&0x03) != 0);
    // On demande l'accès aux RAM
    *access = 1;
    xmodem_send(BASE_RAMV, (u32)286*720*4); // une trame complete

    printf("Send complete.\r\n");


    ram_4k = (volatile u08*)(0x1F00+32);
    zou=0;
    t=0;
    while(1)
    {
        k = uart0_recv();
        if (k == '0')
        {
              t=3;
        }
        else if (k == '1')
        {
             t=0x87;
        }
        else if (k == '7')
        {
             t=0x0;
        }
        else if (k == '8')
        {
             t=0x1;
        }
        else if (k == '9')
        {
             t=0x2;
        }
        else if (k == '4')
        {
             t=0x84;
        }
        else if (k == '5')
        {
             t=0x85;
        }
        else if (k == '6')
        {
             t=0x86;
        }
        else if (k == '2')
        {
             t=0x48;
        }
        else if (k == '/')
        {
             ram_4k++;
        }
        else if (k == '*')
        {
             ram_4k--;
        }
        else if (k == '+')
        {
             zou++; 
        } else {
             zou--;
        }
        printf("%d (%02x) @ %04x\r\n", zou, t, (u16)ram_4k);
        *ram_4k = zou;
        *sw_val = t|0x40;
    }

    while(1)
    {
        *sw_val=0;
        uart0_send(uart0_recv());
        *sw_val=1;
        uart0_send(uart0_recv());
        *sw_val=2;
        uart0_send(uart0_recv());
        *sw_val=3;
        uart0_send(uart0_recv());

        *sw_val=0x84;
        uart0_send(uart0_recv());
        *sw_val=0x85;
        uart0_send(uart0_recv());
        *sw_val=0x86;
        uart0_send(uart0_recv());
        *sw_val=0x87;
        uart0_send(uart0_recv());
        uart0_send('.');
    }
        
    while(1)
    {
        *fifoB_add = 0;
        printf("%i!\r\n", *fifoB_granted);
        *request_fifoB = 1;
        while(*fifoB_granted == 0)
        {
            printf("(%02x) ", *statusfifoB);
            wait_ms(100);
        }

        set_cross(125, 72);

        printf("%i (%02x)!\r\n", *fifoB_granted, *statusfifoB);
        uart0_send('=');
        for (k=0; k<20; k++)
        {
            printf("%02x: ",k);
            for (i=0; i<4; i++)
            {
                t = i+4*k;
                *fifoB_add = t;
                printf("%03i ", *donneeB1);
            }
            printf("\r\n");
        }
        *request_fifoB = 0;
        for (i=0; i<20; )
        {
            k = *statusfifoB;
            t = *statuschoixB;
            if (k!=0)
            {
                i++;
                //printf("%02x(%02x)_", k,t);
            }
        }
        printf("\r\n");
        wait_ms(1000);
    }


    while (1)
        uart0_send(uart0_recv());

    return 0;
}

u08 xmodem_send(u32 memory_address, u32 taille)
{
    unsigned char *buffer;
    u08 i;
    u08 bloc_num;
    unsigned char octet;
    u16 crc;
    u32 nombre;
    unsigned char buf2[128];

    printf("Waiting to send %lu bytes\r\n", taille);

    if (uart0_recv() != 'C') return 1;

    bloc_num = 1;
    nombre = 0;

    do
    {
       uart0_send(SOH);
       uart0_send(bloc_num);
       uart0_send(255-bloc_num);
       for (i=0; i<128; i++)
       {
           if (nombre <= taille)
           {
               buffer = (unsigned char *)mem2add(memory_address);
               octet = *buffer;
           } else {
               octet = CTRLZ;
           }
           memory_address++;
           uart0_send(octet);
           buf2[i] = octet;
           nombre++;
       }
       crc = calcrc(buf2,128);
       uart0_send((u08)(crc>>8));
       uart0_send((u08)(crc & 0x00FF));

       octet = uart0_recv();
       if (octet == ACK)
       {
           bloc_num++;
           //if (bloc_num == 255) bloc_num=1;
       } else {
           memory_address -= 128;
           nombre -= 128;
       }
    } while (nombre <= taille);
    uart0_send(EOT);
    uart0_recv();
    return 0;
}