/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <string.h>

#include "../headers/system_config.h"

#include "../headers/CircBufPacket.h"
#include "../headers/FileSystem.h"
#include "../headers/SystemUtility.h"

#include "Sound.h"

namespace Motion {

WavFile::WavFile()
{
  data = NULL;
  dataSize = 0;
}

WavFile::~WavFile()
{
  if(data!=NULL)
    DeleteLarge(data);
}

bool
WavFile::load(const char *filename) {
  HFS::FILE *file;

  file = HFS::fopen(filename,"rt");
  if(!file)
    return false;

  if(data==NULL)
    data = NewLarge(&data,MaxDataSize);
  if(data==NULL)
    return false;

  static const int ReadSize = 256;
  int read_cnt = 0;
  char *dest = data;
  while((ulong)(dest - data + ReadSize) < MaxDataSize &&
        (read_cnt=HFS::fread(dest,1,ReadSize,file))>0) {
    dest += read_cnt;
  }
  dataSize = dest - data;

  HFS::fclose(file);

  return parseData();
}

bool
WavFile::parseData() {
  char *parse_loc;
  char *data_start=NULL;
  ulong data_size=0UL;

  parse_loc = data;
  // check header
  if(strncmp(parse_loc,"RIFF",4)!=0) {
    pprintf(TextOutputStream,"Not RIFF file.\n");
    return false;
  }
  parse_loc += 4;

  ulong length;
  memcpy(&length,parse_loc,4);
  parse_loc+=4;

  pprintf(TextOutputStream,"length=%lu dataSize=%lu\n",length,dataSize);
  length = dataSize - 8; // shouldn't need this

  if(strncmp(parse_loc,"WAVE",4)!=0) {
    pprintf(TextOutputStream,"Not WAVE file\n");
    return false;
  }
  parse_loc+=4;
  length-=4;

  while(length > 8) {
    ulong chunk_size;
    char *chunk_start;

    memcpy(&chunk_size,parse_loc+4,4);
    chunk_start = parse_loc+8;
    if(strncmp(parse_loc,"fmt ",4)==0) {
      // format chunk

      static const ushort FormatPCF    = 1;
      static const ushort ChannelsMONO = 1;

      ushort format;
      memcpy(&format,chunk_start,2);
      if(format != FormatPCF) {
        pprintf(TextOutputStream,"not PCF format\n");
        return false;
      }

      ushort channels;
      memcpy(&channels,chunk_start+2,2);
      if(channels!=ChannelsMONO) {
        pprintf(TextOutputStream,"not MONO\n");
        return false;
      }

      ulong sample_rate;
      memcpy(&sample_rate,chunk_start+4,4);
      if(sample_rate != 8000) {
        pprintf(TextOutputStream,"not sampled at 8KHz\n");
        return false;
      }

      // skip datasize/sec (long), block size (short)

      ushort bits_per_sample;
      memcpy(&bits_per_sample,chunk_start+14,2);
      if(bits_per_sample != 8) {
        pprintf(TextOutputStream,"not 8bits per sample\n");
        return false;
      }
    } else if(strncmp(parse_loc,"data",4)==0) {
      data_start = chunk_start;
      data_size  = chunk_size;
    }

    parse_loc += chunk_size+8;
    length    -= chunk_size+8;
  }

  if(data_start==NULL) {
    pprintf(TextOutputStream,"no data found\n");
    return false;
  }

  memmove(data,data_start,data_size);
  dataSize = data_size;

  for(char *loc=data; loc<data+data_size; loc++) {
    *loc = *loc ^ 0x80; // convert unsigned to signed, [0,255]->[-128,127]
  }

  return true;
}

Sound::Sound()
{
}

void
Sound::init() {
  for(int sound_idx=0; sound_idx<NUM_SOUND_FILES; sound_idx++)
    valid[sound_idx]=false;
  sound_frequency = 0;
  sound_duration  = 0;
  sound_time = 0;

  for(int k=0; k<256; k++){
    sin8[k] = (char)(127.0 * sin(2*M_PI * (double)k / 256));
  }
  sampling_rate = 8000;

  sound_file = 0;
  repeat_file = false;
}

void
Sound::setSpeakerSamplingRate(int sample_rate) {
  sampling_rate = sample_rate;
}

void
Sound::load() {

    valid[SOUND_FILE_TEST] = sounds[SOUND_FILE_TEST].load("/MS/sounds/test.wav");
    
    valid[SOUND_FILE_SQUARE   ] = sounds[SOUND_FILE_SQUARE   ].load("/MS/sounds/Square.wav"  );
    valid[SOUND_FILE_RECTANGLE] = sounds[SOUND_FILE_RECTANGLE].load("/MS/sounds/Rectangl.wav");
    valid[SOUND_FILE_T        ] = sounds[SOUND_FILE_T        ].load("/MS/sounds/T.wav"       );
    valid[SOUND_FILE_L        ] = sounds[SOUND_FILE_L        ].load("/MS/sounds/L.wav"       );
    valid[SOUND_FILE_TRIANGLE ] = sounds[SOUND_FILE_TRIANGLE ].load("/MS/sounds/Triangle.wav");
    valid[SOUND_FILE_TADA     ] = sounds[SOUND_FILE_TADA     ].load("/MS/sounds/ppg.wav");

}

void
Sound::setCommand(MotionCommand &ncmd) {
  switch(ncmd.sound_cmd) {
  case Motion::SOUND_NONE:
    break;
  case Motion::SOUND_NOTE:
    {
      // reset clock on duration if necessary
      if(sound_cmd       != SOUND_NOTE ||
         sound_frequency != ncmd.sound_frequency)
        sound_time = 0;

      sound_cmd = SOUND_NOTE;

      sound_frequency = ncmd.sound_frequency;
      sound_duration  = (int)(((double)ncmd.sound_duration/1.0e6) * sampling_rate);
    }
    break;
  case Motion::SOUND_FILE:
    {
      if(ncmd.sound_file < NUM_SOUND_FILES &&
         valid[ncmd.sound_file]) {
        if(sound_cmd == SOUND_FILE &&
           sound_file == ncmd.sound_file) {
          // if already playing it, just record we want to repeat
          repeat_file = true;
        } else {
          sound_cmd = SOUND_FILE;
          sound_file = ncmd.sound_file;
          repeat_file = false;
          
          sound_time = 0;
        }
      }
    }
    break;
  }
}

void
Sound::getSoundData(char *data,int data_size) {
  int k;
  unsigned int sin_idx;
  double sin_idx_r;

  switch(sound_cmd) {
  case Motion::SOUND_NONE:
    for(k=0; k<data_size; k++) data[k] = 0; // rand()%256 - 128;
    break;
  case Motion::SOUND_NOTE:
    for(k=0; k<data_size && sound_duration>0; k++, sound_duration--){
      sin_idx_r = (sound_time * (double)sound_frequency*256) / sampling_rate;
      sin_idx_r = fmod(sin_idx_r,256.0);
      sin_idx = (unsigned int)sin_idx_r;
      data[k] = sin8[sin_idx & 0xFF];
      sound_time++;
    }
    if(sound_duration==0){
      sound_cmd = Motion::SOUND_NONE;
      for(; k<data_size; k++)
        data[k] = 0;
    }
    break;
  case Motion::SOUND_FILE:
    {
      int length_left;
      int length_to_copy;
      uint file_data_size;

      k = 0;
      file_data_size = sounds[sound_file].getDataSize();
      do {
        length_left = file_data_size - sound_time;
        length_to_copy = min(length_left,data_size);
        memcpy(data,sounds[sound_file].getData()+sound_time,length_to_copy);
        sound_time += length_to_copy;
        if(sound_time >= file_data_size) {
          sound_time = 0;
          if(repeat_file)
            repeat_file = false;
          else
            sound_cmd = Motion::SOUND_NONE;
        }
        k += length_to_copy;
      } while(repeat_file && k < data_size);

      for(; k<data_size; k++) data[k]=0;
    }
    break;
  }
}

}