/*
Copyright 2003/2004, Freie Universitaet Berlin. All rights reserved.

These sources were developed at the Freie Universität Berlin, Computer
Systems and Telematics group.

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 Freie Universitaet Berlin (FUB) 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 FUB and the contributors on an "as is"
basis, without any representations or warranties of any kind, express
or implied including, but not limited to, representations or
warranties of non-infringement, merchantability or fitness for a
particular purpose. In no event shall FUB or contributors 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.

This implementation was developed by the CST group at the FUB.
Contributors: Enrico Koeppe, Bjoern Lichtblau

For documentation and questions please use the web site
http://www.scatterweb.net and the mailinglist
scatterweb@lists.spline.inf.fu-berlin.de (subscription via the Website).
Berlin, 2003/2004/2005
*/

/** \file camera.c
 ** \ingroup Firmware
 ** \brief Interfacing the camera module.
 **
 **
 ** Control flow goes like this:
 ** 
 ** - handle_PICTURE_REQ_PACKET
 ** - writePictureToEEPROM 
 **     
 **/

#include <stdarg.h>
#include "ScatterWeb.Camera.h"

#define CAMERA_INITIAL          (0x01)     ///< Initial Camera.
#define CAMERA_GET_PICTURE      (0x04)     ///< get Picture.
#define CAMERA_SNAPSHOT         (0x05)     ///< Snapshot.
#define CAMERA_SET_PACKAGE_SIZE (0x06)     ///< set Picture Size.
#define CAMERA_SET_BAUTRATE     (0x07)     ///< set Bautrate
#define CAMERA_RESET            (0x08)     ///< Reset.
#define CAMERA_POWER_OFF        (0x09)     ///< Power off.
#define CAMERA_DATA             (0x0A)     ///< Initial Camera.
#define CAMERA_SYNCH            (0x0D)     ///< Initial Camera.
#define CAMERA_ACK              (0x0E)     ///< Initial Camera.
#define CAMERA_NACK             (0x0F)     ///< Initial Camera.
#define CARG_INITIAL_JPEG_80_64   (0x00070701)
#define CARG_INITIAL_JPEG_160_128   (0x00070703)
#define CARG_INITIAL_JPEG_320_240   (0x00070705)
#define CARG_INITIAL_JPEG_640_480 (0x00070707)
#define CARG_SPS_70               (0x08460000)
#define CARG_GP_JPEG              (0x01000000)
#define CAM_TIMEOUT       (100)
#define CAM_BUF_SIZE      (80)

volatile UINT8 cam_buf[CAM_BUF_SIZE]; ///< The serial ISR writes bytes into this buffer.
volatile int cam_buf_pos;                     ///< Current position in receive buffer ::serial_buffer.
UINT8 cam_com[5];                ///< received commands
UINT8 cam_state;                      ///< State machine.
UINT16 cam_length;                     ///< Size received in date package.
UINT16 cam_pid;                         ///< Next package id to get while downloading images.
UINT32 initialArg;
UINT16 sid;
UINT16 cam_to;

void timeout(UINT16 data); // forward declaration
void camsynch(UINT16 data);
void picture_sender(UINT8 success);

#define CS_OFF            1
#define CS_SYNCH          2
#define CS_SYNCH_ACKED    3
#define CS_INITIAL        4
#define CS_SPS            5
#define CS_SNAPSHOT       6
#define CS_GETPIC         7
#define CS_DATA           8
#define CS_POWEROFF       9
#define CS_SENDING        10
#define CS_IDP            11
#define CS_IDP_READY      12

/// \name Sending fucntions.
//@{

/** Sends on single char, blocking. */
void sendCamera(UINT8 data){     // sends one byte    
  while (!(IFG2 & UTXIFG1));            // loop until USART1 TX buffer ready
  TXBUF1 = data;
}

void sendCameraCommand(UINT8 command, UINT32 arg){  
    sendCamera(0xAA);
    sendCamera(command);
    sendCamera(arg >> 24);
    sendCamera(arg >> 16);
    sendCamera(arg >> 8);
    sendCamera(arg);
}

void sendText(const UINT8* text){
  packet_t packet;
  UINT16 i = 0;
  while(text[i] != '\0') i++;
  i++;
  packet.to = 1; packet.type = MESSAGING_ANS_PACKET;
  packet.header = NULL; packet.header_length = 0;
  packet.data = (UINT8*) text; packet.data_length = i;
  Net_send(&packet, NULL);
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

void writePictureToEEPROM(UINT32 ia){
  if(cam_state != CS_OFF) return;
  // Enable Power for Camera.
  initialArg = ia;
  // Set Uart to 14400 Bps.
  Camera_init(BPS_115200);
  //initCameraBaudrate(BPS_14400);
  cam_state = CS_SYNCH;
  camsynch(0);
}

void camsynch(UINT16 data){
  if(data>80){
    sendText("ERROR Sync > 80");
    cam_state = CS_OFF;
    Data_yellowOff();
    return;
  }
  // Received or currently receiving the SYNCH_ACK?
  if(cam_state != CS_SYNCH || cam_buf_pos > 0){
    //write(term_reply, "Sync ok %i", data);
    //sendText(term_reply);
    Data_yellowOff();
    return;
  }
  sendCameraCommand(CAMERA_SYNCH, 0);
  Timers_add(5, camsynch, data +1);
  Data_yellowToggle();
}

void timeout(UINT16 data){
  switch(cam_state){
    /// TODO 
  }
}

/// Called on bad answer, NACK or timeout.
void retry(){
  /// TODO
}

COMMAND(ca0, 0){ sendCameraCommand(CAMERA_SYNCH, 0); }
COMMAND(ca1, 0){ sendCameraCommand(CAMERA_INITIAL, CARG_INITIAL_JPEG_80_64); }
COMMAND(ca2, 0){ sendCameraCommand(CAMERA_SET_PACKAGE_SIZE, CARG_SPS_70); }
COMMAND(ca3, 0){ sendCameraCommand(CAMERA_GET_PICTURE, CARG_GP_JPEG); }
COMMAND(ca4, 0){ sendCameraCommand(CAMERA_ACK, 0x00000000); }
COMMAND(cr1, 0){ sendCameraCommand(CAMERA_RESET, 0x00000000); } 
COMMAND(cr2, 0){ sendCameraCommand(CAMERA_RESET, 0x000000FF); } 

COMMAND(cdg, 0){
  String_write(term_reply, "pos: %i pid %i st: %i buf: %b %b %b%b%b%b", 
  cam_buf_pos, cam_pid, cam_state, cam_buf[0],cam_buf[1],cam_buf[2],cam_buf[3],cam_buf[4],cam_buf[5]);

}

UINT16 cam_psize;                        ///< Received package size.
/// Serial ISR-Routine for receiving bytes.
interrupt(UART1RX_VECTOR) usart1_rx (void){
  UINT8 temp = RXBUF1;
  //write(term_reply, "rec %b", temp);
  //sendText(term_reply);
  if(cam_buf_pos >= CAM_BUF_SIZE) return; // Buf is full, should not happen.
  if(cam_state<CS_IDP){  // Normal command awaited.
    if(cam_buf_pos==0 && temp!=0xAA) return; // All commands start with 0xAA.
    cam_buf[cam_buf_pos++] = temp; // Story byte and inc pos.  
     if(cam_buf_pos==6){
        // Answer ACK from Camera to our SYNC is treated here because the CAMERA itself requests a SYNC very fast after its ACK.
       if(cam_state==CS_SYNCH){
        if(cam_buf[1]!=CAMERA_ACK || cam_buf[2]!=CAMERA_SYNCH) Data_redOn(); // Other than ACK to Sync?! Should als never happen.
        cam_state = CS_SYNCH_ACKED;       
        cam_buf_pos = 0;
       }
       else {
         cam_com[0] = cam_buf[1];
         cam_com[1] = cam_buf[2];
         cam_com[2] = cam_buf[3];
         cam_com[3] = cam_buf[4];
         cam_com[4] = cam_buf[5];
         cam_buf_pos = 0;
         Timers_add(1, Camera_eventHandler, 0xFFFF);
      }
     }    
  }
  else if(cam_state==CS_IDP){
      cam_buf[cam_buf_pos++] = temp; // Story byte and inc pos.    
      if(cam_buf_pos==4) cam_psize = cam_buf[2] + (cam_buf[3] << 8) + 6; // after 4 bytes read package size. first is package id, second is size
      else if(cam_buf_pos>4 && cam_buf_pos == cam_psize){ // packet complete
        cam_state = CS_IDP_READY; // Now in state CS_DATA for data packet.
        Timers_add(1, Camera_eventHandler, 0xFFFF);
      }  
  }
}

/// RADIO
//////////////////////////////////////////////
void sendDeny(const UINT8* text){
  packet_t packet;
  UINT16 header = 0xFFFF; // means error
  UINT16 i = 0;
  while(text[i] != '\0') i++;
  i++;
  packet.to = 1; packet.type = PICTURE_ANS_PACKET;
  packet.header = (UINT8*) &header; packet.header_length = 2;
  packet.data = (UINT8*) text; packet.data_length = i;
  Net_send(&packet, NULL);
}

void picture_sender(UINT8 success){
  UINT8 buf[64];
  UINT16 offset = sid << 6;
  packet_t packet;
  UINT16 bytes;
  UINT16 header = 0xF0F0;
  if(!success){
    cam_to = 0;
    cam_state = CS_OFF;
    return;
  } 
  if(offset >= cam_length){ 
    //write(term_reply, "all sent %i %i", offset, cam_length);
    //sendText(term_reply); 
    packet.to = cam_to; packet.type = PICTURE_ANS_PACKET;
    packet.header = (UINT8*) &header; packet.header_length = 2; 
    packet.data = NULL; packet.data_length = 0;
    Net_send(&packet, NULL);
    cam_state = CS_OFF;
    return; 
  } // All sent.
  bytes  = cam_length - offset; // Bytes left to send
  if(bytes > 64) bytes = 64; // Truncate into 64b packets
  IO_read(0x1000 + offset, buf, bytes);
  packet.to = cam_to; packet.type = PICTURE_ANS_PACKET;
  packet.header = (UINT8*) &sid; packet.header_length = 2; 
  packet.data = buf; packet.data_length = bytes;
  Net_send(&packet, picture_sender);
  sid++;
}

/// \name Init functions for DCO and Uart.
//@{

/// Initializes UART to defined baud rate, see firmware.h.
/// see http://mspgcc.sourceforge.net/baudrate.html
bool Camera_init(UINT8 bps) { 
  //////////////////////////////////////// RS232
  UCTL1 = CHAR;                         // 8-bit character
  switch(bps){
#if defined(DCO_2458KHZ)
  case BPS_2400  : UTCTL1 = SSEL1; UBR01 = 0x00; UBR11 = 0x04; UMCTL1 = 0x00; break; /* uart1 2457600Hz 2400bps */ // UTCTL1 = SSEL0; UBR01 = 0x0D; UBR11 = 0x00; UMCTL1 = 0x6D; break; // RS232 auf 2400 Bit/s // UCLK = ACLK
  case BPS_14400 : UTCTL1 = SSEL1; UBR01 = 0xAA; UBR11 = 0x00; UMCTL1 = 0x6D; break; /* uart1 2457600Hz 14405bps */                    
  case BPS_19200 : UTCTL1 = SSEL1; UBR01 = 0x80; UBR11 = 0x00; UMCTL1 = 0x00; break; // RS232 auf 19200 Bit/s // UCLK = MCLK // 2,457MHz/19200 = 128 -> 0x80                       
  case BPS_38400 : UTCTL1 = SSEL1; UBR01 = 0x40; UBR11 = 0x00; UMCTL1 = 0x00; break; // RS232 auf 38400 Bit/s UCLK = MCLK  2,457MHz/38400 = 64 -> 0x40                      
  case BPS_57600 : UTCTL1 = SSEL1; UBR01 = 0x2A; UBR11 = 0x00; UMCTL1 = 0x5B; break; // RS232 auf 57600 Bit/s UCLK = MCLK 2,457MHz/57600 = 42.7 -> 0x2A
  case BPS_115200: UTCTL1 = SSEL1; UBR01 = 0x15; UBR11 = 0x00; UMCTL1 = 0x4A; break; // RS232 auf 115200 Bit/s UCLK = MCLK 2,457MHz/115200 = 21.4 -> 0x15 
#elif defined(DCO_4506KHZ)
  case BPS_2400  : UTCTL1 = SSEL1; UBR01 = 0x55; UBR11 = 0x07; UMCTL1 = 0x09; break; /* uart1 4505600Hz 2400bps */ // UTCTL1 = SSEL0; UBR01 = 0x0D; UBR11 = 0x00; UMCTL1 = 0x6D; break; // RS232 with 2400 bps @ 32768 Hz ACLK
  case BPS_14400 : UTCTL1 = SSEL1; UBR01 = 0x38; UBR11 = 0x01; UMCTL1 = 0xEF; break; /* uart1 4505600Hz 14399bps */
  case BPS_19200 : UTCTL1 = SSEL1; UBR01 = 0xEA; UBR11 = 0x00; UMCTL1 = 0x5B; break; /* uart1 4505600Hz 19197bps */   // RS232 with 19200 bps @  Mhz           
  case BPS_38400 : UTCTL1 = SSEL1; UBR01 = 0x75; UBR11 = 0x00; UMCTL1 = 0x52; break; /* uart1 4505600Hz 38378bps */ // RS232 with 38400 bps @  Mhz 
  case BPS_57600 : UTCTL1 = SSEL1; UBR01 = 0x4E; UBR11 = 0x00; UMCTL1 = 0x44; break; /* uart1 4505600Hz 57616bps */  // RS232 with 57600 bps @  Mhz                            
  case BPS_115200: UTCTL1 = SSEL1; UBR01 = 0x27; UBR11 = 0x00; UMCTL1 = 0x10; break; /* uart1 4505600Hz 115232bps */  // RS232 with 115200 bps @  Mhz        
#else
#error No DCO frequency in init_Uart().
#endif
 }
  // init vars
  UINT16 i;
  cam_buf_pos = 0;  
  for(i=0; i<CAM_BUF_SIZE; i++){ cam_buf[i] = 0x00; }
  ME2 |= UTXE1 + URXE1;                 // Enable USART0 TXD/RXD, disabling does not yield any powersavings
  cam_state = CS_OFF;
  IE2 |= URXIE1;                        // Enable USART0 RX interrupt  
  cam_com[0] = 0x00;
  return True;
}
//@}

void Camera_eventHandler(){
  if(cam_com[0] != 0x00){ // Command received
    /// TODO
    if(cam_state>CS_OFF && cam_state<CS_IDP && cam_com[0]==CAMERA_NACK){
    }
    switch(cam_state){
/*      case CS_SYNCH:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_SYNCH) goto error;
        YELLOWOFF;
        sendText("SYNCHED!!");
        cam_state = CS_SYNCH_ACKED;
        break;
  */      
      case CS_SYNCH_ACKED:
        if(cam_com[0]!=CAMERA_SYNCH) goto error;
        sendCameraCommand(CAMERA_ACK, 0x0D000000);
        System_waitA(10); // Perhaps wait a bit?
        //sendText("SYNCHACK!!!");
        sendCameraCommand(CAMERA_INITIAL, initialArg);
        cam_state = CS_INITIAL;
        //sendText("SYNCHACK!!!");
        break;
      case CS_INITIAL:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_INITIAL) goto error;
        sendCameraCommand(CAMERA_SET_PACKAGE_SIZE, CARG_SPS_70);
        cam_state = CS_SPS;
        //sendText("INITIAL!!!");
        break;       
      case CS_SPS:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_SET_PACKAGE_SIZE) goto error;
        //sendCameraCommand(CAMERA_GET_PICTURE, CARG_GP_JPEG);
        //cam_state = CS_GETPIC;
        //sendCameraCommand(CAMERA_SNAPSHOT, 0x00140000); // skip first 20 frames
        sendCameraCommand(CAMERA_SNAPSHOT, 0x00200000); // skip first 32 frames
        cam_state = CS_SNAPSHOT;
        //sendText("SPS!!!");
        break;      
      case CS_SNAPSHOT:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_SNAPSHOT) goto error;
        System_waitA(10); // Perhaps wait a bit?
        sendCameraCommand(CAMERA_GET_PICTURE, CARG_GP_JPEG);
        cam_state = CS_GETPIC;    
        //sendText("SNAPSHOT");  
        break;     
      case CS_GETPIC:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_GET_PICTURE) goto error;
        cam_state = CS_DATA;
        //sendText("GETPIC!!!");
        break; 
      case CS_DATA: 
        if(cam_com[0]!=CAMERA_DATA || cam_com[1]!=0x01) goto error;
        cam_length = cam_com[2] + (cam_com[3]<<8);  
        if(cam_com[4] != 0x00){
          sendText("ERROR Picture too big.");
          cam_state = CS_OFF;
          break;
        }
        cam_pid = 0;
        sendCameraCommand(CAMERA_ACK, 0x00000000); // request packet 0
        cam_state = CS_IDP;
        //write(term_reply, "DATA %i bytes", cam_length);
        //sendText(term_reply);
        break;
      case CS_POWEROFF:
        if(cam_com[0]!=CAMERA_ACK || cam_com[1]!=CAMERA_POWER_OFF) goto error;
        cam_state = CS_SENDING;              
        sid = 0;
        //sendText("POWEROFF!!!");
        picture_sender(1);
        break; 
       default: 
       error:
         String_write(term_reply, "ERROR state: %i command %b%b%b%b%b", cam_state, cam_com[0], cam_com[1], cam_com[2], cam_com[3], cam_com[4]);
          sendText(term_reply);
          cam_state = CS_OFF;
    }
    cam_com[0] = 0x00; // Clear.
  }
  if(cam_state==CS_IDP_READY){
    UINT16 rpid = cam_buf[0] + (cam_buf[1]<<8);
    UINT16 rsize = cam_buf[2] + (cam_buf[3]<<8);
    UINT16 bytes = ((rpid<<6) + rsize);
    // handle line
    IO_write(0x1000 + (rpid<<6), (UINT8*) &cam_buf[4], rsize);
    /*
    if(rpid>290){
      write(term_reply, "[%i,%i,%l,%l]", rpid, rsize, bytes, cam_length);
      sendText(term_reply);
    }*/
    cam_pid++;
    // if last frame
    if(bytes == cam_length){
      UINT16 header = 0xF0F1;
      packet_t packet;
      sendCameraCommand(CAMERA_ACK, 0x0000F0F0);    
      System_waitA(20); // Perhaps wait a bit?
      sendCameraCommand(CAMERA_POWER_OFF, 0x00000000);
      cam_state = CS_POWEROFF;
      cam_buf_pos = 0;
      packet.to = cam_to; packet.type = PICTURE_ANS_PACKET;
      packet.header = (UINT8*) &header; packet.header_length = 2; 
      packet.data = (UINT8*) &cam_length; packet.data_length = 2;
      Net_send(&packet, NULL);
      //write(term_reply, "IDP last: %i %i", bytes, cam_length);
      //sendText(term_reply);
    }
    else {
      sendCameraCommand(CAMERA_ACK, 0x00000000 + ((cam_pid & 0x00FF) << 8) + ((cam_pid & 0xFF00) >> 8)); // request next packet
      cam_buf_pos = 0;
      cam_state = CS_IDP;
    }
  }
}

bool Camera_requestHandler(){
    if(cam_state != CS_OFF){
      sendDeny("Camera is busy!");
    }
  cam_to = rxPacket.from;
  switch(rxPacket.data[0]){
    case 1: writePictureToEEPROM(CARG_INITIAL_JPEG_80_64);   break;
    case 2: writePictureToEEPROM(CARG_INITIAL_JPEG_160_128); break;
    case 3: writePictureToEEPROM(CARG_INITIAL_JPEG_320_240); break; 
    case 4: writePictureToEEPROM(CARG_INITIAL_JPEG_640_480); break;
    default: 
      String_write(term_reply, "Wrong argument %i, 1: 80*64 2: 160*128 3: 320*240 4: 640*480 !", rxPacket.data[0]); // deny
      sendDeny(term_reply);
  }
  return True;
}
